Fixed publishing of navdata2.

ARDroneLib/Soft/Common/navdata_keys.h

@@ -31,10 +31,10 @@ NAVDATA_OPTION( navdata_watchdog_t  ,     navdata_watchdog        , NAVDATA_WATC
 NAVDATA_OPTION( navdata_adc_data_frame_t, navdata_adc_data_frame  , NAVDATA_ADC_DATA_FRAME_TAG   )
 NAVDATA_OPTION( navdata_video_stream_t,   navdata_video_stream    , NAVDATA_VIDEO_STREAM_TAG     )
 NAVDATA_OPTION( navdata_games_t,          navdata_games           , NAVDATA_GAMES_TAG            )
-NAVDATA_OPTION( navdata_pressure_raw_t,   navdata_pressure_raw    , NAVDATA_PRESSURE_RAW_TAG		 )
-NAVDATA_OPTION( navdata_magneto_t,       	navdata_magneto					, NAVDATA_MAGNETO_TAG					 )
-NAVDATA_OPTION( navdata_wind_speed_t,     navdata_wind_speed      , NAVDATA_WIND_TAG						 )
-NAVDATA_OPTION( navdata_kalman_pressure_t,navdata_kalman_pressure	, NAVDATA_KALMAN_PRESSURE_TAG	 )
+NAVDATA_OPTION( navdata_pressure_raw_t,   navdata_pressure_raw    , NAVDATA_PRESSURE_RAW_TAG     )
+NAVDATA_OPTION( navdata_magneto_t,        navdata_magneto  	      , NAVDATA_MAGNETO_TAG	         )
+NAVDATA_OPTION( navdata_wind_speed_t,     navdata_wind_speed      , NAVDATA_WIND_TAG             )
+NAVDATA_OPTION( navdata_kalman_pressure_t,navdata_kalman_pressure , NAVDATA_KALMAN_PRESSURE_TAG  )
 NAVDATA_OPTION( navdata_hdvideo_stream_t ,navdata_hdvideo_stream  , NAVDATA_HDVIDEO_STREAM_TAG	 )
 NAVDATA_OPTION( navdata_wifi_t           ,navdata_wifi            , NAVDATA_WIFI_TAG             )
 

msg/Navdata2.msg

@@ -64,4 +64,3 @@ float32[] tags_distance
 
 #time stamp
 float32 tm
-

src/ardrone_driver.cpp

@@ -19,7 +19,7 @@ ARDroneDriver::ARDroneDriver()
 	vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
 	navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 10);
     if (IS_ARDRONE2)
-	    navdata2_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata2", 10);
+	    navdata2_pub = node_handle.advertise<ardrone_autonomy::Navdata2>("ardrone/navdata2", 10);
 	toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
 	toggleNavdataDemo_service = node_handle.advertiseService("ardrone/togglenavdatademo", toggleNavdataDemoCallback);
 	setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
@@ -377,8 +377,6 @@ void ARDroneDriver::publish_navdata2()
 		msg.tags_distance.push_back(navdata_detect.dist[i]);
 	}
 
-    printf("%d\n", msg.magX);
-
 	navdata2_pub.publish(msg);
 }
 
@@ -396,7 +394,7 @@ int main(int argc, char** argv)
 {        
         // We need to implement our own Signal handler instead of ROS to shutdown
         // the SDK threads correctly.
-    
+
         ros::init(argc, argv, "ardrone_driver", ros::init_options::NoSigintHandler);
         
         signal (SIGABRT, &controlCHandler);

src/ardrone_sdk.cpp

@@ -70,11 +70,6 @@ extern "C" {
         ardrone_application_default_config.control_yaw = (float) rosDriver->getRosParam("~control_yaw", (100.0 /180.0) * 3.1415);
         ardrone_application_default_config.euler_angle_max = (float) rosDriver->getRosParam("~euler_angle_max", (12.0 / 180.0) * 3.1415);                
         ardrone_application_default_config.navdata_demo = (int) rosDriver->getRosParam("~navdata_demo", (double) 1);
-        if (IS_ARDRONE2) {
-            ardrone_application_default_config.navdata_magneto = (int) rosDriver->getRosParam("~navdata_magneto", (double) 1);
-            ardrone_application_default_config.navdata_pressure_raw = (int) rosDriver->getRosParam("~navdata_pressure", (double) 1);
-            ardrone_application_default_config.navdata_wind_speed = (int) rosDriver->getRosParam("~navdata_wind", (double) 1);
-        }
         ardrone_application_default_config.detect_type = (int) rosDriver->getRosParam("~detect_type", (double) CAD_TYPE_MULTIPLE_DETECTION_MODE);
         ardrone_application_default_config.enemy_colors = (int) rosDriver->getRosParam("~enemy_colors", (double) ARDRONE_DETECTION_COLOR_ORANGE_YELLOW);
         ardrone_application_default_config.enemy_without_shell = (bool) rosDriver->getRosParam("~enemy_without_shell", (double) 0.0);