microstrain_3dmgx2_imu: microstrain_3dmgx2_imu: imu

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Microstrain 3DM-GX2 node
00005  *  Copyright (c) 2008-2010, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  */
00035 
00036 #include <assert.h>
00037 #include <math.h>
00038 #include <iostream>
00039 
00040 #include <boost/format.hpp>
00041 
00042 #include "microstrain_3dmgx2_imu/3dmgx2.h"
00043 
00044 #include "ros/time.h"
00045 #include "self_test/self_test.h"
00046 #include "diagnostic_msgs/DiagnosticStatus.h"
00047 #include "diagnostic_updater/diagnostic_updater.h"
00048 #include "diagnostic_updater/update_functions.h"
00049 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00050 
00051 #include "sensor_msgs/Imu.h"
00052 #include "std_srvs/Empty.h"
00053 
00054 #include "tf/transform_datatypes.h"
00055 #include "microstrain_3dmgx2_imu/AddOffset.h"
00056 
00057 #include "std_msgs/Bool.h"
00058 
00059 using namespace std;
00060 
00061 class ImuNode 
00062 {
00063 public:
00064   microstrain_3dmgx2_imu::IMU imu;
00065   sensor_msgs::Imu reading;
00066 
00067   string port;
00068 
00069   microstrain_3dmgx2_imu::IMU::cmd cmd;
00070 
00071   self_test::TestRunner self_test_;
00072   diagnostic_updater::Updater diagnostic_;
00073 
00074   ros::NodeHandle node_handle_;
00075   ros::NodeHandle private_node_handle_;
00076   ros::Publisher imu_data_pub_;
00077   ros::ServiceServer add_offset_serv_;
00078   ros::ServiceServer calibrate_serv_;
00079   ros::Publisher is_calibrated_pub_;
00080 
00081   bool running;
00082 
00083   bool autocalibrate_;
00084   bool calibrate_requested_;
00085   bool calibrated_;
00086   
00087   int error_count_;
00088   int slow_count_;
00089   std::string was_slow_;
00090   std::string error_status_;
00091 
00092   string frameid_;
00093   
00094   double offset_;
00095     
00096   double bias_x_;
00097   double bias_y_;
00098   double bias_z_;
00099 
00100   double angular_velocity_stdev_, angular_velocity_covariance_;
00101   double linear_acceleration_covariance_, linear_acceleration_stdev_;
00102   double orientation_covariance_, orientation_stdev_;
00103 
00104   double max_drift_rate_;
00105 
00106   double desired_freq_;
00107   diagnostic_updater::FrequencyStatus freq_diag_;
00108   
00109   ImuNode(ros::NodeHandle h) : self_test_(), diagnostic_(), 
00110   node_handle_(h), private_node_handle_("~"), calibrate_requested_(false),
00111   error_count_(0), 
00112   slow_count_(0), 
00113   desired_freq_(100), 
00114   freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05))
00115   {
00116     ros::NodeHandle imu_node_handle(node_handle_, "imu");
00117     
00118     private_node_handle_.param("autocalibrate", autocalibrate_, true);
00119     private_node_handle_.param("assume_calibrated", calibrated_, false);
00120     private_node_handle_.param("port", port, string("/dev/ttyUSB0"));
00121     private_node_handle_.param("max_drift_rate", max_drift_rate_, 0.0002);
00122 
00123 
00124     imu_data_pub_ = imu_node_handle.advertise<sensor_msgs::Imu>("data", 100);
00125     add_offset_serv_ = private_node_handle_.advertiseService("add_offset", &ImuNode::addOffset, this);
00126     calibrate_serv_ = imu_node_handle.advertiseService("calibrate", &ImuNode::calibrate, this);
00127     is_calibrated_pub_ = imu_node_handle.advertise<std_msgs::Bool>("is_calibrated", 1, true);
00128 
00129     publish_is_calibrated();
00130 
00131     cmd = microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE_ORIENT;
00132     
00133     running = false;
00134 
00135     bias_x_ = bias_y_ = bias_z_ = 0;
00136 
00137     private_node_handle_.param("frame_id", frameid_, string("imu"));
00138     reading.header.frame_id = frameid_;
00139 
00140     private_node_handle_.param("time_offset", offset_, 0.0);
00141 
00142     private_node_handle_.param("linear_acceleration_stdev", linear_acceleration_stdev_, 0.098); 
00143     private_node_handle_.param("orientation_stdev", orientation_stdev_, 0.035); 
00144     private_node_handle_.param("angular_velocity_stdev", angular_velocity_stdev_, 0.012); 
00145 
00146     double angular_velocity_covariance = angular_velocity_stdev_ * angular_velocity_stdev_;
00147     double orientation_covariance = orientation_stdev_ * orientation_stdev_;
00148     double linear_acceleration_covariance = linear_acceleration_stdev_ * linear_acceleration_stdev_;
00149     
00150     reading.linear_acceleration_covariance[0] = linear_acceleration_covariance;
00151     reading.linear_acceleration_covariance[4] = linear_acceleration_covariance;
00152     reading.linear_acceleration_covariance[8] = linear_acceleration_covariance;
00153 
00154     reading.angular_velocity_covariance[0] = angular_velocity_covariance;
00155     reading.angular_velocity_covariance[4] = angular_velocity_covariance;
00156     reading.angular_velocity_covariance[8] = angular_velocity_covariance;
00157     
00158     reading.orientation_covariance[0] = orientation_covariance;
00159     reading.orientation_covariance[4] = orientation_covariance;
00160     reading.orientation_covariance[8] = orientation_covariance;
00161     
00162     self_test_.add("Close Test", this, &ImuNode::pretest);
00163     self_test_.add("Interruption Test", this, &ImuNode::InterruptionTest);
00164     self_test_.add("Connect Test", this, &ImuNode::ConnectTest);
00165     self_test_.add("Read ID Test", this, &ImuNode::ReadIDTest);
00166     self_test_.add("Gyro Bias Test", this, &ImuNode::GyroBiasTest);
00167     self_test_.add("Streamed Data Test", this, &ImuNode::StreamedDataTest);
00168     self_test_.add("Gravity Test", this, &ImuNode::GravityTest);
00169     self_test_.add("Disconnect Test", this, &ImuNode::DisconnectTest);
00170     self_test_.add("Resume Test", this, &ImuNode::ResumeTest);
00171 
00172     diagnostic_.add( freq_diag_ );
00173     diagnostic_.add( "Calibration Status", this, &ImuNode::calibrationStatus );
00174     diagnostic_.add( "IMU Status", this, &ImuNode::deviceStatus );
00175   }
00176 
00177   ~ImuNode()
00178   {
00179     stop();
00180   }
00181 
00182   void setErrorStatusf(const char *format, ...)
00183   {
00184     va_list va;
00185     char buff[1000]; 
00186     va_start(va, format);
00187     if (vsnprintf(buff, 1000, format, va) >= 1000)
00188       ROS_DEBUG("Really long string in setErrorStatus, it was truncated.");
00189     std::string value = std::string(buff);
00190     setErrorStatus(buff);
00191   }
00192 
00193   // Prints an error message if it isn't the same old error message.
00194   void setErrorStatus(const std::string msg)
00195   {
00196     if (error_status_ != msg)
00197     {
00198       error_status_ = msg;
00199       ROS_ERROR("%s You may find further details at http://www.ros.org/wiki/microstrain_3dmgx2_imu/Troubleshooting", msg.c_str());
00200     }
00201     else
00202     {
00203       ROS_DEBUG("%s You may find further details at http://www.ros.org/wiki/microstrain_3dmgx2_imu/Troubleshooting", msg.c_str());
00204     }
00205   } 
00206 
00207   void clearErrorStatus()
00208   {
00209     error_status_.clear();
00210   }
00211 
00212   int start()
00213   {
00214     stop();
00215 
00216     try
00217     {
00218       try
00219       {
00220         imu.openPort(port.c_str());
00221       } catch (microstrain_3dmgx2_imu::Exception& e) {
00222         error_count_++;
00223         setErrorStatus(e.what());
00224         diagnostic_.broadcast(2, e.what());
00225         return -1;
00226       }
00227 
00228       diagnostic_.setHardwareID(getID(true));
00229 
00230       ROS_INFO("Initializing IMU time with offset %f.", offset_);
00231 
00232       imu.initTime(offset_);
00233 
00234       if (autocalibrate_ || calibrate_requested_)
00235       {
00236         doCalibrate();
00237         calibrate_requested_ = false;
00238         autocalibrate_ = false; // No need to do this each time we reopen the device.
00239       }
00240       else
00241       {
00242         ROS_INFO("Not calibrating the IMU sensor. Use the calibrate service to calibrate it before use.");
00243       }
00244 
00245       ROS_INFO("IMU sensor initialized.");
00246 
00247       imu.setContinuous(cmd);
00248 
00249       freq_diag_.clear();
00250 
00251       running = true;
00252 
00253     } catch (microstrain_3dmgx2_imu::Exception& e) {
00254       error_count_++;
00255       setErrorStatusf("Exception thrown while starting IMU. This sometimes happens if you are not connected to an IMU or if another process is trying to access the IMU port. You may try 'lsof|grep %s' to see if other processes have the port open. %s", port.c_str(), e.what());
00256       diagnostic_.broadcast(2, "Error opening IMU.");
00257       return -1;
00258     }
00259 
00260     return(0);
00261   }
00262       
00263   std::string getID(bool output_info = false)
00264   {
00265       char dev_name[17];
00266       char dev_model_num[17];
00267       char dev_serial_num[17];
00268       char dev_opt[17];
00269       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_DEVICE_NAME, dev_name);
00270       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_MODEL_NUMBER, dev_model_num);
00271       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_SERIAL_NUMBER, dev_serial_num);
00272       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_DEVICE_OPTIONS, dev_opt);
00273       
00274       if (output_info)
00275         ROS_INFO("Connected to IMU [%s] model [%s] s/n [%s] options [%s]",
00276           dev_name, dev_model_num, dev_serial_num, dev_opt);
00277       
00278       char *dev_name_ptr = dev_name;
00279       char *dev_model_num_ptr = dev_model_num;
00280       char *dev_serial_num_ptr = dev_serial_num;
00281       
00282       while (*dev_name_ptr == ' ')
00283         dev_name_ptr++;
00284       while (*dev_model_num_ptr == ' ')
00285         dev_model_num_ptr++;
00286       while (*dev_serial_num_ptr == ' ')
00287         dev_serial_num_ptr++;
00288 
00289       return (boost::format("%s_%s-%s")%dev_name_ptr%dev_model_num_ptr%dev_serial_num_ptr).str();
00290   }
00291   
00292   int stop()
00293   {
00294     if(running)
00295     {
00296       try
00297       {
00298         imu.closePort();
00299       } catch (microstrain_3dmgx2_imu::Exception& e) {
00300         error_count_++;
00301         ROS_INFO("Exception thrown while stopping IMU. %s", e.what());
00302       }
00303       running = false;
00304     }
00305 
00306     return(0);
00307   }
00308 
00309   int publish_datum()
00310   {
00311     try
00312     {
00313       static double prevtime = 0;
00314       double starttime = ros::Time::now().toSec();
00315       if (prevtime && prevtime - starttime > 0.05)
00316       {
00317         ROS_WARN("Full IMU loop took %f ms. Nominal is 10ms.", 1000 * (prevtime - starttime));
00318         was_slow_ = "Full IMU loop was slow.";
00319         slow_count_++;
00320       }
00321       getData(reading);
00322       double endtime = ros::Time::now().toSec();
00323       if (endtime - starttime > 0.05)
00324       {
00325         ROS_WARN("Gathering data took %f ms. Nominal is 10ms.", 1000 * (endtime - starttime));
00326         was_slow_ = "Full IMU loop was slow.";
00327         slow_count_++;
00328       }
00329       prevtime = starttime;
00330       starttime = ros::Time::now().toSec();
00331       imu_data_pub_.publish(reading);
00332       endtime = ros::Time::now().toSec();
00333       if (endtime - starttime > 0.05)
00334       {
00335         ROS_WARN("Publishing took %f ms. Nominal is 10 ms.", 1000 * (endtime - starttime));
00336         was_slow_ = "Full IMU loop was slow.";
00337         slow_count_++;
00338       }
00339         
00340       freq_diag_.tick();
00341       clearErrorStatus(); // If we got here, then the IMU really is working. Next time an error occurs, we want to print it.
00342     } catch (microstrain_3dmgx2_imu::Exception& e) {
00343       error_count_++;
00344       usleep(20000); // Give isShuttingDown a chance to go true.
00345       if (!ros::isShuttingDown()) // Don't warn if we are shutting down.
00346         ROS_WARN("Exception thrown while trying to get the IMU reading. This sometimes happens due to a communication glitch, or if another process is trying to access the IMU port. You may try 'lsof|grep %s' to see if other processes have the port open. %s", port.c_str(), e.what());
00347       return -1;
00348     }
00349 
00350     return(0);
00351   }
00352 
00353   bool spin()
00354   {
00355     while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
00356     {
00357       if (start() == 0)
00358       {
00359         while(node_handle_.ok()) {
00360           if(publish_datum() < 0)
00361             break;
00362           self_test_.checkTest();
00363           diagnostic_.update();
00364           ros::spinOnce();
00365         }
00366       } else {
00367         // No need for diagnostic here since a broadcast occurs in start
00368         // when there is an error.
00369         usleep(1000000);
00370         self_test_.checkTest();
00371         ros::spinOnce();
00372       }
00373     }
00374 
00375     stop();
00376 
00377     return true;
00378   }
00379 
00380   void publish_is_calibrated()
00381   {
00382     std_msgs::Bool msg;
00383     msg.data = calibrated_;
00384     is_calibrated_pub_.publish(msg);
00385   }
00386 
00387   void pretest(diagnostic_updater::DiagnosticStatusWrapper& status)
00388   {
00389     try
00390     {
00391       imu.closePort();
00392 
00393       status.summary(0, "Device closed successfully.");
00394     } catch (microstrain_3dmgx2_imu::Exception& e) {
00395       status.summary(1, "Failed to close device.");
00396     }
00397   }
00398 
00399   void InterruptionTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00400   {
00401     if (imu_data_pub_.getNumSubscribers() == 0 )
00402       status.summary(0, "No operation interrupted.");
00403     else
00404       status.summary(1, "There were active subscribers.  Running of self test interrupted operations.");
00405   }
00406 
00407   void ConnectTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00408   {
00409     imu.openPort(port.c_str());
00410 
00411     status.summary(0, "Connected successfully.");
00412   }
00413 
00414   void ReadIDTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00415   {
00416     self_test_.setID(getID());
00417     
00418     status.summary(0, "Read Successfully");
00419   }
00420 
00421   void GyroBiasTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00422   {
00423     imu.initGyros(&bias_x_, &bias_y_, &bias_z_);
00424 
00425     status.summary(0, "Successfully calculated gyro biases.");
00426 
00427     status.add("Bias_X", bias_x_);
00428     status.add("Bias_Y", bias_y_);
00429     status.add("Bias_Z", bias_z_);
00430   }
00431 
00432 
00433   void getData(sensor_msgs::Imu& data)
00434   {
00435     uint64_t time;
00436     double accel[3];
00437     double angrate[3];
00438     double orientation[9];
00439 
00440     imu.receiveAccelAngrateOrientation(&time, accel, angrate, orientation);
00441     data.linear_acceleration.x = accel[0];
00442     data.linear_acceleration.y = accel[1];
00443     data.linear_acceleration.z = accel[2];
00444  
00445     data.angular_velocity.x = angrate[0];
00446     data.angular_velocity.y = angrate[1];
00447     data.angular_velocity.z = angrate[2];
00448       
00449     tf::Quaternion quat;
00450     (tf::Matrix3x3(-1,0,0,
00451                  0,1,0,
00452                  0,0,-1)*
00453     tf::Matrix3x3(orientation[0], orientation[3], orientation[6],
00454                  orientation[1], orientation[4], orientation[7],
00455                  orientation[2], orientation[5], orientation[8])).getRotation(quat);
00456     
00457     tf::quaternionTFToMsg(quat, data.orientation);
00458       
00459     data.header.stamp = ros::Time::now().fromNSec(time);
00460   }
00461 
00462 
00463   void StreamedDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00464   {
00465     uint64_t time;
00466     double accel[3];
00467     double angrate[3];
00468 
00469     if (!imu.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE))
00470     {
00471       status.summary(2, "Could not start streaming data.");
00472     } else {
00473 
00474       for (int i = 0; i < 100; i++)
00475       {
00476         imu.receiveAccelAngrate(&time, accel, angrate);
00477       }
00478       
00479       imu.stopContinuous();
00480 
00481       status.summary(0, "Data streamed successfully.");
00482     }
00483   }
00484 
00485   void GravityTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00486   {
00487     uint64_t time;
00488     double accel[3];
00489     double angrate[3];
00490 
00491     double grav = 0.0;
00492 
00493     double grav_x = 0.0;
00494     double grav_y = 0.0;
00495     double grav_z = 0.0;
00496 
00497     if (!imu.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE))
00498     {
00499       status.summary(2, "Could not start streaming data.");
00500     } else {
00501 
00502       int num = 200;
00503 
00504       for (int i = 0; i < num; i++)
00505       {
00506         imu.receiveAccelAngrate(&time, accel, angrate);
00507         
00508         grav_x += accel[0];
00509         grav_y += accel[1];
00510         grav_z += accel[2];
00511 
00512       }
00513       
00514       imu.stopContinuous();
00515 
00516       grav += sqrt( pow(grav_x / (double)(num), 2.0) + 
00517                     pow(grav_y / (double)(num), 2.0) + 
00518                     pow(grav_z / (double)(num), 2.0));
00519       
00520       //      double err = (grav - microstrain_3dmgx2_imu::G);
00521       double err = (grav - 9.796);
00522       
00523       if (fabs(err) < .05)
00524       {
00525         status.summary(0, "Gravity detected correctly.");
00526       } else {
00527         status.summaryf(2, "Measured gravity deviates by %f", err);
00528       }
00529 
00530       status.add("Measured gravity", grav);
00531       status.add("Gravity error", err);
00532     }
00533   }
00534 
00535   void DisconnectTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00536   {
00537     imu.closePort();
00538 
00539     status.summary(0, "Disconnected successfully.");
00540   }
00541 
00542   void ResumeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00543   {
00544     if (running)
00545     {
00546 
00547       imu.openPort(port.c_str());
00548       freq_diag_.clear();
00549 
00550       if (imu.setContinuous(cmd) != true)
00551       {
00552         status.summary(2, "Failed to resume previous mode of operation.");
00553         return;
00554       }
00555     }
00556 
00557     status.summary(0, "Previous operation resumed successfully.");    
00558   }
00559 
00560   void deviceStatus(diagnostic_updater::DiagnosticStatusWrapper &status)
00561   {
00562     if (!running)
00563       status.summary(2, "IMU is stopped");
00564     else if (!was_slow_.empty())
00565     {
00566       status.summary(1, "Excessive delay");
00567       was_slow_.clear();
00568     }
00569     else
00570       status.summary(0, "IMU is running");
00571 
00572     status.add("Device", port);
00573     status.add("TF frame", frameid_);
00574     status.add("Error count", error_count_);
00575     status.add("Excessive delay", slow_count_);
00576   }
00577 
00578   void calibrationStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00579   {
00580     if (calibrated_)
00581     {
00582       status.summary(0, "Gyro is calibrated");
00583       status.add("X bias", bias_x_);
00584       status.add("Y bias", bias_y_);
00585       status.add("Z bias", bias_z_);
00586     }
00587     else
00588       status.summary(2, "Gyro not calibrated");
00589       // Note to Kevin. Yes the IMU is running, but it is outputting
00590       // garbage, so this is an error.
00591 
00592   }
00593 
00594   bool addOffset(microstrain_3dmgx2_imu::AddOffset::Request &req, microstrain_3dmgx2_imu::AddOffset::Response &resp)
00595   {
00596     double offset = req.add_offset;
00597     offset_ += offset;
00598 
00599     ROS_INFO("Adding %f to existing IMU time offset.", offset);
00600     ROS_INFO("Total IMU time offset is now %f.", offset_);
00601 
00602     // send changes to imu driver
00603     imu.setFixedOffset(offset_);
00604 
00605     // write changes to param server
00606     private_node_handle_.setParam("time_offset", offset_);
00607 
00608     // set response
00609     resp.total_offset = offset_;
00610 
00611     return true;
00612   }
00613 
00614   bool calibrate(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
00615   {
00616     bool old_running = running;
00617 
00618     try
00619     {
00620       calibrate_requested_ = true;
00621       if (old_running)
00622       {
00623         stop();
00624         start(); // Start will do the calibration.
00625       }
00626       else
00627       {
00628         imu.openPort(port.c_str());
00629         doCalibrate();
00630         imu.closePort();
00631       } 
00632     } catch (microstrain_3dmgx2_imu::Exception& e) {
00633       error_count_++;
00634       calibrated_ = false;
00635       publish_is_calibrated();
00636       ROS_ERROR("Exception thrown while calibrating IMU %s", e.what());
00637       stop();
00638       if (old_running)
00639         start(); // Might throw, but we have nothing to lose... Needs restructuring.
00640       return false;
00641     }
00642     
00643     return true;
00644   }
00645 
00646   void doCalibrate()
00647   { // Expects to be called with the IMU stopped.
00648     ROS_INFO("Calibrating IMU gyros.");
00649     imu.initGyros(&bias_x_, &bias_y_, &bias_z_);
00650     
00651     // check calibration
00652     if (!imu.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE_ORIENT)){
00653       ROS_ERROR("Could not start streaming data to verify calibration");
00654     } 
00655     else {
00656       double x_rate = 0.0;
00657       double y_rate = 0.0;
00658       double z_rate = 0.0;
00659       size_t count = 0;
00660       getData(reading);
00661       ros::Time start_time = reading.header.stamp;
00662 
00663       while(reading.header.stamp - start_time < ros::Duration(2.0)){
00664         getData(reading);
00665         x_rate += reading.angular_velocity.x;
00666         y_rate += reading.angular_velocity.y;
00667         z_rate += reading.angular_velocity.z;
00668         ++count;
00669       }
00670 
00671       double average_rate = sqrt(x_rate*x_rate + y_rate*y_rate + z_rate*z_rate) / count;
00672 
00673       if (count < 200){
00674         ROS_WARN("Imu: calibration check acquired fewer than 200 samples.");
00675       }
00676       
00677       // calibration succeeded
00678       if (average_rate < max_drift_rate_) {
00679         ROS_INFO("Imu: calibration check succeeded: average angular drift %f mdeg/sec < %f mdeg/sec", average_rate*180*1000/M_PI, max_drift_rate_*180*1000/M_PI);
00680         calibrated_ = true;
00681         publish_is_calibrated();
00682         ROS_INFO("IMU gyro calibration completed.");
00683         freq_diag_.clear();
00684       }
00685       // calibration failed
00686       else{
00687         calibrated_ = false;
00688         publish_is_calibrated();
00689         ROS_ERROR("Imu: calibration check failed: average angular drift = %f mdeg/sec > %f mdeg/sec", average_rate*180*1000/M_PI, max_drift_rate_*180*1000/M_PI);
00690       }
00691       imu.stopContinuous();
00692     }
00693   }
00694 };
00695 
00696 int
00697 main(int argc, char** argv)
00698 {
00699   ros::init(argc, argv, "microstrain_3dmgx2_node");
00700 
00701   ros::NodeHandle nh;
00702 
00703   ImuNode in(nh);
00704   in.spin();
00705 
00706   return(0);
00707 }
imu_node.cc
