imu_node.cc
Go to the documentation of this file.
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       usleep(100000); // Give isShuttingDown a chance to go true.
00256       if (!ros::isShuttingDown()){ // Don't warn if we are shutting down.
00257         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());
00258         diagnostic_.broadcast(2, "Error opening IMU.");
00259       }
00260       return -1;
00261     }
00262 
00263     return(0);
00264   }
00265       
00266   std::string getID(bool output_info = false)
00267   {
00268       char dev_name[17];
00269       char dev_model_num[17];
00270       char dev_serial_num[17];
00271       char dev_opt[17];
00272       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_DEVICE_NAME, dev_name);
00273       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_MODEL_NUMBER, dev_model_num);
00274       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_SERIAL_NUMBER, dev_serial_num);
00275       imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_DEVICE_OPTIONS, dev_opt);
00276       
00277       if (output_info)
00278         ROS_INFO("Connected to IMU [%s] model [%s] s/n [%s] options [%s]",
00279           dev_name, dev_model_num, dev_serial_num, dev_opt);
00280       
00281       char *dev_name_ptr = dev_name;
00282       char *dev_model_num_ptr = dev_model_num;
00283       char *dev_serial_num_ptr = dev_serial_num;
00284       
00285       while (*dev_name_ptr == ' ')
00286         dev_name_ptr++;
00287       while (*dev_model_num_ptr == ' ')
00288         dev_model_num_ptr++;
00289       while (*dev_serial_num_ptr == ' ')
00290         dev_serial_num_ptr++;
00291 
00292       return (boost::format("%s_%s-%s")%dev_name_ptr%dev_model_num_ptr%dev_serial_num_ptr).str();
00293   }
00294   
00295   int stop()
00296   {
00297     if(running)
00298     {
00299       try
00300       {
00301         imu.closePort();
00302       } catch (microstrain_3dmgx2_imu::Exception& e) {
00303         error_count_++;
00304         ROS_INFO("Exception thrown while stopping IMU. %s", e.what());
00305       }
00306       running = false;
00307     }
00308 
00309     return(0);
00310   }
00311 
00312   int publish_datum()
00313   {
00314     try
00315     {
00316       static double prevtime = 0;
00317       double starttime = ros::Time::now().toSec();
00318       if (prevtime && prevtime - starttime > 0.05)
00319       {
00320         ROS_WARN("Full IMU loop took %f ms. Nominal is 10ms.", 1000 * (prevtime - starttime));
00321         was_slow_ = "Full IMU loop was slow.";
00322         slow_count_++;
00323       }
00324       getData(reading);
00325       double endtime = ros::Time::now().toSec();
00326       if (endtime - starttime > 0.05)
00327       {
00328         ROS_WARN("Gathering data took %f ms. Nominal is 10ms.", 1000 * (endtime - starttime));
00329         was_slow_ = "Full IMU loop was slow.";
00330         slow_count_++;
00331       }
00332       prevtime = starttime;
00333       starttime = ros::Time::now().toSec();
00334       imu_data_pub_.publish(reading);
00335       endtime = ros::Time::now().toSec();
00336       if (endtime - starttime > 0.05)
00337       {
00338         ROS_WARN("Publishing took %f ms. Nominal is 10 ms.", 1000 * (endtime - starttime));
00339         was_slow_ = "Full IMU loop was slow.";
00340         slow_count_++;
00341       }
00342         
00343       freq_diag_.tick();
00344       clearErrorStatus(); // If we got here, then the IMU really is working. Next time an error occurs, we want to print it.
00345     } catch (microstrain_3dmgx2_imu::Exception& e) {
00346       error_count_++;
00347       usleep(100000); // Give isShuttingDown a chance to go true.
00348       if (!ros::isShuttingDown()) // Don't warn if we are shutting down.
00349         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());
00350       return -1;
00351     }
00352 
00353     return(0);
00354   }
00355 
00356   bool spin()
00357   {
00358     while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
00359     {
00360       if (start() == 0)
00361       {
00362         while(node_handle_.ok()) {
00363           if(publish_datum() < 0)
00364             break;
00365           self_test_.checkTest();
00366           diagnostic_.update();
00367           ros::spinOnce();
00368         }
00369       } else {
00370         // No need for diagnostic here since a broadcast occurs in start
00371         // when there is an error.
00372         usleep(1000000);
00373         self_test_.checkTest();
00374         ros::spinOnce();
00375       }
00376     }
00377 
00378     stop();
00379 
00380     return true;
00381   }
00382 
00383   void publish_is_calibrated()
00384   {
00385     std_msgs::Bool msg;
00386     msg.data = calibrated_;
00387     is_calibrated_pub_.publish(msg);
00388   }
00389 
00390   void pretest(diagnostic_updater::DiagnosticStatusWrapper& status)
00391   {
00392     try
00393     {
00394       imu.closePort();
00395 
00396       status.summary(0, "Device closed successfully.");
00397     } catch (microstrain_3dmgx2_imu::Exception& e) {
00398       status.summary(1, "Failed to close device.");
00399     }
00400   }
00401 
00402   void InterruptionTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00403   {
00404     if (imu_data_pub_.getNumSubscribers() == 0 )
00405       status.summary(0, "No operation interrupted.");
00406     else
00407       status.summary(1, "There were active subscribers.  Running of self test interrupted operations.");
00408   }
00409 
00410   void ConnectTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00411   {
00412     imu.openPort(port.c_str());
00413 
00414     status.summary(0, "Connected successfully.");
00415   }
00416 
00417   void ReadIDTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00418   {
00419     self_test_.setID(getID());
00420     
00421     status.summary(0, "Read Successfully");
00422   }
00423 
00424   void GyroBiasTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00425   {
00426     imu.initGyros(&bias_x_, &bias_y_, &bias_z_);
00427 
00428     status.summary(0, "Successfully calculated gyro biases.");
00429 
00430     status.add("Bias_X", bias_x_);
00431     status.add("Bias_Y", bias_y_);
00432     status.add("Bias_Z", bias_z_);
00433   }
00434 
00435 
00436   void getData(sensor_msgs::Imu& data)
00437   {
00438     uint64_t time;
00439     double accel[3];
00440     double angrate[3];
00441     double orientation[9];
00442 
00443     imu.receiveAccelAngrateOrientation(&time, accel, angrate, orientation);
00444     data.linear_acceleration.x = accel[0];
00445     data.linear_acceleration.y = accel[1];
00446     data.linear_acceleration.z = accel[2];
00447  
00448     data.angular_velocity.x = angrate[0];
00449     data.angular_velocity.y = angrate[1];
00450     data.angular_velocity.z = angrate[2];
00451       
00452     tf::Quaternion quat;
00453     (tf::Matrix3x3(-1,0,0,
00454                  0,1,0,
00455                  0,0,-1)*
00456     tf::Matrix3x3(orientation[0], orientation[3], orientation[6],
00457                  orientation[1], orientation[4], orientation[7],
00458                  orientation[2], orientation[5], orientation[8])).getRotation(quat);
00459     
00460     tf::quaternionTFToMsg(quat, data.orientation);
00461       
00462     data.header.stamp = ros::Time::now().fromNSec(time);
00463   }
00464 
00465 
00466   void StreamedDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00467   {
00468     uint64_t time;
00469     double accel[3];
00470     double angrate[3];
00471 
00472     if (!imu.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE))
00473     {
00474       status.summary(2, "Could not start streaming data.");
00475     } else {
00476 
00477       for (int i = 0; i < 100; i++)
00478       {
00479         imu.receiveAccelAngrate(&time, accel, angrate);
00480       }
00481       
00482       imu.stopContinuous();
00483 
00484       status.summary(0, "Data streamed successfully.");
00485     }
00486   }
00487 
00488   void GravityTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00489   {
00490     uint64_t time;
00491     double accel[3];
00492     double angrate[3];
00493 
00494     double grav = 0.0;
00495 
00496     double grav_x = 0.0;
00497     double grav_y = 0.0;
00498     double grav_z = 0.0;
00499 
00500     if (!imu.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE))
00501     {
00502       status.summary(2, "Could not start streaming data.");
00503     } else {
00504 
00505       int num = 200;
00506 
00507       for (int i = 0; i < num; i++)
00508       {
00509         imu.receiveAccelAngrate(&time, accel, angrate);
00510         
00511         grav_x += accel[0];
00512         grav_y += accel[1];
00513         grav_z += accel[2];
00514 
00515       }
00516       
00517       imu.stopContinuous();
00518 
00519       grav += sqrt( pow(grav_x / (double)(num), 2.0) + 
00520                     pow(grav_y / (double)(num), 2.0) + 
00521                     pow(grav_z / (double)(num), 2.0));
00522       
00523       //      double err = (grav - microstrain_3dmgx2_imu::G);
00524       double err = (grav - 9.796);
00525       
00526       if (fabs(err) < .05)
00527       {
00528         status.summary(0, "Gravity detected correctly.");
00529       } else {
00530         status.summaryf(2, "Measured gravity deviates by %f", err);
00531       }
00532 
00533       status.add("Measured gravity", grav);
00534       status.add("Gravity error", err);
00535     }
00536   }
00537 
00538   void DisconnectTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00539   {
00540     imu.closePort();
00541 
00542     status.summary(0, "Disconnected successfully.");
00543   }
00544 
00545   void ResumeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00546   {
00547     if (running)
00548     {
00549 
00550       imu.openPort(port.c_str());
00551       freq_diag_.clear();
00552 
00553       if (imu.setContinuous(cmd) != true)
00554       {
00555         status.summary(2, "Failed to resume previous mode of operation.");
00556         return;
00557       }
00558     }
00559 
00560     status.summary(0, "Previous operation resumed successfully.");    
00561   }
00562 
00563   void deviceStatus(diagnostic_updater::DiagnosticStatusWrapper &status)
00564   {
00565     if (!running)
00566       status.summary(2, "IMU is stopped");
00567     else if (!was_slow_.empty())
00568     {
00569       status.summary(1, "Excessive delay");
00570       was_slow_.clear();
00571     }
00572     else
00573       status.summary(0, "IMU is running");
00574 
00575     status.add("Device", port);
00576     status.add("TF frame", frameid_);
00577     status.add("Error count", error_count_);
00578     status.add("Excessive delay", slow_count_);
00579   }
00580 
00581   void calibrationStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00582   {
00583     if (calibrated_)
00584     {
00585       status.summary(0, "Gyro is calibrated");
00586       status.add("X bias", bias_x_);
00587       status.add("Y bias", bias_y_);
00588       status.add("Z bias", bias_z_);
00589     }
00590     else
00591       status.summary(2, "Gyro not calibrated");
00592       // Note to Kevin. Yes the IMU is running, but it is outputting
00593       // garbage, so this is an error.
00594 
00595   }
00596 
00597   bool addOffset(microstrain_3dmgx2_imu::AddOffset::Request &req, microstrain_3dmgx2_imu::AddOffset::Response &resp)
00598   {
00599     double offset = req.add_offset;
00600     offset_ += offset;
00601 
00602     ROS_INFO("Adding %f to existing IMU time offset.", offset);
00603     ROS_INFO("Total IMU time offset is now %f.", offset_);
00604 
00605     // send changes to imu driver
00606     imu.setFixedOffset(offset_);
00607 
00608     // write changes to param server
00609     private_node_handle_.setParam("time_offset", offset_);
00610 
00611     // set response
00612     resp.total_offset = offset_;
00613 
00614     return true;
00615   }
00616 
00617   bool calibrate(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
00618   {
00619     bool old_running = running;
00620 
00621     try
00622     {
00623       calibrate_requested_ = true;
00624       if (old_running)
00625       {
00626         stop();
00627         start(); // Start will do the calibration.
00628       }
00629       else
00630       {
00631         imu.openPort(port.c_str());
00632         doCalibrate();
00633         imu.closePort();
00634       } 
00635     } catch (microstrain_3dmgx2_imu::Exception& e) {
00636       error_count_++;
00637       calibrated_ = false;
00638       publish_is_calibrated();
00639       ROS_ERROR("Exception thrown while calibrating IMU %s", e.what());
00640       stop();
00641       if (old_running)
00642         start(); // Might throw, but we have nothing to lose... Needs restructuring.
00643       return false;
00644     }
00645     
00646     return true;
00647   }
00648 
00649   void doCalibrate()
00650   { // Expects to be called with the IMU stopped.
00651     ROS_INFO("Calibrating IMU gyros.");
00652     imu.initGyros(&bias_x_, &bias_y_, &bias_z_);
00653     
00654     // check calibration
00655     if (!imu.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE_ORIENT)){
00656       ROS_ERROR("Could not start streaming data to verify calibration");
00657     } 
00658     else {
00659       double x_rate = 0.0;
00660       double y_rate = 0.0;
00661       double z_rate = 0.0;
00662       size_t count = 0;
00663       getData(reading);
00664       ros::Time start_time = reading.header.stamp;
00665 
00666       while(reading.header.stamp - start_time < ros::Duration(2.0)){
00667         getData(reading);
00668         x_rate += reading.angular_velocity.x;
00669         y_rate += reading.angular_velocity.y;
00670         z_rate += reading.angular_velocity.z;
00671         ++count;
00672       }
00673 
00674       double average_rate = sqrt(x_rate*x_rate + y_rate*y_rate + z_rate*z_rate) / count;
00675 
00676       if (count < 200){
00677         ROS_WARN("Imu: calibration check acquired fewer than 200 samples.");
00678       }
00679       
00680       // calibration succeeded
00681       if (average_rate < max_drift_rate_) {
00682         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);
00683         calibrated_ = true;
00684         publish_is_calibrated();
00685         ROS_INFO("IMU gyro calibration completed.");
00686         freq_diag_.clear();
00687       }
00688       // calibration failed
00689       else{
00690         calibrated_ = false;
00691         publish_is_calibrated();
00692         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);
00693       }
00694       imu.stopContinuous();
00695     }
00696   }
00697 };
00698 
00699 int
00700 main(int argc, char** argv)
00701 {
00702   ros::init(argc, argv, "microstrain_3dmgx2_node");
00703 
00704   ros::NodeHandle nh;
00705 
00706   ImuNode in(nh);
00707   in.spin();
00708 
00709   return(0);
00710 }


microstrain_3dmgx2_imu
Author(s): Jeremy Leibs, Blaise Gassend
autogenerated on Wed Aug 26 2015 12:32:36