Some minor tweak for furthur performance increase.

src/ardrone_driver.cpp

@@ -10,14 +10,14 @@
 ARDroneDriver::ARDroneDriver()
 	: image_transport(node_handle)
 {
-	cmd_vel_sub = node_handle.subscribe("/cmd_vel", 1, &cmdVelCallback);
+	cmd_vel_sub = node_handle.subscribe("/cmd_vel", 100, &cmdVelCallback);
 	takeoff_sub = node_handle.subscribe("/ardrone/takeoff", 1, &takeoffCallback);
 	reset_sub = node_handle.subscribe("/ardrone/reset", 1, &resetCallback);
 	land_sub = node_handle.subscribe("/ardrone/land", 1, &landCallback);
-	image_pub = image_transport.advertiseCamera("/ardrone/image_raw", 1);
-    hori_pub = image_transport.advertiseCamera("/ardrone/front/image_raw", 1);
-	vert_pub = image_transport.advertiseCamera("/ardrone/bottom/image_raw", 1);
-	navdata_pub = node_handle.advertise<ardrone_brown::Navdata>("/ardrone/navdata", 1);
+	image_pub = image_transport.advertiseCamera("/ardrone/image_raw", 10);
+    hori_pub = image_transport.advertiseCamera("/ardrone/front/image_raw", 10);
+	vert_pub = image_transport.advertiseCamera("/ardrone/bottom/image_raw", 10);
+	navdata_pub = node_handle.advertise<ardrone_brown::Navdata>("/ardrone/navdata", 10);
 	//toggleCam_sub = node_handle.subscribe("/ardrone/togglecam", 10, &toggleCamCallback);
 
 	//int cam_state = DEFAULT_CAM_STATE; // 0 for forward and 1 for vertical, change to enum later

src/ardrone_sdk.cpp

@@ -107,11 +107,11 @@ extern "C" {
         params->out_pic = out_picture;
         params->pre_processing_stages_list  = driver_pre_stages;
         params->post_processing_stages_list = driver_post_stages;
-        params->needSetPriority = 0;
-        params->priority = 0;
+        params->needSetPriority = 1;
+        params->priority = 31;
         // Using the provided threaded pipeline implementation from SDK
         START_THREAD(video_stage, params);
-        video_stage_init();
+//        video_stage_init();
 //        if (ARDRONE_VERSION() >= 2)
 //        {
 //            START_THREAD (video_recorder, NULL);
@@ -120,7 +120,7 @@ extern "C" {
 //        }
         // Threads do not start automatically
         video_stage_resume_thread();
-        ardrone_tool_set_refresh_time(30);
+        ardrone_tool_set_refresh_time(25);
 		START_THREAD(update_ros, rosDriver);
 		return C_OK;
 	}
@@ -156,12 +156,12 @@ extern "C" {
     }
     
 	BEGIN_THREAD_TABLE
-    THREAD_TABLE_ENTRY(video_stage, 20)
-    THREAD_TABLE_ENTRY(update_ros, 20)
+    THREAD_TABLE_ENTRY(video_stage, 31)
+    THREAD_TABLE_ENTRY(update_ros, 43)
 //    THREAD_TABLE_ENTRY(video_recorder, 20)
-	THREAD_TABLE_ENTRY(navdata_update, 20)
-	THREAD_TABLE_ENTRY(ATcodec_Commands_Client, 20)
-	THREAD_TABLE_ENTRY(ardrone_control, 20)
+	THREAD_TABLE_ENTRY(navdata_update, 31)
+//	THREAD_TABLE_ENTRY(ATcodec_Commands_Client, 43)
+	THREAD_TABLE_ENTRY(ardrone_control, 31)
 	END_THREAD_TABLE
 
 	BEGIN_NAVDATA_HANDLER_TABLE

src/teleop_twist.cpp

@@ -7,6 +7,10 @@ bool needs_takeoff = false;
 bool needs_land = false;
 bool needs_reset = false;
 geometry_msgs::Twist cmd_vel;
+float old_left_right;
+float old_front_back;
+float old_up_down;
+float old_turn;
 
 int cam_state = DEFAULT_CAM_STATE; // 0 for forward and 1 for vertical, change to enum later
 int set_navdata_demo_value = DEFAULT_NAVDATA_DEMO; 
@@ -102,7 +106,6 @@ C_RESULT update_teleop(void)
 {
 	// This function *toggles* the emergency state, so we only want to toggle the emergency
 	// state when we are in the emergency state (because we want to get out of it).
-    static int i = 0;
     if (needs_reset)
     {
         ardrone_tool_set_ui_pad_select(1);
@@ -120,15 +123,19 @@ C_RESULT update_teleop(void)
     }
     else
     {
-        // This function sets whether or not the robot should be flying.  If it is flying and you
-        // send 0, the robot will slow down the motors and slowly descend to the floor.
-        //ardrone_tool_set_ui_pad_start(is_flying);
 
         float left_right = (float) cmd_vel.linear.y;
         float front_back = (float) cmd_vel.linear.x;
         float up_down = (float) cmd_vel.linear.z;
         float turn = (float) cmd_vel.angular.z;
         
+        bool is_changed = !(
+                (fabs(left_right - old_left_right) < _EPS) && 
+                (fabs(front_back - old_front_back) < _EPS) && 
+                (fabs(up_down - old_up_down) < _EPS) && 
+                (fabs(turn - old_turn) < _EPS)
+                );
+        
         // These lines are for testing, they should be moved to configurations
         // Bit 0 of control_flag: should we hover?
         // Bit 1 of control_flag: should we use combined yaw mode?
@@ -147,8 +154,15 @@ C_RESULT update_teleop(void)
         control_flag |= (combined_yaw << 1);
         //ROS_INFO (">>> Control Flag: %d", control_flag);
         
-
-       ardrone_tool_set_progressive_cmd(control_flag, left_right, front_back, up_down, turn, 0.0, 0.0);
+        old_left_right = left_right;
+        old_front_back = front_back;
+        old_up_down = up_down;
+        old_turn = turn;
+        
+        if (is_changed)
+        {
+            ardrone_tool_set_progressive_cmd(control_flag, left_right, front_back, up_down, turn, 0.0, 0.0);
+        }
 
     }