Added magnetometer, pressure, temperature, and wind speed information to new message 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

@@ -0,0 +1,66 @@
+Header header
+
+# Navdata including the ARDrone 2 specifica sensors
+# (magnetometer, barometer)
+
+# 0 means no battery, 100 means full battery
+float32 batteryPercent
+
+# 0: Unknown, 1: Init, 2: Landed, 3: Flying, 4: Hovering, 5: Test
+# 6: Taking off, 7: Goto Fix Point, 8: Landing, 9: Looping
+# Note: 3,7 seems to discriminate type of flying (isFly = 3 | 7)
+uint32 state
+
+int32 magX
+int32 magY
+int32 magZ
+
+# pressure sensor
+int32 pressure
+
+# apparently, there was a temperature sensor added as well.
+int32 temp
+
+# wind sensing...
+float32 wind_speed
+float32 wind_angle
+float32 wind_comp_angle
+
+# left/right tilt in degrees (rotation about the X axis)
+float32 rotX
+
+# forward/backward tilt in degrees (rotation about the Y axis)
+float32 rotY
+
+# orientation in degrees (rotation about the Z axis)
+float32 rotZ
+
+# estimated altitude (cm)
+int32 altd
+
+# linear velocity (mm/sec)
+float32 vx
+
+# linear velocity (mm/sec)
+float32 vy
+
+# linear velocity (mm/sec)
+float32 vz
+
+#linear accelerations (unit: g)
+float32 ax
+float32 ay
+float32 az
+
+#Tags in Vision Detectoion
+uint32 tags_count
+uint32[] tags_type
+uint32[] tags_xc
+uint32[] tags_yc
+uint32[] tags_width
+uint32[] tags_height
+float32[] tags_orientation
+float32[] tags_distance
+
+#time stamp
+float32 tm

src/ardrone_driver.cpp

@@ -19,6 +19,8 @@ ARDroneDriver::ARDroneDriver()
     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_autonomy::Navdata>("ardrone/navdata", 10);
+    if (IS_ARDRONE2)
+	    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 );
@@ -41,6 +43,8 @@ void ARDroneDriver::run()
 		{
 			publish_video();
 			publish_navdata();
+            if (IS_ARDRONE2)
+			    publish_navdata2();
 			last_frame_id = current_frame_id;
 		}
 		ros::spinOnce();
@@ -60,6 +64,12 @@ double ARDroneDriver::getRosParam(char* param, double defaultVal)
 
 void ARDroneDriver::publish_video()
 {
+	if (image_pub.getNumSubscribers() == 0)
+        return;
+    if (hori_pub.getNumSubscribers() == 0)
+        return;
+	if (vert_pub.getNumSubscribers() == 0)
+        return;
     if (IS_ARDRONE1)
     {
         /*
@@ -230,6 +240,8 @@ void ARDroneDriver::publish_video()
      */
     if (IS_ARDRONE2)
     {
+        if (hori_pub.getNumSubscribers() == 0 || vert_pub.getNumSubscribers() == 0)
+            return;
         sensor_msgs::Image image_msg;
         sensor_msgs::CameraInfo cinfo_msg;
         sensor_msgs::Image::_data_type::iterator _it;
@@ -266,6 +278,8 @@ void ARDroneDriver::publish_video()
 
 void ARDroneDriver::publish_navdata()
 {
+    if (navdata_pub.getNumSubscribers() == 0)
+        return; // why bother, no one is listening.
 	ardrone_autonomy::Navdata msg;
 
 	msg.batteryPercent = navdata.vbat_flying_percentage;
@@ -318,6 +332,56 @@ void ARDroneDriver::publish_navdata()
 	navdata_pub.publish(msg);
 }
 
+void ARDroneDriver::publish_navdata2()
+{
+    if (navdata2_pub.getNumSubscribers() == 0)
+        return; // why bother, no one is listening.
+	ardrone_autonomy::Navdata2 msg;
+
+	msg.batteryPercent = navdata.vbat_flying_percentage;
+    msg.state = (navdata.ctrl_state >> 16);
+
+	// positive means counterclockwise rotation around axis
+	msg.rotX = navdata.phi / 1000.0; // tilt left/right
+	msg.rotY = -navdata.theta / 1000.0; // tilt forward/backward
+	msg.rotZ = -navdata.psi / 1000.0; // orientation
+
+	msg.altd = navdata.altitude; // cm
+	msg.vx = navdata.vx; // mm/sec
+	msg.vy = -navdata.vy; // mm/sec
+	msg.vz = -navdata.vz; // mm/sec
+	msg.tm = arnavtime.time;
+	msg.ax = navdata_phys.phys_accs[ACC_X] / 1000.0; // g
+	msg.ay = -navdata_phys.phys_accs[ACC_Y] / 1000.0; // g
+	msg.az = -navdata_phys.phys_accs[ACC_Z] / 1000.0; // g
+
+    msg.magX = (int32_t)navdata_magneto.mx;
+    msg.magY = (int32_t)navdata_magneto.my;
+    msg.magZ = (int32_t)navdata_magneto.mz;
+
+    msg.pressure = navdata_pressure.Pression_meas; // typo in the SDK!
+    msg.temp = navdata_pressure.Temperature_meas;
+
+    msg.wind_speed = navdata_wind.wind_speed;
+    msg.wind_angle = navdata_wind.wind_angle;
+    msg.wind_comp_angle = navdata_wind.wind_compensation_phi;
+
+	// Tag Detection
+	msg.tags_count = navdata_detect.nb_detected;
+	for (int i = 0; i < navdata_detect.nb_detected; i++)
+	{
+		msg.tags_type.push_back(navdata_detect.type[i]);
+		msg.tags_xc.push_back(navdata_detect.xc[i]);
+		msg.tags_yc.push_back(navdata_detect.yc[i]);
+		msg.tags_width.push_back(navdata_detect.width[i]);
+		msg.tags_height.push_back(navdata_detect.height[i]);
+		msg.tags_orientation.push_back(navdata_detect.orientation_angle[i]);
+		msg.tags_distance.push_back(navdata_detect.dist[i]);
+	}
+
+	navdata2_pub.publish(msg);
+}
+
 void controlCHandler (int signal)
 {
     ros::shutdown();
@@ -332,7 +396,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_driver.h

@@ -5,6 +5,7 @@
 #include <image_transport/image_transport.h>
 #include <sensor_msgs/Image.h>
 #include <ardrone_autonomy/Navdata.h>
+#include <ardrone_autonomy/Navdata2.h>
 #include "ardrone_sdk.h"
 
 class ARDroneDriver
@@ -18,6 +19,7 @@ class ARDroneDriver
 private:
 	void publish_video();
 	void publish_navdata();
+	void publish_navdata2();
 
 	ros::NodeHandle node_handle;
 	ros::Subscriber cmd_vel_sub;
@@ -30,6 +32,7 @@ class ARDroneDriver
 	image_transport::CameraPublisher vert_pub;
 
 	ros::Publisher navdata_pub;
+	ros::Publisher navdata2_pub;
 
 	//ros::Subscriber toggleCam_sub;
 	ros::ServiceServer toggleCam_service;

src/ardrone_sdk.cpp

@@ -7,6 +7,10 @@ navdata_demo_t navdata;
 navdata_phys_measures_t navdata_phys;
 navdata_vision_detect_t navdata_detect;
 
+navdata_pressure_raw_t navdata_pressure;
+navdata_magneto_t navdata_magneto;
+navdata_wind_speed_t navdata_wind;
+
 navdata_time_t arnavtime;
 
 ARDroneDriver* rosDriver;
@@ -177,6 +181,9 @@ extern "C" {
 		navdata_phys = pnd->navdata_phys_measures;
 		navdata = pnd->navdata_demo;
 		arnavtime = pnd->navdata_time;
+        navdata_pressure = pnd->navdata_pressure_raw;
+        navdata_magneto = pnd->navdata_magneto;
+        navdata_wind = pnd->navdata_wind_speed;
 		return C_OK;
 	}
 

src/ardrone_sdk.h

@@ -44,6 +44,10 @@ extern navdata_phys_measures_t navdata_phys;
 extern navdata_demo_t navdata;
 extern navdata_time_t arnavtime;
 
+extern navdata_pressure_raw_t navdata_pressure;
+extern navdata_magneto_t navdata_magneto;
+extern navdata_wind_speed_t navdata_wind;
+
 extern int32_t should_exit;
 
 #endif