Enables real-time publishing of new-style navdata

[mikehamer]

This pull request contains the improvements found in my last pull request #38, while adding the following two improvements:

Two new parameters have been added:

• fullspeed_navdata, which controls whether new-style navdata is published when received, or at looprate
• looprate, which controls the speed of the internal ros-loop. Note that legacy navdata is always published at looprate

Also made various speed improvements to the navdata processing pipeline to enable this faster processing.

Also fixed a problem with enable_legacy_navdata, whereby it wasn't read correctly and thus had no effect. Also mentioning it here because I forgot to mention it when I added it ages ago.

Also updated and commented the launch file to reflect these changes.

Regarding testing, I've flown with this for three batteries, with fullspeed_navdata=true, looprate=50, while rosbagging everything and experienced no unusual behaviour.

[JakobEngel]

i just installed the package on a new pc, your last update from 11 days ago (the current master branch) broke the navdata_demo parameter. your code assumes the onboard-default is 0, while it acutally is 1, hence it cannot be set to 0 (= send full navdata) anymore. pls fix ;)

[mikehamer]

That's very strange.... the default value for navdata_demo listed in config_keys.h:243 is FALSE (=0). My code generation uses these values as defaults.

I've also just tested it and I am able to set navdata_demo to both 0 (not explicitly sent to drone) and 1 (explicitly sent):

SEND: CAT_COMMON/navdata_demo = 1.000000 (DEFAULT = 0.000000)

If you telnet into the drone, there are a few configuration files that contain the drone's current configuration:

• /data/config.ini (CAT_COMMON settings)
• /data/custom.configs/applis/config.e182b69b.ini (CAT_APPLI settings)
• /data/custom.configs/profiles/config.cea48d22.ini (CAT_USER settings)
• /data/custom.configs/sessions/<the only file here> (CAT_SESSION settings)

Querying the value of navdata_demo in config.ini, on the default launch with navdata_demo=0, I get:

# cat config.ini | grep navdata
navdata_demo = FALSE

And then in the situation above, where I set navdata_demo=1:

# cat config.ini | grep navdata
navdata_demo = TRUE

What makes you suspect that navdata_demo isn't being set properly / that my update is the issue?

In any case, I've just published a new branch, which will send all parameters to the drone, irrespective of whether they are default or not.

Let me know if any of the above helps!

[JakobEngel]

I'm back home & don't have the drone here, but
navdata_demo == true <=> drone sends navdata at 15Hz (default);
navdata_demo == false <=> drone sends navdata at 200Hz;

So with navdata_demo == false (and loop_rate set to 50), "rostopic hz /ardrone/navdata" should (and did) give 50Hz.
With the current master branch, it always is 15Hz.

After removing the if-check in ardrone_driver.cpp:189, it worked again. ("rosrun ardrone_autonomy ardrone_driver _navdata_demo:=0" => 50Hz).

[mikehamer]

Ahh okay, now I understand. Thanks!

So removing the check (ardrone_driver.cpp:176)

        if(ardrone_application_default_config.NAME!=DEFAULT)

Worked for you?

[mani-monaj]

Thanks @mikehamer. I merged your previous pull request #38 to dev-unstable. Because of the duplicate commits in this pull request, the merge is not automatically possible for this one. I will try to manually merge the new changes.

[mani-monaj]

The changes are now in dev-unstable, the bug that @JakobEngel reported may still exist. I cherry-picked your commits from the faster_navdata remote branch. The code compiles, but I've not tested it yet.

[mani-monaj]

@JakobEngel Are you talking about this line: https://github.com/AutonomyLab/ardrone_autonomy/blob/master/src/ardrone_driver.cpp#L189

[mani-monaj]

@JakobEngel I have problem reproducing the bug you reported. I am on the master branch after a complete rebuild.

rosrun ardrone_autonomy ardrone_driver _navdata_demo:=0
...
rostopic hz /ardrone/navdata
>>> 50Hz

While

rosrun ardrone_autonomy ardrone_driver _navdata_demo:=1
...
rostopic hz /ardrone/navdata
>>> 15Hz

[JakobEngel]

strange, ill test it again on monday... but as I understand it, it has been fixed anyway ;)

[mikehamer]

I tested this branch yesterday and have found some potential issues, the largest being that the new-style navdata now takes up a lot of space. Previously I had recorded (through an unpublished branch) 200Hz new-style data for 4 minutes and the bag-file was 180MB. Now a minute of flight is 2+GB and eventually will overflow the rosbag buffer. I'm still trying to understand the reason for this – a task for tomorrow.

[mani-monaj]

@JakobEngel I tested the latest tum_vision with the current state of dev-unstable (1ae6d48) successfully. I would really appreciate it if you test it yourself. The only change is to change _loop_rate to _looprate in command line switches.

I am really impressed by the autopilot's performance. I still need to test everything more to have a better understanding of the system. In the meantime, we can work together to make the driver and your code work flawlessly together.

[mani-monaj]

@mikehamer I did not test the rosbag file sizes, however I was not able to get update frequencies above 170Hz on a very descent PC.

[mani-monaj]

I did more tests on "realtime" and "legacy" publishing of "Navdata" and here are my thoughts:

1. "Full Speed Navdata" is not a good idea. The current implementation bypasses main ROS loop running in a different thread while instantly publishes the data which may generate undefined behaviour. I say this because I could not find any information about ROS that states it is safe to bypass the same ROS loop from another thread.
2. When navdata_demo is turned on and the Drone is publishing at 200Hz, the main ROS loop can not catch up even with 500Hz loop rate. I think this is because of expensive memory copies done in video and navdata callback functions, many locks/unlocks between two threads and other custom code/ROS overheads during the ROS loop.

In order to examine it more, I put a ROS_INFO inside this if block to print the difference between copy_current_frame_id and last_frame_id. If ROS loop is in sync with drone update loop, this number should be always 1, otherwise it shows the number of navdata misses between consequent ROS updates which consequently leads to frame rates less than 200Hz. The result was as I expected. There are lots of misses when the looprate is set to 200Hz. Increasing that number will lead to better navdata frame rate (and less misses). In my case setting that number to 1000Hz would lead to 200Hz navdata update rate.

In summary, I suggest to remove the "Full Speed Navdata". Anyone who is interested in getting 200Hz navdata update, can increase the looprate variable appropriately.

[mikehamer]

Actually, I'm fairly sure all the above is due to a bug which I introduced when making the change from a local message variable to a class message variable (to save allocation time):

Some of the messages from the drone have arrays of data which need to be extracted from the struct and placed into a rosmessage. This in essence requires a: navdata_raw_measures_msg.raw_gyros.push_back(m); for example. Because I don't clear this ros array between cycles (before I did not need to because a new message was allocated), it accumulates over time, thus the amount of data ros needs to push gets huge very quickly with 200Hz messages.

I'm fixing and testing this at the moment.

[mikehamer]

Fixed the bug in the current dev-unstable branch and have submitted the changes as a new pull request #41.

Note that the above two changes do fix the problem, but were for the private branch of mine (which I have now deleted). I then pulled your current dev-unstable branch and merged the above two changes, plus a few more improvements, into that. See #41 for more details.