imu.cpp
Go to the documentation of this file.
00001 
00034 #include <multisense_ros/imu.h>
00035 #include <multisense_ros/RawImuData.h>
00036 #include <std_msgs/Time.h>
00037 
00038 using namespace crl::multisense;
00039 
00040 namespace multisense_ros {
00041 
00042 namespace { // anonymous
00043 
00044 //
00045 // Shim for C-style driver callbacks
00046 
00047 void imuCB(const imu::Header& header, void* userDataP)
00048 { reinterpret_cast<Imu*>(userDataP)->imuCallback(header); }
00049 
00050 
00051 }; // anonymous
00052 
00053 Imu::Imu(Channel* driver, std::string tf_prefix) :
00054     driver_(driver),
00055     device_nh_(""),
00056     imu_nh_(device_nh_, "imu"),
00057     accelerometer_pub_(),
00058     gyroscope_pub_(),
00059     magnetometer_pub_(),
00060     imu_pub_(),
00061     accelerometer_vector_pub_(),
00062     gyroscope_vector_pub_(),
00063     magnetometer_vector_pub_(),
00064     imu_message_(),
00065     sub_lock_(),
00066     total_subscribers_(0),
00067     tf_prefix_(tf_prefix),
00068     accel_frameId_(tf_prefix_ + "/accel"),
00069     gyro_frameId_(tf_prefix_ + "/gyro"),
00070     mag_frameId_(tf_prefix_ + "/mag")
00071 {
00072 
00073     //
00074     // Initialize the sensor_msgs::Imu topic
00075     // We will publish the data in the accelerometer frame applying the
00076     // transform from the /gyro to the /accel frame to the gyroscope data
00077 
00078     imu_message_.header.frame_id = accel_frameId_;
00079 
00080     //
00081     // Covariance matrix for linear acceleration and angular velocity were
00082     // generated using 2 minutes of logged data with the default imu
00083     // settings. Note the angular velocity covariance has the nominal gyro
00084     // to accelerometer transform applied to it
00085 
00086     imu_message_.linear_acceleration_covariance[0] = 6.98179077e-04;
00087     imu_message_.linear_acceleration_covariance[1] = 2.46789341e-06;
00088     imu_message_.linear_acceleration_covariance[2] =-2.50549745e-06;
00089     imu_message_.linear_acceleration_covariance[3] = 2.46789341e-06;
00090     imu_message_.linear_acceleration_covariance[4] = 5.02177646e-04;
00091     imu_message_.linear_acceleration_covariance[5] = 5.26265558e-05;
00092     imu_message_.linear_acceleration_covariance[6] =-2.50549745e-06;
00093     imu_message_.linear_acceleration_covariance[7] = 5.26265558e-05;
00094     imu_message_.linear_acceleration_covariance[8] = 9.22796937e-04;
00095 
00096     imu_message_.angular_velocity_covariance[0] = 8.79376936e-06;
00097     imu_message_.angular_velocity_covariance[1] = -3.56007627e-07;
00098     imu_message_.angular_velocity_covariance[2] = 2.22611968e-07;
00099     imu_message_.angular_velocity_covariance[3] = -3.56007627e-07;
00100     imu_message_.angular_velocity_covariance[4] = 8.78939245e-06;
00101     imu_message_.angular_velocity_covariance[5] = -8.08367486e-07;
00102     imu_message_.angular_velocity_covariance[6] = 1.33981577e-05;
00103     imu_message_.angular_velocity_covariance[7] = 2.22611968e-07;
00104     imu_message_.angular_velocity_covariance[8] = -8.08367486e-07;
00105 
00106     //
00107     // Get device info
00108 
00109     system::DeviceInfo  deviceInfo;
00110     Status status = driver_->getDeviceInfo(deviceInfo);
00111     if (Status_Ok != status) {
00112         ROS_ERROR("IMU: failed to query device info: %s",
00113                   Channel::statusString(status));
00114         return;
00115     }
00116 
00117     if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {
00118         ROS_INFO("hardware does not support an IMU");
00119         return;
00120     }
00121 
00122     system::VersionInfo v;
00123     status = driver_->getVersionInfo(v);
00124     if (Status_Ok != status) {
00125         ROS_ERROR("IMU: Unable to query sensor firmware version: %s",
00126                   Channel::statusString(status));
00127         return;
00128     }
00129 
00130 
00131     if (v.sensorFirmwareVersion < 0x0203)
00132         ROS_WARN("IMU support requires sensor firmware v2.3 or greater (sensor is running v%d.%d)",
00133                  v.sensorFirmwareVersion >> 8, v.sensorFirmwareVersion & 0xFF);
00134     else {
00135 
00136         //
00137         // Only publish IMU if we know firmware 2.3 or greater is running.
00138 
00139         driver_->stopStreams(Source_Imu);
00140 
00141         accelerometer_pub_ = imu_nh_.advertise<multisense_ros::RawImuData>("accelerometer", 20,
00142                                                boost::bind(&Imu::startStreams, this),
00143                                                boost::bind(&Imu::stopStreams, this));
00144         gyroscope_pub_     = imu_nh_.advertise<multisense_ros::RawImuData>("gyroscope", 20,
00145                                                boost::bind(&Imu::startStreams, this),
00146                                                boost::bind(&Imu::stopStreams, this));
00147         magnetometer_pub_  = imu_nh_.advertise<multisense_ros::RawImuData>("magnetometer", 20,
00148                                                boost::bind(&Imu::startStreams, this),
00149                                                boost::bind(&Imu::stopStreams, this));
00150         imu_pub_           = imu_nh_.advertise<sensor_msgs::Imu>("imu_data", 20,
00151                                                boost::bind(&Imu::startStreams, this),
00152                                                boost::bind(&Imu::stopStreams, this));
00153 
00154         accelerometer_vector_pub_ = imu_nh_.advertise<geometry_msgs::Vector3Stamped>("accelerometer_vector", 20,
00155                                                       boost::bind(&Imu::startStreams, this),
00156                                                       boost::bind(&Imu::stopStreams, this));
00157         gyroscope_vector_pub_     = imu_nh_.advertise<geometry_msgs::Vector3Stamped>("gyroscope_vector", 20,
00158                                                       boost::bind(&Imu::startStreams, this),
00159                                                       boost::bind(&Imu::stopStreams, this));
00160         magnetometer_vector_pub_  = imu_nh_.advertise<geometry_msgs::Vector3Stamped>("magnetometer_vector", 20,
00161                                                       boost::bind(&Imu::startStreams, this),
00162                                                       boost::bind(&Imu::stopStreams, this));
00163 
00164         driver_->addIsolatedCallback(imuCB, this);
00165     }
00166 }
00167 
00168 Imu::~Imu()
00169 {
00170     driver_->stopStreams(Source_Imu);
00171     driver_->removeIsolatedCallback(imuCB);
00172 }
00173 
00174 void Imu::imuCallback(const imu::Header& header)
00175 {
00176     std::vector<imu::Sample>::const_iterator it = header.samples.begin();
00177 
00178     uint32_t accel_subscribers = accelerometer_pub_.getNumSubscribers();
00179     uint32_t gyro_subscribers = gyroscope_pub_.getNumSubscribers();
00180     uint32_t mag_subscribers = magnetometer_pub_.getNumSubscribers();
00181     uint32_t imu_subscribers = imu_pub_.getNumSubscribers();
00182     uint32_t accel_vector_subscribers = accelerometer_vector_pub_.getNumSubscribers();
00183     uint32_t gyro_vector_subscribers = gyroscope_vector_pub_.getNumSubscribers();
00184     uint32_t mag_vector_subscribers = magnetometer_vector_pub_.getNumSubscribers();
00185 
00186     for(; it != header.samples.end(); ++it) {
00187 
00188         const imu::Sample& s = *it;
00189 
00190         multisense_ros::RawImuData msg;
00191         geometry_msgs::Vector3Stamped vector_msg;
00192 
00193         msg.time_stamp = ros::Time(s.timeSeconds,
00194                                    1000 * s.timeMicroSeconds);
00195         msg.x = s.x;
00196         msg.y = s.y;
00197         msg.z = s.z;
00198 
00199         vector_msg.header.stamp = msg.time_stamp;
00200         vector_msg.vector.x = s.x;
00201         vector_msg.vector.y = s.y;
00202         vector_msg.vector.z = s.z;
00203 
00204         imu_message_.header.stamp = msg.time_stamp;
00205 
00206         switch(s.type) {
00207         case imu::Sample::Type_Accelerometer:
00208             //
00209             // Convert from g to m/s^2
00210 
00211             imu_message_.linear_acceleration.x = s.x * 9.80665;
00212             imu_message_.linear_acceleration.y = s.y * 9.80665;
00213             imu_message_.linear_acceleration.z = s.z * 9.80665;
00214 
00215 
00216             if (accel_subscribers > 0)
00217                 accelerometer_pub_.publish(msg);
00218 
00219             if (imu_subscribers > 0)
00220                 imu_pub_.publish(imu_message_);
00221 
00222             if (accel_vector_subscribers > 0) {
00223                 vector_msg.header.frame_id = accel_frameId_;
00224                 accelerometer_vector_pub_.publish(vector_msg);
00225             }
00226 
00227             break;
00228         case imu::Sample::Type_Gyroscope:
00229 
00230             //
00231             // Convert from deg/sec to rad/sec and apply the nominal
00232             // calibration from the gyro to the accelerometer. Since all points
00233             // on a rigid body have the same angular velocity only the rotation
00234             // about the z axis of 90 degrees needs to be applied. (i.e.
00235             // new_x = orig_y ; new_y = -orig_x)
00236 
00237             imu_message_.angular_velocity.x = s.y * M_PI/180.;
00238             imu_message_.angular_velocity.y = -s.x * M_PI/180.;
00239             imu_message_.angular_velocity.z = s.z * M_PI/180.;
00240 
00241 
00242             if (gyro_subscribers > 0)
00243                 gyroscope_pub_.publish(msg);
00244 
00245             if (imu_subscribers > 0)
00246                 imu_pub_.publish(imu_message_);
00247 
00248             if (gyro_vector_subscribers > 0) {
00249                 vector_msg.header.frame_id = gyro_frameId_;
00250                 gyroscope_vector_pub_.publish(vector_msg);
00251             }
00252 
00253             break;
00254         case imu::Sample::Type_Magnetometer:
00255 
00256 
00257             if (mag_subscribers > 0)
00258                 magnetometer_pub_.publish(msg);
00259 
00260             if (mag_vector_subscribers > 0) {
00261                 vector_msg.header.frame_id = mag_frameId_;
00262                 magnetometer_vector_pub_.publish(vector_msg);
00263             }
00264 
00265             break;
00266         }
00267     }
00268 }
00269 
00270 
00271 void Imu::startStreams()
00272 {
00273     if (0 == total_subscribers_) {
00274         Status status = driver_->startStreams(Source_Imu);
00275         if (Status_Ok != status)
00276             ROS_ERROR("IMU: failed to start streams: %s",
00277                       Channel::statusString(status));
00278     }
00279 
00280     total_subscribers_ = accelerometer_pub_.getNumSubscribers()
00281                        + gyroscope_pub_.getNumSubscribers()
00282                        + magnetometer_pub_.getNumSubscribers()
00283                        + imu_pub_.getNumSubscribers();
00284 }
00285 
00286 void Imu::stopStreams()
00287 {
00288     total_subscribers_ = accelerometer_pub_.getNumSubscribers()
00289                        + gyroscope_pub_.getNumSubscribers()
00290                        + magnetometer_pub_.getNumSubscribers()
00291                        + imu_pub_.getNumSubscribers();
00292 
00293     if (total_subscribers_ <= 0){
00294         Status status = driver_->stopStreams(Source_Imu);
00295         if (Status_Ok != status)
00296             ROS_ERROR("IMU: failed to stop streams: %s",
00297                       Channel::statusString(status));
00298     }
00299 }
00300 
00301 } // namespace


multisense_ros
Author(s):
autogenerated on Thu Aug 27 2015 14:01:22