New topic imu of standard type sensor_msgs::Imu

README.md

@@ -6,6 +6,7 @@
 
 ### Updates
 
+- *August 17 2012*: Experimental `tf` support added. New published topic `imu`.
 - *August 1 2012*: Enhanced `Navdata` message. `Navdata` now includes magnetometer data, barometer data, temperature and wind information for AR-Drone 2. [Issue #2](https://github.com/AutonomyLab/ardrone_autonomy/pull/2)
 - *July 27 2012*: LED Animation Support added to the driver as a service
 - *July 19 2012*: Initial Public Release
@@ -14,7 +15,7 @@
 
 ### Pre-requirements
 
-This driver has been tested on Linux machines running Ubuntu 11.10 & 12.04 (32 bit and 64 bit). However it should also work on any other mainstream Linux distribution. The driver has been tested on both ROS "electric" and "fuerte". The AR-Drone SDK has its own build system which usually handles system wide dependencies itself. The ROS package depends on these standard ROS packages: `roscpp`, `image_transport`, `sensor_msgs` and `std_srvs`. 
+This driver has been tested on Linux machines running Ubuntu 11.10 & 12.04 (32 bit and 64 bit). However it should also work on any other mainstream Linux distribution. The driver has been tested on both ROS "electric" and "fuerte". The AR-Drone SDK has its own build system which usually handles system wide dependencies itself. The ROS package depends on these standard ROS packages: `roscpp`, `image_transport`, `sensor_msgs`, `tf` and `std_srvs`.
 
 ### Installation Steps
 
@@ -47,6 +48,10 @@ The installation follows the same steps needed usually to compile a ROS driver.
 
 The driver's executable node is `ardrone_driver`. You can either use `rosrun ardrone_autonomy ardrone_driver` or put it in a custom launch file with your desired parameters.
 
+## Coordinate Frames
+
+[TBA]
+
 ## Reading from AR-Drone
 
 ### Navigation Data
@@ -79,6 +84,10 @@ Information received from the drone will be published to the `ardrone/navdata` t
 * `ax`, `ay`, `az`: Linear acceleration (g) [TBA: Convention]
 * `tm`: Timestamp of the data returned by the Drone
 
+### IMU data
+
+The acceleration and orientation from the `Navdata` is also published to a standard ROS [`sensor_msgs/Imu`](http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html) message. The units are all metric and the reference frame is in `IMU` frame. This topic is experimental and needs to be enhanced by covariance matrices.
+
 ### Cameras
 
 Both AR-Drone 1.0 and 2.0 are equipped with two cameras. One frontal camera pointing forward and one vertical camera pointing downward. This driver will create three topics for each drone: `ardrone/image_raw`, `ardrone/front/image_raw` and `ardrone/bottom/image_raw`. Each of these three are standard [ROS camera interface](http://ros.org/wiki/camera_drivers) and publish messages of type [image transport](http://www.ros.org/wiki/image_transport). 
@@ -152,7 +161,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"
+* `drone_frame_id` - The "frame_id" prefix to be used in all messages headers - default: "ardrone_frame"
 * `bitrate_ctrl_mode` - default: DISABLED
 * `max_bitrate` - (AR-Drone 2.0 only) Default: 4000 Kbps
 * `bitrate` -  Default: 4000 Kbps

src/ardrone_driver.cpp

@@ -19,7 +19,8 @@ ARDroneDriver::ARDroneDriver()
 	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_autonomy::Navdata>("ardrone/navdata", 10);
+    navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
+    imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
 	toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
 	toggleNavdataDemo_service = node_handle.advertiseService("ardrone/togglenavdatademo", toggleNavdataDemoCallback);
 	setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
@@ -32,6 +33,26 @@ ARDroneDriver::ARDroneDriver()
     droneFrameIMU = droneFrameId + "_imu";
     droneFrameFrontCam = droneFrameId + "_frontcam";
     droneFrameBottomCam = droneFrameId + "_bottomcam";
+
+    // Fill constant parts of IMU Message
+
+    // No covariance yet for linear acceleration
+    for (sensor_msgs::Imu::_angular_velocity_covariance_type::iterator ita = imu_msg.linear_acceleration_covariance.begin();
+         ita != imu_msg.linear_acceleration_covariance.end(); ita++)
+    {
+        *ita = 0.0;
+    }
+
+    // No covariance yet for rotation
+    for (sensor_msgs::Imu::_orientation_covariance_type::iterator ito = imu_msg.orientation_covariance.begin();
+         ito != imu_msg.orientation_covariance.end(); ito++)
+    {
+        *ito = 0.0;
+    }
+
+    // No angular velocity at all
+    imu_msg.angular_velocity_covariance[0] = -1.0;
+
 }
 
 ARDroneDriver::~ARDroneDriver()
@@ -329,7 +350,7 @@ void ARDroneDriver::publish_video()
 
 void ARDroneDriver::publish_navdata()
 {
-    if (navdata_pub.getNumSubscribers() == 0)
+    if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0))
         return; // why bother, no one is listening.
 	ardrone_autonomy::Navdata msg;
 
@@ -379,7 +400,7 @@ void ARDroneDriver::publish_navdata()
 
 	// Tag Detection
 	msg.tags_count = navdata_detect.nb_detected;
-	for (int i = 0; i < navdata_detect.nb_detected; i++)
+    for (int i = 0; i < navdata_detect.nb_detected; i++)
 	{
 		/*
 		 * The tags_type is in raw format. In order to extract the information 
@@ -401,7 +422,21 @@ void ARDroneDriver::publish_navdata()
 		msg.tags_distance.push_back(navdata_detect.dist[i]);
 	}
 
-	navdata_pub.publish(msg);
+    navdata_pub.publish(msg);
+
+    /* IMU */
+    imu_msg.header.frame_id = droneFrameIMU;
+    imu_msg.header.stamp = ros::Time::now();
+
+    // IMU - Linear Acc
+    imu_msg.linear_acceleration.x = msg.ax * 9.8;
+    imu_msg.linear_acceleration.y = msg.ay * 9.8;
+    imu_msg.linear_acceleration.z = msg.az * 9.8;
+
+    // IMU - Rotation Matrix
+    imu_msg.orientation = tf::createQuaternionMsgFromRollPitchYaw(msg.rotX * _DEG2RAD, msg.rotY * _DEG2RAD, msg.rotZ * _DEG2RAD);
+
+    imu_pub.publish(imu_msg);
 }
 
 void ARDroneDriver::publish_tf()

src/ardrone_driver.h

@@ -5,6 +5,7 @@
 #include <image_transport/image_transport.h>
 #include <tf/transform_broadcaster.h>
 #include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
 #include <ardrone_autonomy/Navdata.h>
 #include "ardrone_sdk.h"
 
@@ -35,6 +36,8 @@ private:
 	image_transport::CameraPublisher vert_pub;
 
     ros::Publisher navdata_pub;
+    ros::Publisher imu_pub;
+
     tf::TransformBroadcaster tf_broad;
 
 	//ros::Subscriber toggleCam_sub;
@@ -69,6 +72,9 @@ private:
      */
     std::string droneFrameBase, droneFrameIMU, droneFrameFrontCam, droneFrameBottomCam;
 
+    // Huge part of IMU message is constant, let's fill'em once.
+    sensor_msgs::Imu imu_msg;
+
 };
 
 #endif