drone_frame_id is now can be set as a param. This param is used in image messages header.

A bug in 5s wait time fixed.

README.md

@@ -152,6 +152,7 @@ You can test these animations in command line using commands like `rosservice ca
 
 The parameters listed below are named according to AR-Drone's SDK 2.0 configuration. Unless you set the parameters using `rosparam` or in your `lauch` file, the default values will be used. These values are applied during driver's initialization phase. Please refer to AR-Drone SDK 2.0's [developer's guide](https://projects.ardrone.org/projects/show/ardrone-api/) for information about valid values.
 
+* `drone_frame_id` - The "frame_id" to be used in camera & image messages header - default: "ardrone"
 * `bitrate_ctrl_mode` - default: DISABLED
 * `max_bitrate` - (AR-Drone 2.0 only) Default: 4000 Kbps
 * `bitrate` -  Default: 4000 Kbps

src/ardrone_driver.cpp

@@ -26,6 +26,8 @@ ARDroneDriver::ARDroneDriver()
 	setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
 //	setEnemyColor_service = node_handle.advertiseService("/ardrone/setenemycolor", setEnemyColorCallback);
 //	setHullType_service = node_handle.advertiseService("/ardrone/sethulltype", setHullTypeCallback);
+
+    droneFrameId = (ros::param::get("~drone_frame_id", droneFrameId)) ? droneFrameId : "ardrone";
 }
 
 ARDroneDriver::~ARDroneDriver()
@@ -57,8 +59,8 @@ void ARDroneDriver::run()
                 publish_navdata();
                 last_frame_id = current_frame_id;
             }
-            ros::spinOnce();
         }
+        ros::spinOnce();
 		loop_rate.sleep();
 	}
     printf("ROS loop terminated ... \n");
@@ -107,6 +109,8 @@ void ARDroneDriver::publish_video()
 
         image_msg.header.stamp = ros::Time::now();
         cinfo_msg.header.stamp = ros::Time::now();
+        image_msg.header.frame_id = droneFrameId;
+        cinfo_msg.header.frame_id = droneFrameId;
         image_msg.width = D1_STREAM_WIDTH;
         image_msg.height = D1_STREAM_HEIGHT;
         image_msg.encoding = "rgb8";
@@ -260,6 +264,9 @@ void ARDroneDriver::publish_video()
 
         image_msg.header.stamp = ros::Time::now();
         cinfo_msg.header.stamp = ros::Time::now();
+        image_msg.header.frame_id = droneFrameId;
+        cinfo_msg.header.frame_id = droneFrameId;
+
         image_msg.width = D2_STREAM_WIDTH;
         image_msg.height = D2_STREAM_HEIGHT;
         image_msg.encoding = "rgb8";

src/ardrone_driver.h

@@ -54,6 +54,7 @@ private:
 	int flying_state;
 
     bool inited;
+    std::string droneFrameId;
 
 };