Manually merging #25 to add magnetometer topic (by @sameerparekh)

AutonomyLab · Nov 10, 2012 · 0dc6a2a · 0dc6a2a
1 parent 0b58364
commit 0dc6a2a
Show file tree

Hide file tree

Showing 3 changed files with 83 additions and 54 deletions.
diff --git a/README.md b/README.md
@@ -6,6 +6,7 @@
 
 ### Updates
 
+- *November 9 2012*: Critical Bug in sending configurations to drone fixed ([More info](https://github.com/AutonomyLab/ardrone_autonomy/issues/24)). Seperate topic for magnetometer data added ([More info](https://github.com/AutonomyLab/ardrone_autonomy/pull/25)).
 - *September 5 2012*: Experimental automatic IMU bias removal.
 - *August 27 2012*: Thread-safe SDK data access. Synchronized `navdata` and `camera` topics.
 - *August 20 2012*: The driver is now provides ROS standard camera interface.
@@ -91,6 +92,10 @@ Information received from the drone will be published to the `ardrone/navdata` t
 
 The linear acceleration, angular velocity 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 `Base` frame. This topic is experimental. The covariance values are specified by specific parameters.
 
+### Megnetometer Data
+
+The normalized magnetometer readings are also published to `ardrone/mag` topic as a standard ROS [`geometry_msgs/Vector3Stamped`](http://www.ros.org/doc/api/geometry_msgs/html/msg/Vector3Stamped.html) message.
+
 ### 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). The driver is also a standard [ROS camera driver](http://www.ros.org/wiki/camera_drivers), therefor if camera calibration information is provided either as a set of ROS parameters or appropriate `ardrone_front.yaml` and/or `ardrone_bottom.yaml`, the information will be published in appropriate `camera_info` topics. Please check the FAQ section for more information.
@@ -208,9 +213,11 @@ The Parrot's license, copyright and disclaimer for `ARDroneLib` are included wit
 
 ## Contributors
 
-- [Rachel Brindle](https://github.com/younata) - [Enhanced Navdata for AR-Drone 2.0](https://github.com/AutonomyLab/ardrone_autonomy/pull/2)
+- [Mike Hamer](https://github.com/mikehamer) - Added support for proper SDK2 way of configuring the Drone via parameter (critical bug fix). [More Info](https://github.com/AutonomyLab/ardrone_autonomy/pull/26)
+- [Sameer Parekh](https://github.com/sameerparekh) - [Seperate Magnetometer Topic](https://github.com/AutonomyLab/ardrone_autonomy/pull/25)
 - [Devmax](https://github.com/devmax) - [Flat Trim](https://github.com/AutonomyLab/ardrone_autonomy/issues/18) + Various
 comments for enhancements
+- [Rachel Brindle](https://github.com/younata) - [Enhanced Navdata for AR-Drone 2.0](https://github.com/AutonomyLab/ardrone_autonomy/pull/2)
 
 ## FAQ
 

diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp
@@ -14,17 +14,18 @@ ARDroneDriver::ARDroneDriver()
     last_frame_id = -1;
     last_navdata_id = -1;
     cmd_vel_sub = node_handle.subscribe("cmd_vel", 1, &cmdVelCallback);
-	takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &takeoffCallback);
-	reset_sub = node_handle.subscribe("ardrone/reset", 1, &resetCallback);
-	land_sub = node_handle.subscribe("ardrone/land", 1, &landCallback);
-	image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
+    takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &takeoffCallback);
+    reset_sub = node_handle.subscribe("ardrone/reset", 1, &resetCallback);
+    land_sub = node_handle.subscribe("ardrone/land", 1, &landCallback);
+    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);
+    vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 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);
+    mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
+    toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
     setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
-	setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
+    setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
     flatTrim_service = node_handle.advertiseService("ardrone/flattrim", flatTrimCallback);
 
     /*
@@ -34,8 +35,8 @@ ARDroneDriver::ARDroneDriver()
     imuReCalib_service = node_handle.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>
             ("ardrone/imu_recalib", boost::bind(&ARDroneDriver::imuReCalibCallback, this, _1, _2));
 
-//	setEnemyColor_service = node_handle.advertiseService("/ardrone/setenemycolor", setEnemyColorCallback);
-//	setHullType_service = node_handle.advertiseService("/ardrone/sethulltype", setHullTypeCallback);
+    //	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_base";
     droneFrameBase = droneFrameId + "_link";
@@ -104,7 +105,7 @@ ARDroneDriver::ARDroneDriver()
     {
         tf_base_bottom.setData(tf_base_bottom.inverse());
         tf_base_bottom.child_frame_id_.swap(tf_base_bottom.frame_id_);
-    }   
+    }
 
 }
 
@@ -613,29 +614,31 @@ void ARDroneDriver::publish_navdata()
         navdata.vz -= vel_bias[2];
 
     }
-    if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0))
+    if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
         return; // why bother, no one is listening.
-	ardrone_autonomy::Navdata msg;
+    const ros::Time _now = ros::Time::now();
+
+    ardrone_autonomy::Navdata msg;
 
-    msg.header.stamp = ros::Time::now();
+    msg.header.stamp = _now;
     msg.header.frame_id = droneFrameBase;
-	msg.batteryPercent = navdata.vbat_flying_percentage;
+    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
-	
+    // 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
+
     // New stuff
 
     if (IS_ARDRONE2)
@@ -661,35 +664,33 @@ void ARDroneDriver::publish_navdata()
         msg.wind_comp_angle = 0.0;
     }
 
-	// Tag Detection
-	msg.tags_count = navdata_detect.nb_detected;
+    // 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]);
-	}
-
-    navdata_pub.publish(msg);
+    {
+        /*
+         * 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]);
+    }
 
     /* IMU */
     imu_msg.header.frame_id = droneFrameBase;
-    imu_msg.header.stamp = ros::Time::now();
+    imu_msg.header.stamp = _now;
 
     // IMU - Linear Acc
     imu_msg.linear_acceleration.x = msg.ax * 9.8;
@@ -707,6 +708,24 @@ void ARDroneDriver::publish_navdata()
     imu_msg.angular_velocity.y = -navdata_phys.phys_gyros[GYRO_Y] * DEG_TO_RAD;
     imu_msg.angular_velocity.z = -navdata_phys.phys_gyros[GYRO_Z] * DEG_TO_RAD;
 
+    mag_msg.header.frame_id = droneFrameBase;
+    mag_msg.header.stamp = _now;
+    const float mag_normalizer = sqrt( msg.magX * msg.magX + msg.magY * msg.magY + msg.magZ * msg.magZ );
+
+    // TODO: Check if it is really needed that magnetometer message includes normalized value
+    if (fabs(mag_normalizer) > 1e-9f)
+    {
+        mag_msg.vector.x = msg.magX / mag_normalizer;
+        mag_msg.vector.y = msg.magY / mag_normalizer;
+        mag_msg.vector.z = msg.magZ / mag_normalizer;
+        mag_pub.publish(mag_msg);
+    }
+    else
+    {
+        ROS_WARN_THROTTLE(1, "There is something wrong with the magnetometer readings (Magnitude is extremely small).");
+    }
+
+    navdata_pub.publish(msg);
     imu_pub.publish(imu_msg);
 }
 

diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h
@@ -7,6 +7,7 @@
 #include <camera_info_manager/camera_info_manager.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
+#include <geometry_msgs/Vector3Stamped.h>
 #include <std_srvs/Empty.h>
 #include <ardrone_autonomy/Navdata.h>
 #include "ardrone_sdk.h"
@@ -64,6 +65,7 @@ class ARDroneDriver
 
     ros::Publisher navdata_pub;
     ros::Publisher imu_pub;
+    ros::Publisher mag_pub;
 
     tf::TransformBroadcaster tf_broad;
 
@@ -113,6 +115,7 @@ class ARDroneDriver
 
     // Huge part of IMU message is constant, let's fill'em once.
     sensor_msgs::Imu imu_msg;
+    geometry_msgs::Vector3Stamped mag_msg;
 
     // Manual IMU caliberation
     bool do_caliberation;