imu_node.cc
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Microstrain 3DM-GX2 node
5  * Copyright (c) 2008-2010, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 #include <assert.h>
37 #include <math.h>
38 #include <iostream>
39 
40 #include <boost/format.hpp>
41 
43 
44 #include "ros/time.h"
45 #include "self_test/self_test.h"
46 #include "diagnostic_msgs/DiagnosticStatus.h"
50 
51 #include "sensor_msgs/Imu.h"
52 #include "std_srvs/Empty.h"
53 
54 #include "tf/transform_datatypes.h"
55 #include "microstrain_3dmgx2_imu/AddOffset.h"
56 
57 #include "std_msgs/Bool.h"
58 
59 using namespace std;
60 
61 class ImuNode
62 {
63 public:
65  sensor_msgs::Imu reading;
66 
67  string port;
68 
70 
73 
80 
81  bool running;
82 
86 
89  std::string was_slow_;
90  std::string error_status_;
91 
92  string frameid_;
93 
94  double offset_;
95 
96  double bias_x_;
97  double bias_y_;
98  double bias_z_;
99 
100  double angular_velocity_stdev_, angular_velocity_covariance_;
101  double linear_acceleration_covariance_, linear_acceleration_stdev_;
102  double orientation_covariance_, orientation_stdev_;
103 
105 
108 
109  ImuNode(ros::NodeHandle h) : self_test_(), diagnostic_(),
110  node_handle_(h), private_node_handle_("~"), calibrate_requested_(false),
111  error_count_(0),
112  slow_count_(0),
113  desired_freq_(100),
114  freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05))
115  {
116  ros::NodeHandle imu_node_handle(node_handle_, "imu");
117 
118  private_node_handle_.param("autocalibrate", autocalibrate_, true);
119  private_node_handle_.param("assume_calibrated", calibrated_, false);
120  private_node_handle_.param("port", port, string("/dev/ttyUSB0"));
121  private_node_handle_.param("max_drift_rate", max_drift_rate_, 0.0002);
122 
123 
124  imu_data_pub_ = imu_node_handle.advertise<sensor_msgs::Imu>("data", 100);
125  add_offset_serv_ = private_node_handle_.advertiseService("add_offset", &ImuNode::addOffset, this);
126  calibrate_serv_ = imu_node_handle.advertiseService("calibrate", &ImuNode::calibrate, this);
127  is_calibrated_pub_ = imu_node_handle.advertise<std_msgs::Bool>("is_calibrated", 1, true);
128 
129  publish_is_calibrated();
130 
132 
133  running = false;
134 
135  bias_x_ = bias_y_ = bias_z_ = 0;
136 
137  private_node_handle_.param("frame_id", frameid_, string("imu"));
138  reading.header.frame_id = frameid_;
139 
140  private_node_handle_.param("time_offset", offset_, 0.0);
141 
142  private_node_handle_.param("linear_acceleration_stdev", linear_acceleration_stdev_, 0.098);
143  private_node_handle_.param("orientation_stdev", orientation_stdev_, 0.035);
144  private_node_handle_.param("angular_velocity_stdev", angular_velocity_stdev_, 0.012);
145 
146  double angular_velocity_covariance = angular_velocity_stdev_ * angular_velocity_stdev_;
147  double orientation_covariance = orientation_stdev_ * orientation_stdev_;
148  double linear_acceleration_covariance = linear_acceleration_stdev_ * linear_acceleration_stdev_;
149 
150  reading.linear_acceleration_covariance[0] = linear_acceleration_covariance;
151  reading.linear_acceleration_covariance[4] = linear_acceleration_covariance;
152  reading.linear_acceleration_covariance[8] = linear_acceleration_covariance;
153 
154  reading.angular_velocity_covariance[0] = angular_velocity_covariance;
155  reading.angular_velocity_covariance[4] = angular_velocity_covariance;
156  reading.angular_velocity_covariance[8] = angular_velocity_covariance;
157 
158  reading.orientation_covariance[0] = orientation_covariance;
159  reading.orientation_covariance[4] = orientation_covariance;
160  reading.orientation_covariance[8] = orientation_covariance;
161 
162  self_test_.add("Close Test", this, &ImuNode::pretest);
163  self_test_.add("Interruption Test", this, &ImuNode::InterruptionTest);
164  self_test_.add("Connect Test", this, &ImuNode::ConnectTest);
165  self_test_.add("Read ID Test", this, &ImuNode::ReadIDTest);
166  self_test_.add("Gyro Bias Test", this, &ImuNode::GyroBiasTest);
167  self_test_.add("Streamed Data Test", this, &ImuNode::StreamedDataTest);
168  self_test_.add("Gravity Test", this, &ImuNode::GravityTest);
169  self_test_.add("Disconnect Test", this, &ImuNode::DisconnectTest);
170  self_test_.add("Resume Test", this, &ImuNode::ResumeTest);
171 
172  diagnostic_.add( freq_diag_ );
173  diagnostic_.add( "Calibration Status", this, &ImuNode::calibrationStatus );
174  diagnostic_.add( "IMU Status", this, &ImuNode::deviceStatus );
175  }
176 
178  {
179  stop();
180  }
181 
182  void setErrorStatusf(const char *format, ...)
183  {
184  va_list va;
185  char buff[1000];
186  va_start(va, format);
187  if (vsnprintf(buff, 1000, format, va) >= 1000)
188  ROS_DEBUG("Really long string in setErrorStatus, it was truncated.");
189  std::string value = std::string(buff);
190  setErrorStatus(buff);
191  }
192 
193  // Prints an error message if it isn't the same old error message.
194  void setErrorStatus(const std::string msg)
195  {
196  if (error_status_ != msg)
197  {
198  error_status_ = msg;
199  ROS_ERROR("%s You may find further details at http://www.ros.org/wiki/microstrain_3dmgx2_imu/Troubleshooting", msg.c_str());
200  }
201  else
202  {
203  ROS_DEBUG("%s You may find further details at http://www.ros.org/wiki/microstrain_3dmgx2_imu/Troubleshooting", msg.c_str());
204  }
205  }
206 
208  {
209  error_status_.clear();
210  }
211 
212  int start()
213  {
214  stop();
215 
216  try
217  {
218  try
219  {
220  imu.openPort(port.c_str());
221  } catch (microstrain_3dmgx2_imu::Exception& e) {
222  error_count_++;
223  setErrorStatus(e.what());
224  diagnostic_.broadcast(2, e.what());
225  return -1;
226  }
227 
228  diagnostic_.setHardwareID(getID(true));
229 
230  ROS_INFO("Initializing IMU time with offset %f.", offset_);
231 
232  imu.initTime(offset_);
233 
234  if (autocalibrate_ || calibrate_requested_)
235  {
236  doCalibrate();
237  calibrate_requested_ = false;
238  autocalibrate_ = false; // No need to do this each time we reopen the device.
239  }
240  else
241  {
242  ROS_INFO("Not calibrating the IMU sensor. Use the calibrate service to calibrate it before use.");
243  }
244 
245  ROS_INFO("IMU sensor initialized.");
246 
247  imu.setContinuous(cmd);
248 
249  freq_diag_.clear();
250 
251  running = true;
252 
253  } catch (microstrain_3dmgx2_imu::Exception& e) {
254  error_count_++;
255  usleep(100000); // Give isShuttingDown a chance to go true.
256  if (!ros::isShuttingDown()){ // Don't warn if we are shutting down.
257  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());
258  diagnostic_.broadcast(2, "Error opening IMU.");
259  }
260  return -1;
261  }
262 
263  return(0);
264  }
265 
266  std::string getID(bool output_info = false)
267  {
268  char dev_name[17];
269  char dev_model_num[17];
270  char dev_serial_num[17];
271  char dev_opt[17];
276 
277  if (output_info)
278  ROS_INFO("Connected to IMU [%s] model [%s] s/n [%s] options [%s]",
279  dev_name, dev_model_num, dev_serial_num, dev_opt);
280 
281  char *dev_name_ptr = dev_name;
282  char *dev_model_num_ptr = dev_model_num;
283  char *dev_serial_num_ptr = dev_serial_num;
284 
285  while (*dev_name_ptr == ' ')
286  dev_name_ptr++;
287  while (*dev_model_num_ptr == ' ')
288  dev_model_num_ptr++;
289  while (*dev_serial_num_ptr == ' ')
290  dev_serial_num_ptr++;
291 
292  return (boost::format("%s_%s-%s")%dev_name_ptr%dev_model_num_ptr%dev_serial_num_ptr).str();
293  }
294 
295  int stop()
296  {
297  if(running)
298  {
299  try
300  {
301  imu.closePort();
302  } catch (microstrain_3dmgx2_imu::Exception& e) {
303  error_count_++;
304  ROS_INFO("Exception thrown while stopping IMU. %s", e.what());
305  }
306  running = false;
307  }
308 
309  return(0);
310  }
311 
313  {
314  try
315  {
316  static double prevtime = 0;
317  double starttime = ros::Time::now().toSec();
318  if (prevtime && prevtime - starttime > 0.05)
319  {
320  ROS_WARN("Full IMU loop took %f ms. Nominal is 10ms.", 1000 * (prevtime - starttime));
321  was_slow_ = "Full IMU loop was slow.";
322  slow_count_++;
323  }
324  getData(reading);
325  double endtime = ros::Time::now().toSec();
326  if (endtime - starttime > 0.05)
327  {
328  ROS_WARN("Gathering data took %f ms. Nominal is 10ms.", 1000 * (endtime - starttime));
329  was_slow_ = "Full IMU loop was slow.";
330  slow_count_++;
331  }
332  prevtime = starttime;
333  starttime = ros::Time::now().toSec();
334  imu_data_pub_.publish(reading);
335  endtime = ros::Time::now().toSec();
336  if (endtime - starttime > 0.05)
337  {
338  ROS_WARN("Publishing took %f ms. Nominal is 10 ms.", 1000 * (endtime - starttime));
339  was_slow_ = "Full IMU loop was slow.";
340  slow_count_++;
341  }
342 
343  freq_diag_.tick();
344  clearErrorStatus(); // If we got here, then the IMU really is working. Next time an error occurs, we want to print it.
345  } catch (microstrain_3dmgx2_imu::Exception& e) {
346  error_count_++;
347  usleep(100000); // Give isShuttingDown a chance to go true.
348  if (!ros::isShuttingDown()) // Don't warn if we are shutting down.
349  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());
350  return -1;
351  }
352 
353  return(0);
354  }
355 
356  bool spin()
357  {
358  while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
359  {
360  if (start() == 0)
361  {
362  while(node_handle_.ok()) {
363  if(publish_datum() < 0)
364  break;
365  self_test_.checkTest();
366  diagnostic_.update();
367  ros::spinOnce();
368  }
369  } else {
370  // No need for diagnostic here since a broadcast occurs in start
371  // when there is an error.
372  usleep(1000000);
373  self_test_.checkTest();
374  ros::spinOnce();
375  }
376  }
377 
378  stop();
379 
380  return true;
381  }
382 
384  {
385  std_msgs::Bool msg;
386  msg.data = calibrated_;
387  is_calibrated_pub_.publish(msg);
388  }
389 
391  {
392  try
393  {
394  imu.closePort();
395 
396  status.summary(0, "Device closed successfully.");
397  } catch (microstrain_3dmgx2_imu::Exception& e) {
398  status.summary(1, "Failed to close device.");
399  }
400  }
401 
403  {
404  if (imu_data_pub_.getNumSubscribers() == 0 )
405  status.summary(0, "No operation interrupted.");
406  else
407  status.summary(1, "There were active subscribers. Running of self test interrupted operations.");
408  }
409 
411  {
412  imu.openPort(port.c_str());
413 
414  status.summary(0, "Connected successfully.");
415  }
416 
418  {
419  self_test_.setID(getID());
420 
421  status.summary(0, "Read Successfully");
422  }
423 
425  {
426  imu.initGyros(&bias_x_, &bias_y_, &bias_z_);
427 
428  status.summary(0, "Successfully calculated gyro biases.");
429 
430  status.add("Bias_X", bias_x_);
431  status.add("Bias_Y", bias_y_);
432  status.add("Bias_Z", bias_z_);
433  }
434 
435 
436  void getData(sensor_msgs::Imu& data)
437  {
438  uint64_t time;
439  double accel[3];
440  double angrate[3];
441  double orientation[9];
442 
443  imu.receiveAccelAngrateOrientation(&time, accel, angrate, orientation);
444  data.linear_acceleration.x = accel[0];
445  data.linear_acceleration.y = accel[1];
446  data.linear_acceleration.z = accel[2];
447 
448  data.angular_velocity.x = angrate[0];
449  data.angular_velocity.y = angrate[1];
450  data.angular_velocity.z = angrate[2];
451 
452  tf::Quaternion quat;
453  (tf::Matrix3x3(-1,0,0,
454  0,1,0,
455  0,0,-1)*
456  tf::Matrix3x3(orientation[0], orientation[3], orientation[6],
457  orientation[1], orientation[4], orientation[7],
458  orientation[2], orientation[5], orientation[8])).getRotation(quat);
459 
460  tf::quaternionTFToMsg(quat, data.orientation);
461 
462  data.header.stamp = ros::Time::now().fromNSec(time);
463  }
464 
465 
467  {
468  uint64_t time;
469  double accel[3];
470  double angrate[3];
471 
473  {
474  status.summary(2, "Could not start streaming data.");
475  } else {
476 
477  for (int i = 0; i < 100; i++)
478  {
479  imu.receiveAccelAngrate(&time, accel, angrate);
480  }
481 
482  imu.stopContinuous();
483 
484  status.summary(0, "Data streamed successfully.");
485  }
486  }
487 
489  {
490  uint64_t time;
491  double accel[3];
492  double angrate[3];
493 
494  double grav = 0.0;
495 
496  double grav_x = 0.0;
497  double grav_y = 0.0;
498  double grav_z = 0.0;
499 
501  {
502  status.summary(2, "Could not start streaming data.");
503  } else {
504 
505  int num = 200;
506 
507  for (int i = 0; i < num; i++)
508  {
509  imu.receiveAccelAngrate(&time, accel, angrate);
510 
511  grav_x += accel[0];
512  grav_y += accel[1];
513  grav_z += accel[2];
514 
515  }
516 
517  imu.stopContinuous();
518 
519  grav += sqrt( pow(grav_x / (double)(num), 2.0) +
520  pow(grav_y / (double)(num), 2.0) +
521  pow(grav_z / (double)(num), 2.0));
522 
523  // double err = (grav - microstrain_3dmgx2_imu::G);
524  double err = (grav - 9.796);
525 
526  if (fabs(err) < .05)
527  {
528  status.summary(0, "Gravity detected correctly.");
529  } else {
530  status.summaryf(2, "Measured gravity deviates by %f", err);
531  }
532 
533  status.add("Measured gravity", grav);
534  status.add("Gravity error", err);
535  }
536  }
537 
539  {
540  imu.closePort();
541 
542  status.summary(0, "Disconnected successfully.");
543  }
544 
546  {
547  if (running)
548  {
549 
550  imu.openPort(port.c_str());
551  freq_diag_.clear();
552 
553  if (imu.setContinuous(cmd) != true)
554  {
555  status.summary(2, "Failed to resume previous mode of operation.");
556  return;
557  }
558  }
559 
560  status.summary(0, "Previous operation resumed successfully.");
561  }
562 
564  {
565  if (!running)
566  status.summary(2, "IMU is stopped");
567  else if (!was_slow_.empty())
568  {
569  status.summary(1, "Excessive delay");
570  was_slow_.clear();
571  }
572  else
573  status.summary(0, "IMU is running");
574 
575  status.add("Device", port);
576  status.add("TF frame", frameid_);
577  status.add("Error count", error_count_);
578  status.add("Excessive delay", slow_count_);
579  }
580 
582  {
583  if (calibrated_)
584  {
585  status.summary(0, "Gyro is calibrated");
586  status.add("X bias", bias_x_);
587  status.add("Y bias", bias_y_);
588  status.add("Z bias", bias_z_);
589  }
590  else
591  status.summary(2, "Gyro not calibrated");
592  // Note to Kevin. Yes the IMU is running, but it is outputting
593  // garbage, so this is an error.
594 
595  }
596 
597  bool addOffset(microstrain_3dmgx2_imu::AddOffset::Request &req, microstrain_3dmgx2_imu::AddOffset::Response &resp)
598  {
599  double offset = req.add_offset;
600  offset_ += offset;
601 
602  ROS_INFO("Adding %f to existing IMU time offset.", offset);
603  ROS_INFO("Total IMU time offset is now %f.", offset_);
604 
605  // send changes to imu driver
606  imu.setFixedOffset(offset_);
607 
608  // write changes to param server
609  private_node_handle_.setParam("time_offset", offset_);
610 
611  // set response
612  resp.total_offset = offset_;
613 
614  return true;
615  }
616 
617  bool calibrate(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
618  {
619  bool old_running = running;
620 
621  try
622  {
623  calibrate_requested_ = true;
624  if (old_running)
625  {
626  stop();
627  start(); // Start will do the calibration.
628  }
629  else
630  {
631  imu.openPort(port.c_str());
632  doCalibrate();
633  imu.closePort();
634  }
635  } catch (microstrain_3dmgx2_imu::Exception& e) {
636  error_count_++;
637  calibrated_ = false;
638  publish_is_calibrated();
639  ROS_ERROR("Exception thrown while calibrating IMU %s", e.what());
640  stop();
641  if (old_running)
642  start(); // Might throw, but we have nothing to lose... Needs restructuring.
643  return false;
644  }
645 
646  return true;
647  }
648 
649  void doCalibrate()
650  { // Expects to be called with the IMU stopped.
651  ROS_INFO("Calibrating IMU gyros.");
652  imu.initGyros(&bias_x_, &bias_y_, &bias_z_);
653 
654  // check calibration
656  ROS_ERROR("Could not start streaming data to verify calibration");
657  }
658  else {
659  double x_rate = 0.0;
660  double y_rate = 0.0;
661  double z_rate = 0.0;
662  size_t count = 0;
663  getData(reading);
664  ros::Time start_time = reading.header.stamp;
665 
666  while(reading.header.stamp - start_time < ros::Duration(2.0)){
667  getData(reading);
668  x_rate += reading.angular_velocity.x;
669  y_rate += reading.angular_velocity.y;
670  z_rate += reading.angular_velocity.z;
671  ++count;
672  }
673 
674  double average_rate = sqrt(x_rate*x_rate + y_rate*y_rate + z_rate*z_rate) / count;
675 
676  if (count < 200){
677  ROS_WARN("Imu: calibration check acquired fewer than 200 samples.");
678  }
679 
680  // calibration succeeded
681  if (average_rate < max_drift_rate_) {
682  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);
683  calibrated_ = true;
684  publish_is_calibrated();
685  ROS_INFO("IMU gyro calibration completed.");
686  freq_diag_.clear();
687  }
688  // calibration failed
689  else{
690  calibrated_ = false;
691  publish_is_calibrated();
692  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);
693  }
694  imu.stopContinuous();
695  }
696  }
697 };
698 
699 int
700 main(int argc, char** argv)
701 {
702  ros::init(argc, argv, "microstrain_3dmgx2_node");
703 
704  ros::NodeHandle nh;
705 
706  ImuNode in(nh);
707  in.spin();
708 
709  return(0);
710 }
msg
void setErrorStatusf(const char *format,...)
Definition: imu_node.cc:182
void ConnectTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:410
void clearErrorStatus()
Definition: imu_node.cc:207
void initGyros(double *bias_x=0, double *bias_y=0, double *bias_z=0)
Initial gyros.
Definition: 3dmgx2.cc:224
std::string was_slow_
Definition: imu_node.cc:89
void GyroBiasTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:424
void publish(const boost::shared_ptr< M > &message) const
Time & fromNSec(uint64_t t)
diagnostic_updater::Updater diagnostic_
Definition: imu_node.cc:72
void summary(unsigned char lvl, const std::string s)
void ReadIDTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:417
ros::ServiceServer add_offset_serv_
Definition: imu_node.cc:77
void openPort(const char *port_name)
Open the port.
Definition: 3dmgx2.cc:113
void doCalibrate()
Definition: imu_node.cc:649
~ImuNode()
Definition: imu_node.cc:177
double linear_acceleration_stdev_
Definition: imu_node.cc:101
void setHardwareID(const std::string &hwid)
void closePort()
Close the port.
Definition: 3dmgx2.cc:170
bool spin()
Definition: imu_node.cc:356
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::NodeHandle node_handle_
Definition: imu_node.cc:74
void add(const std::string &name, TaskFunction f)
void calibrationStatus(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:581
void receiveAccelAngrate(uint64_t *time, double accel[3], double angrate[3])
Read a message of type "ACCEL_ANGRATE".
Definition: 3dmgx2.cc:389
void InterruptionTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:402
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
diagnostic_updater::FrequencyStatus freq_diag_
Definition: imu_node.cc:107
#define ROS_WARN(...)
void StreamedDataTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:466
void initTime(double fix_off)
Initialize timing variables.
Definition: 3dmgx2.cc:196
int publish_datum()
Definition: imu_node.cc:312
ros::NodeHandle private_node_handle_
Definition: imu_node.cc:75
void setFixedOffset(double fix_off)
Set the fixed time offset.
Definition: 3dmgx2.h:269
double bias_y_
Definition: imu_node.cc:97
double orientation_stdev_
Definition: imu_node.cc:102
double desired_freq_
Definition: imu_node.cc:106
A class for interfacing to the microstrain 3dmgx2 and inertialink IMUs.
Definition: 3dmgx2.h:91
void GravityTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:488
cmd
Enumeration of possible IMU commands.
Definition: 3dmgx2.h:118
double bias_x_
Definition: imu_node.cc:96
ros::ServiceServer calibrate_serv_
Definition: imu_node.cc:78
void summaryf(unsigned char lvl, const char *format,...)
void stopContinuous()
Take the device out of continous mode.
Definition: 3dmgx2.cc:277
void receiveAccelAngrateOrientation(uint64_t *time, double accel[3], double angrate[3], double orientation[9])
Read a message of type "ACCEL_ANGRATE_ORIENTATION".
Definition: 3dmgx2.cc:346
bool calibrated_
Definition: imu_node.cc:85
ros::Publisher is_calibrated_pub_
Definition: imu_node.cc:79
std::string getID(microstrain_3dmgx2_imu::IMU &imu)
Recovers hardware ID from IMU device.
Definition: get_id.cpp:46
std::string getID(bool output_info=false)
Definition: imu_node.cc:266
void setErrorStatus(const std::string msg)
Definition: imu_node.cc:194
#define ROS_INFO(...)
bool running
Definition: imu_node.cc:81
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void getData(sensor_msgs::Imu &data)
Definition: imu_node.cc:436
sensor_msgs::Imu reading
Definition: imu_node.cc:65
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
int error_count_
Definition: imu_node.cc:87
microstrain_3dmgx2_imu::IMU imu
Definition: imu_node.cc:64
ros::Publisher imu_data_pub_
Definition: imu_node.cc:76
ImuNode(ros::NodeHandle h)
Definition: imu_node.cc:109
bool setContinuous(cmd command)
Put the device in continuous mode.
Definition: 3dmgx2.cc:252
ROSCPP_DECL bool isShuttingDown()
void pretest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:390
std::string error_status_
Definition: imu_node.cc:90
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double bias_z_
Definition: imu_node.cc:98
string frameid_
Definition: imu_node.cc:92
void DisconnectTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:538
bool calibrate(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Definition: imu_node.cc:617
bool addOffset(microstrain_3dmgx2_imu::AddOffset::Request &req, microstrain_3dmgx2_imu::AddOffset::Response &resp)
Definition: imu_node.cc:597
bool autocalibrate_
Definition: imu_node.cc:83
double max_drift_rate_
Definition: imu_node.cc:104
int main(int argc, char **argv)
Definition: imu_node.cc:700
int start()
Definition: imu_node.cc:212
void deviceStatus(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:563
self_test::TestRunner self_test_
Definition: imu_node.cc:71
uint32_t getNumSubscribers() const
bool calibrate_requested_
Definition: imu_node.cc:84
double offset_
Definition: imu_node.cc:94
void setID(std::string id)
bool getDeviceIdentifierString(id_string type, char id[17])
Read one of the device identifier strings.
Definition: 3dmgx2.cc:476
int slow_count_
Definition: imu_node.cc:88
static Time now()
int stop()
Definition: imu_node.cc:295
microstrain_3dmgx2_imu::IMU::cmd cmd
Definition: imu_node.cc:69
bool ok() const
void add(const std::string &key, const T &val)
void broadcast(int lvl, const std::string msg)
void ResumeTest(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: imu_node.cc:545
string port
Definition: imu_node.cc:67
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
double angular_velocity_stdev_
Definition: imu_node.cc:100
void publish_is_calibrated()
Definition: imu_node.cc:383
#define ROS_DEBUG(...)


microstrain_3dmgx2_imu
Author(s): Jeremy Leibs, Blaise Gassend
autogenerated on Tue Jul 2 2019 19:16:18