imu.cpp
Go to the documentation of this file.
1 
34 #include <multisense_ros/imu.h>
35 #include <multisense_ros/RawImuData.h>
36 #include <std_msgs/Time.h>
37 
38 using namespace crl::multisense;
39 
40 namespace multisense_ros {
41 
42 namespace { // anonymous
43 
44 //
45 // Shim for C-style driver callbacks
46 
47 void imuCB(const imu::Header& header, void* userDataP)
48 { reinterpret_cast<Imu*>(userDataP)->imuCallback(header); }
49 
50 
51 }; // anonymous
52 
53 Imu::Imu(Channel* driver, std::string tf_prefix) :
54  driver_(driver),
55  device_nh_(""),
56  imu_nh_(device_nh_, "imu"),
57  accelerometer_pub_(),
58  gyroscope_pub_(),
59  magnetometer_pub_(),
60  imu_pub_(),
61  accelerometer_vector_pub_(),
62  gyroscope_vector_pub_(),
63  magnetometer_vector_pub_(),
64  imu_message_(),
65  sub_lock_(),
66  total_subscribers_(0),
67  tf_prefix_(tf_prefix),
68  accel_frameId_(tf_prefix_ + "/accel"),
69  gyro_frameId_(tf_prefix_ + "/gyro"),
70  mag_frameId_(tf_prefix_ + "/mag")
71 {
72 
73  //
74  // Initialize the sensor_msgs::Imu topic
75  // We will publish the data in the accelerometer frame applying the
76  // transform from the /gyro to the /accel frame to the gyroscope data
77 
78  imu_message_.header.frame_id = accel_frameId_;
79 
80  //
81  // Covariance matrix for linear acceleration and angular velocity were
82  // generated using 2 minutes of logged data with the default imu
83  // settings. Note the angular velocity covariance has the nominal gyro
84  // to accelerometer transform applied to it
85 
86  imu_message_.linear_acceleration_covariance[0] = 6.98179077e-04;
87  imu_message_.linear_acceleration_covariance[1] = 2.46789341e-06;
88  imu_message_.linear_acceleration_covariance[2] =-2.50549745e-06;
89  imu_message_.linear_acceleration_covariance[3] = 2.46789341e-06;
90  imu_message_.linear_acceleration_covariance[4] = 5.02177646e-04;
91  imu_message_.linear_acceleration_covariance[5] = 5.26265558e-05;
92  imu_message_.linear_acceleration_covariance[6] =-2.50549745e-06;
93  imu_message_.linear_acceleration_covariance[7] = 5.26265558e-05;
94  imu_message_.linear_acceleration_covariance[8] = 9.22796937e-04;
95 
96  imu_message_.angular_velocity_covariance[0] = 8.79376936e-06;
97  imu_message_.angular_velocity_covariance[1] = -3.56007627e-07;
98  imu_message_.angular_velocity_covariance[2] = 2.22611968e-07;
99  imu_message_.angular_velocity_covariance[3] = -3.56007627e-07;
100  imu_message_.angular_velocity_covariance[4] = 8.78939245e-06;
101  imu_message_.angular_velocity_covariance[5] = -8.08367486e-07;
102  imu_message_.angular_velocity_covariance[6] = 1.33981577e-05;
103  imu_message_.angular_velocity_covariance[7] = 2.22611968e-07;
104  imu_message_.angular_velocity_covariance[8] = -8.08367486e-07;
105 
106  //
107  // Get device info
108 
109  system::DeviceInfo deviceInfo;
110  Status status = driver_->getDeviceInfo(deviceInfo);
111  if (Status_Ok != status) {
112  ROS_ERROR("IMU: failed to query device info: %s",
113  Channel::statusString(status));
114  return;
115  }
116 
117  if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {
118  ROS_INFO("hardware does not support an IMU");
119  return;
120  }
121 
123  status = driver_->getVersionInfo(v);
124  if (Status_Ok != status) {
125  ROS_ERROR("IMU: Unable to query sensor firmware version: %s",
126  Channel::statusString(status));
127  return;
128  }
129 
130 
131  if (v.sensorFirmwareVersion < 0x0203)
132  ROS_WARN("IMU support requires sensor firmware v2.3 or greater (sensor is running v%d.%d)",
134  else {
135 
136  //
137  // Only publish IMU if we know firmware 2.3 or greater is running.
138 
139  driver_->stopStreams(Source_Imu);
140 
141  accelerometer_pub_ = imu_nh_.advertise<multisense_ros::RawImuData>("accelerometer", 20,
142  boost::bind(&Imu::startStreams, this),
143  boost::bind(&Imu::stopStreams, this));
144  gyroscope_pub_ = imu_nh_.advertise<multisense_ros::RawImuData>("gyroscope", 20,
145  boost::bind(&Imu::startStreams, this),
146  boost::bind(&Imu::stopStreams, this));
147  magnetometer_pub_ = imu_nh_.advertise<multisense_ros::RawImuData>("magnetometer", 20,
148  boost::bind(&Imu::startStreams, this),
149  boost::bind(&Imu::stopStreams, this));
150  imu_pub_ = imu_nh_.advertise<sensor_msgs::Imu>("imu_data", 20,
151  boost::bind(&Imu::startStreams, this),
152  boost::bind(&Imu::stopStreams, this));
153 
154  accelerometer_vector_pub_ = imu_nh_.advertise<geometry_msgs::Vector3Stamped>("accelerometer_vector", 20,
155  boost::bind(&Imu::startStreams, this),
156  boost::bind(&Imu::stopStreams, this));
157  gyroscope_vector_pub_ = imu_nh_.advertise<geometry_msgs::Vector3Stamped>("gyroscope_vector", 20,
158  boost::bind(&Imu::startStreams, this),
159  boost::bind(&Imu::stopStreams, this));
160  magnetometer_vector_pub_ = imu_nh_.advertise<geometry_msgs::Vector3Stamped>("magnetometer_vector", 20,
161  boost::bind(&Imu::startStreams, this),
162  boost::bind(&Imu::stopStreams, this));
163 
164  driver_->addIsolatedCallback(imuCB, this);
165  }
166 }
167 
169 {
170  driver_->stopStreams(Source_Imu);
172 }
173 
174 void Imu::imuCallback(const imu::Header& header)
175 {
176  std::vector<imu::Sample>::const_iterator it = header.samples.begin();
177 
178  uint32_t accel_subscribers = accelerometer_pub_.getNumSubscribers();
179  uint32_t gyro_subscribers = gyroscope_pub_.getNumSubscribers();
180  uint32_t mag_subscribers = magnetometer_pub_.getNumSubscribers();
181  uint32_t imu_subscribers = imu_pub_.getNumSubscribers();
182  uint32_t accel_vector_subscribers = accelerometer_vector_pub_.getNumSubscribers();
183  uint32_t gyro_vector_subscribers = gyroscope_vector_pub_.getNumSubscribers();
184  uint32_t mag_vector_subscribers = magnetometer_vector_pub_.getNumSubscribers();
185 
186  for(; it != header.samples.end(); ++it) {
187 
188  const imu::Sample& s = *it;
189 
190  multisense_ros::RawImuData msg;
191  geometry_msgs::Vector3Stamped vector_msg;
192 
193  msg.time_stamp = ros::Time(s.timeSeconds,
194  1000 * s.timeMicroSeconds);
195  msg.x = s.x;
196  msg.y = s.y;
197  msg.z = s.z;
198 
199  vector_msg.header.stamp = msg.time_stamp;
200  vector_msg.vector.x = s.x;
201  vector_msg.vector.y = s.y;
202  vector_msg.vector.z = s.z;
203 
204  imu_message_.header.stamp = msg.time_stamp;
205 
206  switch(s.type) {
207  case imu::Sample::Type_Accelerometer:
208  //
209  // Convert from g to m/s^2
210 
211  imu_message_.linear_acceleration.x = s.x * 9.80665;
212  imu_message_.linear_acceleration.y = s.y * 9.80665;
213  imu_message_.linear_acceleration.z = s.z * 9.80665;
214 
215 
216  if (accel_subscribers > 0)
218 
219  if (imu_subscribers > 0)
221 
222  if (accel_vector_subscribers > 0) {
223  vector_msg.header.frame_id = accel_frameId_;
225  }
226 
227  break;
228  case imu::Sample::Type_Gyroscope:
229 
230  //
231  // Convert from deg/sec to rad/sec and apply the nominal
232  // calibration from the gyro to the accelerometer. Since all points
233  // on a rigid body have the same angular velocity only the rotation
234  // about the z axis of 90 degrees needs to be applied. (i.e.
235  // new_x = orig_y ; new_y = -orig_x)
236 
237  imu_message_.angular_velocity.x = s.y * M_PI/180.;
238  imu_message_.angular_velocity.y = -s.x * M_PI/180.;
239  imu_message_.angular_velocity.z = s.z * M_PI/180.;
240 
241 
242  if (gyro_subscribers > 0)
243  gyroscope_pub_.publish(msg);
244 
245  if (imu_subscribers > 0)
247 
248  if (gyro_vector_subscribers > 0) {
249  vector_msg.header.frame_id = gyro_frameId_;
250  gyroscope_vector_pub_.publish(vector_msg);
251  }
252 
253  break;
254  case imu::Sample::Type_Magnetometer:
255 
256 
257  if (mag_subscribers > 0)
259 
260  if (mag_vector_subscribers > 0) {
261  vector_msg.header.frame_id = mag_frameId_;
262  magnetometer_vector_pub_.publish(vector_msg);
263  }
264 
265  break;
266  }
267  }
268 }
269 
270 
272 {
273  if (0 == total_subscribers_) {
274  Status status = driver_->startStreams(Source_Imu);
275  if (Status_Ok != status)
276  ROS_ERROR("IMU: failed to start streams: %s",
277  Channel::statusString(status));
278  }
279 
284 }
285 
287 {
292 
293  if (total_subscribers_ <= 0){
294  Status status = driver_->stopStreams(Source_Imu);
295  if (Status_Ok != status)
296  ROS_ERROR("IMU: failed to stop streams: %s",
297  Channel::statusString(status));
298  }
299 }
300 
301 } // namespace
const std::string mag_frameId_
Definition: imu.h:103
virtual Status startStreams(DataSource mask)=0
ros::Publisher magnetometer_vector_pub_
Definition: imu.h:84
void imuCallback(const crl::multisense::imu::Header &header)
Definition: imu.cpp:174
const std::string accel_frameId_
Definition: imu.h:101
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher imu_pub_
Definition: imu.h:78
std::vector< Sample > samples
sensor_msgs::Imu imu_message_
Definition: imu.h:88
int32_t total_subscribers_
Definition: imu.h:94
virtual Status stopStreams(DataSource mask)=0
XmlRpcServer s
ros::Publisher gyroscope_pub_
Definition: imu.h:73
#define ROS_WARN(...)
virtual Status getVersionInfo(system::VersionInfo &v)=0
ros::Publisher magnetometer_pub_
Definition: imu.h:74
void stopStreams()
Definition: imu.cpp:286
virtual Status removeIsolatedCallback(image::Callback callback)=0
virtual Status addIsolatedCallback(image::Callback callback, DataSource imageSourceMask, void *userDataP=NULL)=0
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher accelerometer_vector_pub_
Definition: imu.h:82
void startStreams()
Definition: imu.cpp:271
const std::string gyro_frameId_
Definition: imu.h:102
ros::Publisher gyroscope_vector_pub_
Definition: imu.h:83
uint32_t getNumSubscribers() const
ros::Publisher accelerometer_pub_
Definition: imu.h:72
crl::multisense::Channel * driver_
Definition: imu.h:61
virtual Status getDeviceInfo(system::DeviceInfo &info)=0
#define ROS_ERROR(...)
ros::NodeHandle imu_nh_
Definition: imu.h:67


multisense_ros
Author(s):
autogenerated on Wed Jan 8 2020 03:37:59