Before, we were stamping the /ardrone/navdata,mag,imu messages with the time that they were processed in the main ros loop. This caused unnecessary delays in the timestamp. Now we save the time of navdata reception in the navdata handling loop, which is then used to stamp the output messages when processed by the loop.
This bug was introduced when moving local scope message variables to class-level. This move meant that messages wouldn't be reset after being sent. This was not a problem for non-array type message members, which were reassigned before being sent. However for array-type message members, which are filled using .push_back, no reset occurred. This fix appropriately clears these members before re-filling them on every send-loop.
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.
Conflicts:
src/ardrone_driver.h
We now have the ability to access any of the navdata structs which are
sent back by the drone. Still to do is to enable/disable the drone
sending based on whether we're interested in receiving the message.
Mani Monajjemi