Discussion on Driver's Threading Architecture about ardrone_autonomy

[JakobEngel]

I'm only semi-experienced with ROS (never had to do with it before writing tum_ardrone), but as I understand it:

• publishing from any thread is fine, at least as long as no two threads publish on the same topic at the same time. In this case, that would never be the case anyway (navdata thread publishes on navdata topic; video thread on video topic). I publish on a single publisher from different threads (synchronized via a http://en.wikipedia.org/wiki/Critical_section), which works absolutely fine.
• spin() has nothing to do with publishing (you can publish without calling spin anywhere ever), it's only for receiving messages (i.e. calling the respective callback).

=> publishing directly in the SDK callbacks is the best and most efficient solution, while producing the smallest latency. Just make shure the topics are advertised before any other thread publishes on them ;)

The reason why even with looprate=200 some navdatas are skipped, is because thy might get stuck in some Network buffer before, that is the SDK-navdata callback is not called every 5ms, but maybe 3 times within 1ms, and then again only 15ms later. Have a look at the ros-timestamps of the navdata packages sent out, they are everything but regular, especially if other computationally-expensive stuff is running on ur pc.
Its not an issue of anything taking too long (computationally), but simply of very irregular intervals, which should be solved by publishing directly in the sdk callbacks.

[mikehamer]

How well the loop is keeping up with the received data can be determined by comparing the frequencies of rostopic hz /ardrone/navdata vs rostopic hz /ardrone/navdata_demo.

I also inserted a printout of the time between loops in comparison to the time between navdata receptions. This confirms what @JakobEngel mentioned about network delays, which seem to be the major problem here.

In the following, we see that the loop keeps up with the navdata, when the navdata is received at a regular interval. However, there are cases where the network delay causes no packets to be received for a few loops, and cases where many navdata packets come at once.

[ INFO] [1354094907.319353778]: NAVDATA Period = 4045 usec.
[ INFO] [1354094907.321747973]: LOOP Period = 3672 usec.
[ INFO] [1354094907.323910639]: NAVDATA Period = 4551 usec.
[ INFO] [1354094907.327006364]: LOOP Period = 5192 usec.
[ INFO] [1354094907.327649755]: NAVDATA Period = 3769 usec.
[ INFO] [1354094907.332633835]: LOOP Period = 5616 usec.
[ INFO] [1354094907.334162937]: NAVDATA Period = 6462 usec.
[ INFO] [1354094907.336750306]: LOOP Period = 4156 usec.
[ INFO] [1354094907.337876744]: NAVDATA Period = 3752 usec.
[ INFO] [1354094907.341746467]: LOOP Period = 4995 usec.
[ INFO] [1354094907.342984021]: NAVDATA Period = 5085 usec.
[ INFO] [1354094907.348036933]: LOOP Period = 6279 usec.
[ INFO] [1354094907.352097578]: LOOP Period = 4012 usec.
[ INFO] [1354094907.357068480]: LOOP Period = 5033 usec.
[ INFO] [1354094907.361564322]: NAVDATA Period = 18525 usec.
[ INFO] [1354094907.362275359]: LOOP Period = 5230 usec.
[ INFO] [1354094907.366761187]: LOOP Period = 4457 usec.
[ INFO] [1354094907.372257735]: LOOP Period = 5507 usec.
[ INFO] [1354094907.372761666]: NAVDATA Period = 11139 usec.
[ INFO] [1354094907.376143762]: NAVDATA Period = 3476 usec.
[ INFO] [1354094907.376288293]: NAVDATA Period = 211 usec.
[ INFO] [1354094907.378004932]: LOOP Period = 5777 usec.
[ INFO] [1354094907.379070091]: NAVDATA Period = 2748 usec.
[ INFO] [1354094907.380202435]: NAVDATA Period = 1109 usec.
[ INFO] [1354094907.382106182]: LOOP Period = 4063 usec.
[ INFO] [1354094907.383237719]: NAVDATA Period = 3035 usec.
[ INFO] [1354094907.387215018]: LOOP Period = 5115 usec.
[ INFO] [1354094907.388007705]: NAVDATA Period = 4260 usec.
[ INFO] [1354094907.392086955]: LOOP Period = 4850 usec.
[ INFO] [1354094907.393764073]: NAVDATA Period = 6278 usec.
[ INFO] [1354094907.394070765]: NAVDATA Period = 344 usec.
[ INFO] [1354094907.398257646]: LOOP Period = 6197 usec.
[ INFO] [1354094907.400090020]: NAVDATA Period = 5962 usec.
[ INFO] [1354094907.401839753]: LOOP Period = 3592 usec.
[ INFO] [1354094907.406114524]: NAVDATA Period = 5999 usec.
[ INFO] [1354094907.409146595]: LOOP Period = 7064 usec.
[ INFO] [1354094907.411828679]: LOOP Period = 2892 usec.
[ INFO] [1354094907.412320936]: NAVDATA Period = 6071 usec.
[ INFO] [1354094907.416609960]: NAVDATA Period = 4444 usec.
[ INFO] [1354094907.418218664]: LOOP Period = 6430 usec.

One improvement I am implementing at the moment, is saving the ros time of packet reception, rather than the time of packet processing. This will ensure that the navdata output by the loop is stamped correctly.

Although all in all, I favor the approach that we output packets (and video) when received – we just need to make sure these functions are fast enough to keep up with the send rate.

What is also interesting, is that the loop period varies so much. The loop_rate.sleep(...) command should sleep for enough time such that a constant loop rate is achieved...

[mikehamer]

Continuing the discussion with respect to #44.

Implementation of realtime video publishing (and to some degree, navdata as well), indicates that the current driver architecture is not so well suited to this task, requiring a few hacks and lots of shared data to enable these functions.

The biggest problem we have, are the calls from ardrone_sdk.cpp and video.cpp, to what I have now made public functions in the ARDroneDriver class. These calls require a shared class, whose class definition is in ardrone_driver, and whose global variable declaration in ardrone_sdk. This is a clear dependency from sdk to driver, but the reverse dependency also exists, with driver dependent on shared data and parameters loaded in sdk.

I think we could solve a lot of these problems by externalizing the video and navdata publishing methods, which wouldn't be such a drastic change to the architecture.

Optimally though, perhaps thinking about how we can convert the currently external methods into internal driver methods may be a better option, leaving the sdk.cpp file to simply setup the ARDroneSDK environment and callbacks to driver functions. This way all execution outside of the initialization would be done internally to the driver.

What are your thoughts?

[mani-monaj]

Thanks for all of your feedbacks and tests @mikehamer & @JakobEngel .

publishing from any thread is fine, at least as long as no two threads publish on the same topic at the same time.

Fair enough.

spin() has nothing to do with publishing

Thanks. That is completely right. I misunderstood the underlying ROS architecture.

I am currently examining the changes in #44. I think what you concluded about network delay is correct

[mani-monaj]

#44 merged into dev-unstable, I tested it a little bit. This needs to be more tested.

[mani-monaj]

Thanks @mikehamer for the changes. I did not have enough time today for the in-depth testing. But it looks like it works as expected.

[mani-monaj]

Optimally though, perhaps thinking about how we can convert the currently external methods into internal driver methods may be a better option, leaving the sdk.cpp file to simply setup the ARDroneSDK environment and callbacks to driver functions. This way all execution outside of the initialization would be done internally to the driver.

Can you please explain it a little further?

[maxpfingsthorn]

I was wondering if reimplementing the relevant parts of the ARDroneSDK would have any advantage. Looking at the manual, it shouldn't be too hard to do, but I'm not sure about the final benefit. Someone did it for Java (see javadrone project on Google Code). What is your opinion about this? Is the SDK fast and clever about things? Looking at the insides of the SDK is hard, because it uses a very exotic threading model and video decoding library, probably because for portability. I have the feeling it's optimized for iOS, not really for PC. Using the FreeFlight App on my iDevice, I get much better results (less video hiccups, less control latency) than on my PC, which is orders of magnitudes faster. Have you looked into GPU-accelerated video decoding? Any decent PC has one capable of it these days.

An important optimization point, especially for image transports in ROS, would be to implement the driver as a nodelet. Other ROS nodelets running in the same container will be passed the pointer to the original data (a boost::shared_ptr) and no serialization of the ROS message is needed. Other ROS node(let)s can still receive the messages over TCP/IP as before. In high-volume data applications, like stereo vision, this often means a performance difference of an order of magnitude or so. Maybe here as well?