Minor changes inside ROS loop.

src/ardrone_driver.cpp

@@ -137,7 +137,7 @@ void ARDroneDriver::run()
                          ardrone_control_config.num_version_soft,
                          shared_raw_navdata.navdata_demo.vbat_flying_percentage);
                 ROS_INFO("Navdata Publish Settings:");
-                ROS_INFO("    Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off");
+                //ROS_INFO("    Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off"); //Bug: This is being inited after in the NavdataMessage*.h
                 ROS_INFO("    ROS Loop Rate: %d", looprate);
                 ROS_INFO("    Instant New Navdata Publish: %s", fullspeed_navdata ? "On" : "Off");
                 // TODO: Enabled Navdata Demo
@@ -153,8 +153,8 @@ void ARDroneDriver::run()
             vp_os_mutex_unlock(&video_lock);
             if (copy_current_frame_id != last_frame_id)
             {
-                publish_video();                                
                 last_frame_id = copy_current_frame_id;
+                publish_video();
             }
 
             vp_os_mutex_lock(&navdata_lock);
@@ -162,8 +162,8 @@ void ARDroneDriver::run()
             vp_os_mutex_unlock(&navdata_lock);
             if (copy_current_navdata_id != last_navdata_id)
             {
-                publish_navdata();
                 last_navdata_id = copy_current_navdata_id;
+                publish_navdata();                
             }
             if (freq_dev == 0) publish_tf();
 

src/ardrone_sdk.cpp

@@ -247,7 +247,7 @@ extern "C" {
     THREAD_TABLE_ENTRY(video_stage, 31)
     THREAD_TABLE_ENTRY(update_ros, 43)
     THREAD_TABLE_ENTRY(video_recorder, 20)
-	THREAD_TABLE_ENTRY(navdata_update, 31)
+    THREAD_TABLE_ENTRY(navdata_update, 31)
 //	THREAD_TABLE_ENTRY(ATcodec_Commands_Client, 43)
 	THREAD_TABLE_ENTRY(ardrone_control, 31)
 	END_THREAD_TABLE