Automatic IMU caliberation can now be disabled using

`do_imu_caliberation` rosparam and reset using `imu_recalib` service.
The `cmd_vel` queue size resized to 1.

CMakeLists.txt

@@ -28,6 +28,7 @@ rosbuild_gensrv()
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #target_link_libraries(example ${PROJECT_NAME})
 
+rosbuild_add_boost_directories()
 set(SDK ARDroneLib/)
 link_directories(${PROJECT_SOURCE_DIR}/lib/)
 include_directories(${SDK} ${SDK}/FFMPEG/Includes ${SDK}/Soft/Common ${SDK}/Soft/Lib ${SDK}/VP_SDK ${SDK}/VP_SDK/VP_Os/linux )

src/ardrone_driver.cpp

@@ -14,7 +14,7 @@ ARDroneDriver::ARDroneDriver()
     inited = false;
     last_frame_id = -1;
     last_navdata_id = -1;
-	cmd_vel_sub = node_handle.subscribe("cmd_vel", 100, &cmdVelCallback);
+    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);
@@ -27,6 +27,14 @@ ARDroneDriver::ARDroneDriver()
 	toggleNavdataDemo_service = node_handle.advertiseService("ardrone/togglenavdatademo", toggleNavdataDemoCallback);
 	setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
 	setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
+
+    /*
+        To be honest, I am not sure why advertising a service using class members should be this complicated!
+        One day, I should learn what is exactly happenning here. /M
+    */
+    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);
 
@@ -55,16 +63,10 @@ ARDroneDriver::ARDroneDriver()
 
     // Caliberation
     max_num_samples = 50;
-    caliberated = false;
-
-    for (int i = 0; i < 3; i++)
-    {
-        acc_bias[i] = 0.0;
-        vel_bias[i] = 0.0;
-        gyro_bias[i] = 0.0;
-        acc_samples.push_back(std::vector<double> ());
-        gyro_samples.push_back(std::vector<double> ());
-        vel_samples.push_back(std::vector<double> ());
+    do_caliberation = (ros::param::get("~do_imu_caliberation", do_caliberation)) ? do_caliberation : false;
+    if (do_caliberation) {
+        resetCaliberation();
+        ROS_WARN("Automatic IMU Caliberation is active.");
     }
 
     // Camera Info Manager
@@ -159,6 +161,23 @@ void ARDroneDriver::run()
     printf("ROS loop terminated ... \n");
 }
 
+void ARDroneDriver::resetCaliberation()
+{
+    caliberated = false;
+    acc_samples.clear();
+    gyro_samples.clear();
+    vel_samples.clear();
+    for (int i = 0; i < 3; i++)
+    {
+        acc_bias[i] = 0.0;
+        vel_bias[i] = 0.0;
+        gyro_bias[i] = 0.0;
+        acc_samples.push_back(std::vector<double> ());
+        gyro_samples.push_back(std::vector<double> ());
+        vel_samples.push_back(std::vector<double> ());
+    }
+}
+
 double ARDroneDriver::calcAverage(std::vector<double> &vec)
 {
     double ret = 0.0;
@@ -500,7 +519,7 @@ void ARDroneDriver::publish_navdata()
     vp_os_mutex_unlock(&navdata_lock);
 
 
-    if (!caliberated)
+    if ((do_caliberation) && (!caliberated))
     {
         acc_samples[0].push_back(navdata_phys.phys_accs[ACC_X]);
         acc_samples[1].push_back(navdata_phys.phys_accs[ACC_Y]);
@@ -519,13 +538,15 @@ void ARDroneDriver::publish_navdata()
                 gyro_bias[j] = calcAverage(gyro_samples[j]);
                 vel_bias[j] = calcAverage(vel_samples[j]);
             }
-            ROS_INFO("Bias in linear acceleration: [%4.4lf, %4.4lf, %4.4lf]", acc_bias[0], acc_bias[1], acc_bias[2]);
-            ROS_INFO("Bias in angular velocity: [%4.4lf, %4.4lf, %4.4lf]", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
-            ROS_INFO("Bias in linear velocity: [%4.4lf, %4.4lf, %4.4lf]", vel_bias[0], vel_bias[1], vel_bias[2]);
+            ROS_INFO("Bias in linear acceleration (mg): [%4.4lf, %4.4lf, %4.4lf]", acc_bias[0], acc_bias[1], acc_bias[2]);
+            ROS_INFO("Bias in angular velocity (deg/s): [%4.4lf, %4.4lf, %4.4lf]", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
+            ROS_INFO("Bias in linear velocity (mm/s): [%4.4lf, %4.4lf, %4.4lf]", vel_bias[0], vel_bias[1], vel_bias[2]);
+            ROS_INFO("Above values (except z-axis accel) will be substracted from actual IMU data in `navdata` and `imu` topic.");
+            ROS_INFO("This feature can be disabled using `do_imu_caliberation` parameter. Recaliberate using `imu_recalib` service.");
             caliberated = true;
         }
     }
-    if (caliberated)
+    if ((do_caliberation) && (caliberated))
     {
         for (int j = 0; j < 3; j++)
         {
@@ -642,6 +663,21 @@ void ARDroneDriver::publish_tf()
     tf_broad.sendTransform(tf_base_bottom);
 }
 
+bool ARDroneDriver::imuReCalibCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
+{
+    if (!do_caliberation)
+    {
+        ROS_WARN("Automatic IMU Caliberation is not active. Activate first using `do_imu_caliberation` parameter");
+        return false;
+    }
+    else
+    {
+        ROS_WARN("Recaliberating IMU, please do not move the drone for a couple of seconds.");
+        resetCaliberation();
+        return true;
+    }
+}
+
 void controlCHandler (int signal)
 {
     ros::shutdown();

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 <std_srvs/Empty.h>
 #include <ardrone_autonomy/Navdata.h>
 #include "ardrone_sdk.h"
 #include <vector>
@@ -30,12 +31,15 @@ public:
 
 	void run();
     double getRosParam(char* param, double defaultVal);
+    bool imuReCalibCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response &response);
+
 private:
 	void publish_video();
 	void publish_navdata();
     void publish_tf();
     bool readCovParams(std::string param_name, boost::array<double, 9> &cov_array);
     double calcAverage(std::vector<double> &vec);
+    void resetCaliberation();    
 
     ros::NodeHandle node_handle;
 	ros::Subscriber cmd_vel_sub;
@@ -60,6 +64,7 @@ private:
 	ros::ServiceServer toggleNavdataDemo_service;
 	ros::ServiceServer setCamChannel_service;
 	ros::ServiceServer setLedAnimation_service;
+    ros::ServiceServer imuReCalib_service;
 	
 	/*
 	 * Orange Green : 1
@@ -102,6 +107,7 @@ private:
     sensor_msgs::Imu imu_msg;
 
     // Manual IMU caliberation
+    bool do_caliberation;
     int max_num_samples;
     bool caliberated;
     double acc_bias[3];

src/ardrone_sdk.h

@@ -42,8 +42,8 @@ extern "C" {
 extern video_decoder_config_t vec;
 }
 
-#define NB_DRIVER_POST_STAGES   10
 
+#define NB_DRIVER_POST_STAGES   10
 extern navdata_vision_detect_t shared_navdata_detect;
 extern navdata_phys_measures_t shared_navdata_phys;
 extern navdata_demo_t shared_navdata;