imu_ros_i.cpp
Go to the documentation of this file.
00001 #include "phidgets_imu/imu_ros_i.h"
00002 
00003 namespace phidgets {
00004 
00005 ImuRosI::ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private):
00006   Imu(),
00007   nh_(nh), 
00008   nh_private_(nh_private),
00009   initialized_(false)
00010 {
00011   ROS_INFO ("Starting Phidgets IMU");
00012 
00013   // **** get parameters
00014 
00015   if (!nh_private_.getParam ("period", period_))
00016     period_ = 8; // 8 ms
00017   if (!nh_private_.getParam ("frame_id", frame_id_))
00018     frame_id_ = "imu";
00019   if (!nh_private_.getParam ("angular_velocity_stdev", angular_velocity_stdev_))
00020     angular_velocity_stdev_ = 0.02 * (M_PI / 180.0); // 0.02 deg/s resolution, as per manual
00021   if (!nh_private_.getParam ("linear_acceleration_stdev", linear_acceleration_stdev_))
00022     linear_acceleration_stdev_ = 300.0 * 1e-6 * G; // 300 ug as per manual
00023 
00024   // **** advertise topics
00025 
00026   imu_publisher_ = nh_.advertise<ImuMsg>(
00027     "imu/data_raw", 5);
00028   mag_publisher_ = nh_.advertise<MagMsg>(
00029     "imu/mag", 5);
00030   cal_publisher_ = nh_.advertise<std_msgs::Bool>(
00031     "imu/is_calibrated", 5);
00032 
00033   // **** advertise services
00034 
00035   cal_srv_ = nh_.advertiseService(
00036     "imu/calibrate", &ImuRosI::calibrateService, this);
00037 
00038   // **** initialize variables and device
00039   
00040   imu_msg_.header.frame_id = frame_id_;
00041 
00042   // build covariance matrices
00043 
00044   double ang_vel_var = angular_velocity_stdev_ * angular_velocity_stdev_;
00045   double lin_acc_var = linear_acceleration_stdev_ * linear_acceleration_stdev_;
00046 
00047   for (int i = 0; i < 3; ++i)
00048   for (int j = 0; j < 3; ++j)
00049   {
00050     int idx = j*3 +i;
00051 
00052     if (i == j)
00053     {
00054       imu_msg_.angular_velocity_covariance[idx]    = ang_vel_var;
00055       imu_msg_.linear_acceleration_covariance[idx] = lin_acc_var;
00056     }
00057     else
00058     {
00059       imu_msg_.angular_velocity_covariance[idx]    = 0.0;
00060       imu_msg_.linear_acceleration_covariance[idx] = 0.0;
00061     }
00062   }
00063 
00064   initDevice();
00065 }
00066 
00067 void ImuRosI::initDevice()
00068 {
00069         ROS_INFO("Opening device");
00070         open(-1);
00071 
00072         ROS_INFO("Waiting for IMU to be attached...");
00073         int result = waitForAttachment(10000);
00074         if(result)
00075         {
00076           const char *err;
00077                 CPhidget_getErrorDescription(result, &err);
00078                 ROS_FATAL("Problem waiting for IMU attachment: %s Make sure the USB cable is connected and you have executed the phidgets_c_api/setup-udev.sh script.", err);
00079         }
00080 
00081         // set the data rate for the spatial events
00082   setDataRate(period_);
00083 
00084   // calibrate on startup
00085   calibrate();
00086 }
00087 
00088 bool ImuRosI::calibrateService(std_srvs::Empty::Request  &req,
00089                                std_srvs::Empty::Response &res)
00090 {
00091   calibrate();
00092   return true;
00093 }
00094 
00095 void ImuRosI::calibrate()
00096 {
00097   ROS_INFO("Calibrating IMU...");
00098   zero();
00099   ROS_INFO("Calibrating IMU done.");
00100 
00101   time_zero_ = ros::Time::now();
00102 
00103   // publish message
00104   std_msgs::Bool is_calibrated_msg;
00105   is_calibrated_msg.data = true;
00106   cal_publisher_.publish(is_calibrated_msg);
00107 }
00108 
00109 void ImuRosI::processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i)
00110 {
00111   // **** calculate time from timestamp
00112   ros::Duration time_imu(data[i]->timestamp.seconds + 
00113                          data[i]->timestamp.microseconds * 1e-6);
00114 
00115   ros::Time time_now = time_zero_ + time_imu;
00116 
00117   double timediff = time_now.toSec() - ros::Time::now().toSec();
00118   if (fabs(timediff) > 0.1) {
00119     ROS_WARN("IMU time lags behind by %f seconds, resetting IMU time offset!", timediff);
00120     time_zero_ = ros::Time::now() - time_imu;
00121     time_now = ros::Time::now();
00122   }
00123 
00124   // **** initialize if needed
00125 
00126   if (!initialized_)
00127   { 
00128     last_imu_time_ = time_now;
00129     initialized_ = true;
00130   }
00131 
00132   // **** create and publish imu message
00133 
00134   boost::shared_ptr<ImuMsg> imu_msg = 
00135     boost::make_shared<ImuMsg>(imu_msg_);
00136 
00137   imu_msg->header.stamp = time_now;
00138 
00139   // set linear acceleration
00140   imu_msg->linear_acceleration.x = - data[i]->acceleration[0] * G;
00141   imu_msg->linear_acceleration.y = - data[i]->acceleration[1] * G;
00142   imu_msg->linear_acceleration.z = - data[i]->acceleration[2] * G;
00143 
00144   // set angular velocities
00145   imu_msg->angular_velocity.x = data[i]->angularRate[0] * (M_PI / 180.0);
00146   imu_msg->angular_velocity.y = data[i]->angularRate[1] * (M_PI / 180.0);
00147   imu_msg->angular_velocity.z = data[i]->angularRate[2] * (M_PI / 180.0);
00148 
00149   imu_publisher_.publish(imu_msg);
00150 
00151   // **** create and publish magnetic field message
00152 
00153   boost::shared_ptr<MagMsg> mag_msg = 
00154     boost::make_shared<MagMsg>();
00155   
00156   mag_msg->header.frame_id = frame_id_;
00157   mag_msg->header.stamp = time_now;
00158 
00159   if (data[i]->magneticField[0] != PUNK_DBL)
00160   {
00161     mag_msg->vector.x = data[i]->magneticField[0];
00162     mag_msg->vector.y = data[i]->magneticField[1];
00163     mag_msg->vector.z = data[i]->magneticField[2];
00164   }
00165   else
00166   {
00167     double nan = std::numeric_limits<double>::quiet_NaN();
00168 
00169     mag_msg->vector.x = nan;
00170     mag_msg->vector.y = nan;
00171     mag_msg->vector.z = nan;
00172   }
00173    
00174   mag_publisher_.publish(mag_msg);
00175 }
00176 
00177 void ImuRosI::dataHandler(CPhidgetSpatial_SpatialEventDataHandle *data, int count)
00178 {
00179   for(int i = 0; i < count; i++)
00180     processImuData(data, i);
00181 }
00182 
00183 } // namespace phidgets
00184 


phidgets_imu
Author(s): Ivan Dryanovski
autogenerated on Sat Dec 28 2013 17:19:33