main.cpp
Go to the documentation of this file.
1 
35 #include <string>
36 
37 #include "geometry_msgs/Vector3Stamped.h"
38 #include "ros/ros.h"
39 #include "sensor_msgs/Imu.h"
40 #include "sensor_msgs/MagneticField.h"
41 #include "serial/serial.h"
42 #include "std_msgs/Float32.h"
43 #include "std_msgs/Header.h"
44 #include "um7/comms.h"
45 #include "um7/registers.h"
46 #include "um7/Reset.h"
47 
48 const char VERSION[10] = "0.0.2"; // um7_driver version
49 
50 // Don't try to be too clever. Arrival of this message triggers
51 // us to publish everything we have.
53 
55 {
57  {
59  };
60 }
62 
67 template<typename RegT>
69  std::string param, std::string human_name)
70 {
71  if (reg.length != 3)
72  {
73  throw std::logic_error("configureVector3 may only be used with 3-field registers!");
74  }
75 
76  if (ros::param::has(param))
77  {
78  double x, y, z;
79  ros::param::get(param + "/x", x);
80  ros::param::get(param + "/y", y);
81  ros::param::get(param + "/z", z);
82  ROS_INFO_STREAM("Configuring " << human_name << " to ("
83  << x << ", " << y << ", " << z << ")");
84  reg.set_scaled(0, x);
85  reg.set_scaled(1, y);
86  reg.set_scaled(2, z);
87  if (sensor->sendWaitAck(reg))
88  {
89  throw std::runtime_error("Unable to configure vector.");
90  }
91  }
92 }
93 
98 template<typename RegT>
99 void sendCommand(um7::Comms* sensor, const um7::Accessor<RegT>& reg, std::string human_name)
100 {
101  ROS_INFO_STREAM("Sending command: " << human_name);
102  if (!sensor->sendWaitAck(reg))
103  {
104  throw std::runtime_error("Command to device failed.");
105  }
106 }
107 
108 
113 void configureSensor(um7::Comms* sensor, ros::NodeHandle *private_nh)
114 {
115  um7::Registers r;
116 
117  uint32_t comm_reg = (BAUD_115200 << COM_BAUD_START);
118  r.communication.set(0, comm_reg);
119  if (!sensor->sendWaitAck(r.comrate2))
120  {
121  throw std::runtime_error("Unable to set CREG_COM_SETTINGS.");
122  }
123 
124  // set the broadcast rate of the device
125  int rate;
126  private_nh->param<int>("update_rate", rate, 20);
127  if (rate < 20 || rate > 255)
128  {
129  ROS_WARN("Potentially unsupported update rate of %d", rate);
130  }
131 
132  uint32_t rate_bits = static_cast<uint32_t>(rate);
133  ROS_INFO("Setting update rate to %uHz", rate);
134  uint32_t raw_rate = (rate_bits << RATE2_ALL_RAW_START);
135  r.comrate2.set(0, raw_rate);
136  if (!sensor->sendWaitAck(r.comrate2))
137  {
138  throw std::runtime_error("Unable to set CREG_COM_RATES2.");
139  }
140 
141  uint32_t proc_rate = (rate_bits << RATE4_ALL_PROC_START);
142  r.comrate4.set(0, proc_rate);
143  if (!sensor->sendWaitAck(r.comrate4))
144  {
145  throw std::runtime_error("Unable to set CREG_COM_RATES4.");
146  }
147 
148  uint32_t misc_rate = (rate_bits << RATE5_EULER_START) | (rate_bits << RATE5_QUAT_START);
149  r.comrate5.set(0, misc_rate);
150  if (!sensor->sendWaitAck(r.comrate5))
151  {
152  throw std::runtime_error("Unable to set CREG_COM_RATES5.");
153  }
154 
155  uint32_t health_rate = (5 << RATE6_HEALTH_START); // note: 5 gives 2 hz rate
156  r.comrate6.set(0, health_rate);
157  if (!sensor->sendWaitAck(r.comrate6))
158  {
159  throw std::runtime_error("Unable to set CREG_COM_RATES6.");
160  }
161 
162 
163  // Options available using parameters)
164  uint32_t misc_config_reg = 0; // initialize all options off
165 
166  // Optionally disable mag updates in the sensor's EKF.
167  bool mag_updates;
168  private_nh->param<bool>("mag_updates", mag_updates, true);
169  if (mag_updates)
170  {
171  misc_config_reg |= MAG_UPDATES_ENABLED;
172  }
173  else
174  {
175  ROS_WARN("Excluding magnetometer updates from EKF.");
176  }
177 
178  // Optionally enable quaternion mode .
179  bool quat_mode;
180  private_nh->param<bool>("quat_mode", quat_mode, true);
181  if (quat_mode)
182  {
183  misc_config_reg |= QUATERNION_MODE_ENABLED;
184  }
185  else
186  {
187  ROS_WARN("Excluding quaternion mode.");
188  }
189 
190  r.misc_config.set(0, misc_config_reg);
191  if (!sensor->sendWaitAck(r.misc_config))
192  {
193  throw std::runtime_error("Unable to set CREG_MISC_SETTINGS.");
194  }
195 
196  // Optionally disable performing a zero gyros command on driver startup.
197  bool zero_gyros;
198  private_nh->param<bool>("zero_gyros", zero_gyros, true);
199  if (zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes");
200 }
201 
202 
204  const um7::Reset::Request& req, const um7::Reset::Response& resp)
205 {
206  um7::Registers r;
207  if (req.zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes");
208  if (req.reset_ekf) sendCommand(sensor, r.cmd_reset_ekf, "reset EKF");
209  if (req.set_mag_ref) sendCommand(sensor, r.cmd_set_mag_ref, "set magnetometer reference");
210  return true;
211 }
212 
217 void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& imu_msg,
218  OutputAxisOption axes, bool use_magnetic_field_msg)
219 {
220  static ros::Publisher imu_pub = imu_nh->advertise<sensor_msgs::Imu>("data", 1, false);
221  static ros::Publisher mag_pub;
222  if (use_magnetic_field_msg)
223  {
224  mag_pub = imu_nh->advertise<sensor_msgs::MagneticField>("mag", 1, false);
225  }
226  else
227  {
228  mag_pub = imu_nh->advertise<geometry_msgs::Vector3Stamped>("mag", 1, false);
229  }
230  static ros::Publisher rpy_pub = imu_nh->advertise<geometry_msgs::Vector3Stamped>("rpy", 1, false);
231  static ros::Publisher temp_pub = imu_nh->advertise<std_msgs::Float32>("temperature", 1, false);
232 
233  if (imu_pub.getNumSubscribers() > 0)
234  {
235  switch (axes)
236  {
238  {
239  // body-fixed frame NED to ENU: (x y z)->(x -y -z) or (w x y z)->(x -y -z w)
240  // world frame NED to ENU: (x y z)->(y x -z) or (w x y z)->(y x -z w)
241  // world frame
242  imu_msg.orientation.w = r.quat.get_scaled(2);
243  imu_msg.orientation.x = r.quat.get_scaled(1);
244  imu_msg.orientation.y = -r.quat.get_scaled(3);
245  imu_msg.orientation.z = r.quat.get_scaled(0);
246 
247  // body-fixed frame
248  imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
249  imu_msg.angular_velocity.y = -r.gyro.get_scaled(1);
250  imu_msg.angular_velocity.z = -r.gyro.get_scaled(2);
251 
252  // body-fixed frame
253  imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
254  imu_msg.linear_acceleration.y = -r.accel.get_scaled(1);
255  imu_msg.linear_acceleration.z = -r.accel.get_scaled(2);
256  break;
257  }
259  {
260  // body-fixed frame
261  imu_msg.orientation.w = -r.quat.get_scaled(0);
262  imu_msg.orientation.x = -r.quat.get_scaled(1);
263  imu_msg.orientation.y = r.quat.get_scaled(2);
264  imu_msg.orientation.z = r.quat.get_scaled(3);
265 
266  // body-fixed frame
267  imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
268  imu_msg.angular_velocity.y = -r.gyro.get_scaled(1);
269  imu_msg.angular_velocity.z = -r.gyro.get_scaled(2);
270 
271  // body-fixed frame
272  imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
273  imu_msg.linear_acceleration.y = -r.accel.get_scaled(1);
274  imu_msg.linear_acceleration.z = -r.accel.get_scaled(2);
275  break;
276  }
278  {
279  imu_msg.orientation.w = r.quat.get_scaled(0);
280  imu_msg.orientation.x = r.quat.get_scaled(1);
281  imu_msg.orientation.y = r.quat.get_scaled(2);
282  imu_msg.orientation.z = r.quat.get_scaled(3);
283 
284  imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
285  imu_msg.angular_velocity.y = r.gyro.get_scaled(1);
286  imu_msg.angular_velocity.z = r.gyro.get_scaled(2);
287 
288  imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
289  imu_msg.linear_acceleration.y = r.accel.get_scaled(1);
290  imu_msg.linear_acceleration.z = r.accel.get_scaled(2);
291  break;
292  }
293  default:
294  ROS_ERROR("OuputAxes enum value invalid");
295  }
296 
297  imu_pub.publish(imu_msg);
298  }
299 
300  // Magnetometer. transform to ROS axes
301  if (mag_pub.getNumSubscribers() > 0)
302  {
303  if (use_magnetic_field_msg)
304  {
305  sensor_msgs::MagneticField mag_msg;
306  mag_msg.header = imu_msg.header;
307 
308  switch (axes)
309  {
311  {
312  mag_msg.magnetic_field.x = r.mag.get_scaled(1);
313  mag_msg.magnetic_field.y = r.mag.get_scaled(0);
314  mag_msg.magnetic_field.z = -r.mag.get_scaled(2);
315  break;
316  }
318  {
319  // body-fixed frame
320  mag_msg.magnetic_field.x = r.mag.get_scaled(0);
321  mag_msg.magnetic_field.y = -r.mag.get_scaled(1);
322  mag_msg.magnetic_field.z = -r.mag.get_scaled(2);
323  break;
324  }
326  {
327  mag_msg.magnetic_field.x = r.mag.get_scaled(0);
328  mag_msg.magnetic_field.y = r.mag.get_scaled(1);
329  mag_msg.magnetic_field.z = r.mag.get_scaled(2);
330  break;
331  }
332  default:
333  ROS_ERROR("OuputAxes enum value invalid");
334  }
335 
336  mag_pub.publish(mag_msg);
337  }
338  else
339  {
340  geometry_msgs::Vector3Stamped mag_msg;
341  mag_msg.header = imu_msg.header;
342 
343  switch (axes)
344  {
346  {
347  mag_msg.vector.x = r.mag.get_scaled(1);
348  mag_msg.vector.y = r.mag.get_scaled(0);
349  mag_msg.vector.z = -r.mag.get_scaled(2);
350  break;
351  }
353  {
354  // body-fixed frame
355  mag_msg.vector.x = r.mag.get_scaled(0);
356  mag_msg.vector.y = -r.mag.get_scaled(1);
357  mag_msg.vector.z = -r.mag.get_scaled(2);
358  break;
359  }
361  {
362  mag_msg.vector.x = r.mag.get_scaled(0);
363  mag_msg.vector.y = r.mag.get_scaled(1);
364  mag_msg.vector.z = r.mag.get_scaled(2);
365  break;
366  }
367  default:
368  ROS_ERROR("OuputAxes enum value invalid");
369  }
370 
371  mag_pub.publish(mag_msg);
372  }
373  }
374 
375  // Euler attitudes. transform to ROS axes
376  if (rpy_pub.getNumSubscribers() > 0)
377  {
378  geometry_msgs::Vector3Stamped rpy_msg;
379  rpy_msg.header = imu_msg.header;
380 
381  switch (axes)
382  {
384  {
385  // world frame
386  rpy_msg.vector.x = r.euler.get_scaled(1);
387  rpy_msg.vector.y = r.euler.get_scaled(0);
388  rpy_msg.vector.z = -r.euler.get_scaled(2);
389  break;
390  }
392  {
393  rpy_msg.vector.x = r.euler.get_scaled(0);
394  rpy_msg.vector.y = -r.euler.get_scaled(1);
395  rpy_msg.vector.z = -r.euler.get_scaled(2);
396  break;
397  }
399  {
400  rpy_msg.vector.x = r.euler.get_scaled(0);
401  rpy_msg.vector.y = r.euler.get_scaled(1);
402  rpy_msg.vector.z = r.euler.get_scaled(2);
403  break;
404  }
405  default:
406  ROS_ERROR("OuputAxes enum value invalid");
407  }
408 
409  rpy_pub.publish(rpy_msg);
410  }
411 
412  // Temperature
413  if (temp_pub.getNumSubscribers() > 0)
414  {
415  std_msgs::Float32 temp_msg;
416  temp_msg.data = r.temperature.get_scaled(0);
417  temp_pub.publish(temp_msg);
418  }
419 }
420 
424 int main(int argc, char **argv)
425 {
426  ros::init(argc, argv, "um7_driver");
427 
428  // Load parameters from private node handle.
429  std::string port;
430  int32_t baud;
431 
432  ros::NodeHandle imu_nh("imu"), private_nh("~");
433  private_nh.param<std::string>("port", port, "/dev/ttyUSB0");
434  private_nh.param<int32_t>("baud", baud, 115200);
435 
436  serial::Serial ser;
437  ser.setPort(port);
438  ser.setBaudrate(baud);
439  serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0);
440  ser.setTimeout(to);
441 
442  sensor_msgs::Imu imu_msg;
443  double linear_acceleration_stdev, angular_velocity_stdev;
444  private_nh.param<std::string>("frame_id", imu_msg.header.frame_id, "imu_link");
445  // Defaults obtained experimentally from hardware, no device spec exists
446  private_nh.param<double>("linear_acceleration_stdev", linear_acceleration_stdev, (4.0 * 1e-3f * 9.80665));
447  private_nh.param<double>("angular_velocity_stdev", angular_velocity_stdev, (0.06 * 3.14159 / 180.0));
448 
449  double linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev;
450  double angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev;
451 
452  // From the UM7 datasheet for the dynamic accuracy from the EKF.
453  double orientation_x_stdev, orientation_y_stdev, orientation_z_stdev;
454  private_nh.param<double>("orientation_x_stdev", orientation_x_stdev, (3.0 * 3.14159 / 180.0));
455  private_nh.param<double>("orientation_y_stdev", orientation_y_stdev, (3.0 * 3.14159 / 180.0));
456  private_nh.param<double>("orientation_z_stdev", orientation_z_stdev, (5.0 * 3.14159 / 180.0));
457 
458  double orientation_x_covar = orientation_x_stdev * orientation_x_stdev;
459  double orientation_y_covar = orientation_y_stdev * orientation_y_stdev;
460  double orientation_z_covar = orientation_z_stdev * orientation_z_stdev;
461 
462  // Enable converting from NED to ENU by default
463  bool tf_ned_to_enu;
464  bool orientation_in_robot_frame;
465  private_nh.param<bool>("tf_ned_to_enu", tf_ned_to_enu, true);
466  private_nh.param<bool>("orientation_in_robot_frame", orientation_in_robot_frame, false);
468  if (tf_ned_to_enu && orientation_in_robot_frame)
469  {
470  ROS_ERROR("Requested IMU data in two separate frames.");
471  }
472  else if (tf_ned_to_enu)
473  {
474  axes = OutputAxisOptions::ENU;
475  }
476  else if (orientation_in_robot_frame)
477  {
479  }
480 
481  // Use MagneticField message rather than Vector3Stamped.
482  bool use_magnetic_field_msg;
483  private_nh.param<bool>("use_magnetic_field_msg", use_magnetic_field_msg, false);
484 
485  // These values do not need to be converted
486  imu_msg.linear_acceleration_covariance[0] = linear_acceleration_cov;
487  imu_msg.linear_acceleration_covariance[4] = linear_acceleration_cov;
488  imu_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
489 
490  imu_msg.angular_velocity_covariance[0] = angular_velocity_cov;
491  imu_msg.angular_velocity_covariance[4] = angular_velocity_cov;
492  imu_msg.angular_velocity_covariance[8] = angular_velocity_cov;
493 
494  imu_msg.orientation_covariance[0] = orientation_x_covar;
495  imu_msg.orientation_covariance[4] = orientation_y_covar;
496  imu_msg.orientation_covariance[8] = orientation_z_covar;
497 
498  // Real Time Loop
499  bool first_failure = true;
500  while (ros::ok())
501  {
502  try
503  {
504  ser.open();
505  }
506  catch (const serial::IOException& e)
507  {
508  ROS_WARN("um7_driver was unable to connect to port %s.", port.c_str());
509  }
510  if (ser.isOpen())
511  {
512  ROS_INFO("um7_driver successfully connected to serial port %s.", port.c_str());
513  first_failure = true;
514  try
515  {
516  um7::Comms sensor(&ser);
517  configureSensor(&sensor, &private_nh);
518  um7::Registers registers;
519  ros::ServiceServer srv = imu_nh.advertiseService<um7::Reset::Request, um7::Reset::Response>(
520  "reset", boost::bind(handleResetService, &sensor, _1, _2));
521 
522  while (ros::ok())
523  {
524  // triggered by arrival of last message packet
525  if (sensor.receive(&registers) == TRIGGER_PACKET)
526  {
527  // Triggered by arrival of final message in group.
528  imu_msg.header.stamp = ros::Time::now();
529  publishMsgs(registers, &imu_nh, imu_msg, axes, use_magnetic_field_msg);
530  ros::spinOnce();
531  }
532  }
533  }
534  catch(const std::exception& e)
535  {
536  if (ser.isOpen()) ser.close();
537  ROS_ERROR_STREAM(e.what());
538  ROS_INFO("Attempting reconnection after error.");
539  ros::Duration(1.0).sleep();
540  }
541  }
542  else
543  {
544  ROS_WARN_STREAM_COND(first_failure, "Could not connect to serial device "
545  << port << ". Trying again every 1 second.");
546  first_failure = false;
547  ros::Duration(1.0).sleep();
548  }
549  }
550 }
const Accessor< uint32_t > cmd_set_mag_ref
Definition: registers.h:198
void publish(const boost::shared_ptr< M > &message) const
const Accessor< float > accel
Definition: registers.h:189
const Accessor< uint32_t > cmd_reset_ekf
Definition: registers.h:198
f
#define MAG_UPDATES_ENABLED
int main(int argc, char **argv)
Definition: main.cpp:424
bool handleResetService(um7::Comms *sensor, const um7::Reset::Request &req, const um7::Reset::Response &resp)
Definition: main.cpp:203
Provides the Registers class, which initializes with a suite of accessors suitable for reading and wr...
bool sleep() const
const Accessor< int16_t > quat
Definition: registers.h:187
const Accessor< float > mag
Definition: registers.h:189
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void configureVector3(um7::Comms *sensor, const um7::Accessor< RegT > &reg, std::string param, std::string human_name)
Definition: main.cpp:68
const Accessor< uint32_t > comrate5
Definition: registers.h:192
void sendCommand(um7::Comms *sensor, const um7::Accessor< RegT > &reg, std::string human_name)
Definition: main.cpp:99
void set(uint8_t field, RegT value) const
Definition: registers.h:143
#define ROS_WARN(...)
void set_scaled(uint16_t field, double value) const
Definition: registers.h:149
bool sendWaitAck(const Accessor_ &a)
Definition: comms.cpp:198
void publishMsgs(um7::Registers &r, ros::NodeHandle *imu_nh, sensor_msgs::Imu &imu_msg, OutputAxisOption axes, bool use_magnetic_field_msg)
Definition: main.cpp:217
const uint8_t TRIGGER_PACKET
Definition: main.cpp:52
const char VERSION[10]
Definition: main.cpp:48
ROSCPP_DECL bool get(const std::string &key, std::string &s)
int16_t receive(Registers *r)
Definition: comms.cpp:54
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
#define ROS_WARN_STREAM_COND(cond, args)
ROSCPP_DECL bool has(const std::string &key)
#define BAUD_115200
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const Accessor< uint32_t > comrate4
Definition: registers.h:192
const uint16_t length
Definition: registers.h:116
const Accessor< uint32_t > cmd_zero_gyros
Definition: registers.h:198
const Accessor< uint32_t > comrate6
Definition: registers.h:192
const Accessor< uint32_t > communication
Definition: registers.h:192
const Accessor< uint32_t > comrate2
Definition: registers.h:192
#define RATE2_ALL_RAW_START
uint32_t getNumSubscribers() const
void setPort(const std::string &port)
#define ROS_INFO_STREAM(args)
#define COM_BAUD_START
Comms class definition. Does not manage the serial connection itself, but takes care of reading and w...
#define RATE4_ALL_PROC_START
void configureSensor(um7::Comms *sensor, ros::NodeHandle *private_nh)
Definition: main.cpp:113
const Accessor< uint32_t > misc_config
Definition: registers.h:192
#define RATE5_QUAT_START
#define QUATERNION_MODE_ENABLED
#define RATE6_HEALTH_START
static Time now()
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
const Accessor< float > gyro
Definition: registers.h:189
#define ROS_ERROR(...)
double get_scaled(uint16_t field) const
Definition: registers.h:138
#define DREG_EULER_PHI_THETA
#define RATE5_EULER_START
const Accessor< float > temperature
Definition: registers.h:189
const Accessor< int16_t > euler
Definition: registers.h:187


um7
Author(s): Mike Purvis , Alex Brown
autogenerated on Tue Feb 11 2020 03:26:50