Add support for some ARDrone 2 specific sensors.

Navdata no longer publishes when there is no one listening.
2012-07-26 17:16:19 -07:00
commit 1ba71f1916
@@ -0,0 +1,67 @@
+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
+
@@ -18,6 +18,7 @@ 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);
+	navdata2_pub = node_handle.advertise<ardrone_autonomy::Navdata>("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 );
@@ -264,6 +265,8 @@ void ARDroneDriver::publish_video()

 void ARDroneDriver::publish_navdata()
 {
+    if (navdata.getNumSubscribers() == 0)
+        return // why bother, no one is listening.
 	ardrone_autonomy::Navdata msg;

 	msg.batteryPercent = navdata.vbat_flying_percentage;
@@ -316,6 +319,73 @@ void ARDroneDriver::publish_navdata()
 	navdata_pub.publish(msg);
 }

+void ARDroneDriver::publish_navdata2()
+{
+    if (navdata2.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++)
+	{
+		/*
+		 * The tags_type is in raw format. In order to extract the information 
+		 * macros from ardrone_api.h is needed.
+		 *
+		 * #define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) )
+		 * #define DETECTION_EXTRACT_SOURCE(type)  ( ((type)>>16) & 0x0FF )
+		 * #define DETECTION_EXTRACT_TAG(type)     ( (type) & 0x0FF )
+		 * 
+		 * Please also note that the xc, yc, width and height are in [0,1000] range
+		 * and must get converted back based on image resolution.
+		 */
+		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]);
+	}
+	// TODO: Ideally we would be able to figure out whether we are in an emergency state
+	// using the navdata.ctrl_state bitfield with the ARDRONE_EMERGENCY_MASK flag, but
+	// it seems to always be 0.  The emergency state seems to be correlated with the
+	// inverse of the ARDRONE_TIMER_ELAPSED flag, but that just makes so little sense
+	// that I don't want to use it because it's probably wrong.  So we'll just use a
+	// manual reset for now.
+
+	navdata2_pub.publish(msg);
+}
+
 void controlCHandler (int signal)
 {
    ros::shutdown();
@@ -18,6 +18,7 @@ public:
 private:
 	void publish_video();
 	void publish_navdata();
+	void publish_navdata2();

 	ros::NodeHandle node_handle;
 	ros::Subscriber cmd_vel_sub;
@@ -30,6 +31,7 @@ private:
 	image_transport::CameraPublisher vert_pub;

 	ros::Publisher navdata_pub;
+	ros::Publisher navdata2_pub;

 	//ros::Subscriber toggleCam_sub;
 	ros::ServiceServer toggleCam_service;
@@ -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;
 	}

@@ -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