wheel_odometry.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2017 Pavlo Kolomiiets.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
18 #include <mavros_msgs/WheelOdomStamped.h>
19 
20 #include <geometry_msgs/TwistWithCovarianceStamped.h>
21 #include <nav_msgs/Odometry.h>
24 #include <tf2_eigen/tf2_eigen.h>
25 
26 namespace mavros {
27 namespace extra_plugins {
36 public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 
40  wo_nh("~wheel_odometry"),
41  count(0),
42  odom_mode(OM::NONE),
43  raw_send(false),
44  twist_send(false),
45  tf_send(false),
46  yaw_initialized(false),
47  rpose(Eigen::Vector3d::Zero()),
48  rtwist(Eigen::Vector3d::Zero()),
49  rpose_cov(Eigen::Matrix3d::Zero()),
50  rtwist_cov(Eigen::Vector3d::Zero())
51  { }
52 
53  void initialize(UAS &uas_) override
54  {
56 
57  // General params
58  wo_nh.param("send_raw", raw_send, false);
59  // Wheels configuration
60  wo_nh.param("count", count, 2);
61  count = std::max(1, count); // bound check
62 
63  bool use_rpm;
64  wo_nh.param("use_rpm", use_rpm, false);
65  if (use_rpm)
67  else
69 
70  // Odometry params
71  wo_nh.param("send_twist", twist_send, false);
72  wo_nh.param<std::string>("frame_id", frame_id, "odom");
73  wo_nh.param<std::string>("child_frame_id", child_frame_id, "base_link");
74  wo_nh.param("vel_error", vel_cov, 0.1);
75  vel_cov = vel_cov*vel_cov; // std -> cov
76  // TF subsection
77  wo_nh.param("tf/send", tf_send, false);
78  wo_nh.param<std::string>("tf/frame_id", tf_frame_id, "odom");
79  wo_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");
80 
81  // Read parameters for each wheel.
82  {
83  int iwheel = 0;
84  while (true) {
85  // Build the string in the form "wheelX", where X is the wheel number.
86  // Check if we have "wheelX" parameter.
87  // Indices starts from 0 and should increase without gaps.
88  auto name = utils::format("wheel%i", iwheel++);
89 
90  // Check if we have "wheelX" parameter
91  if (!wo_nh.hasParam(name)) break;
92 
93  // Read
94  Eigen::Vector2d offset;
95  double radius;
96 
97  wo_nh.param(name+"/x", offset[0], 0.0);
98  wo_nh.param(name+"/y", offset[1], 0.0);
99  wo_nh.param(name+"/radius", radius, 0.05);
100 
101  wheel_offset.push_back(offset);
102  wheel_radius.push_back(radius);
103  }
104 
105  // Check for all wheels specified
106  if (wheel_offset.size() >= count) {
107  // Duplicate 1st wheel if only one is available.
108  // This generalizes odometry computations for 1- and 2-wheels configurations.
109  if (wheel_radius.size() == 1) {
110  wheel_offset.resize(2);
111  wheel_radius.resize(2);
112  wheel_offset[1].x() = wheel_offset[0].x();
113  wheel_offset[1].y() = wheel_offset[0].y() + 1.0; // make separation non-zero to avoid div-by-zero
114  wheel_radius[1] = wheel_radius[0];
115  }
116 
117  // Check for non-zero wheel separation (first two wheels)
118  double separation = std::abs(wheel_offset[1].y() - wheel_offset[0].y());
119  if (separation < 1.e-5) {
121  ROS_WARN_NAMED("wo", "WO: Separation between the first two wheels is too small (%f).", separation);
122  }
123 
124  // Check for reasonable radiuses
125  for (int i = 0; i < wheel_radius.size(); i++) {
126  if (wheel_radius[i] <= 1.e-5) {
128  ROS_WARN_NAMED("wo", "WO: Wheel #%i has incorrect radius (%f).", i, wheel_radius[i]);
129  }
130  }
131  }
132  else {
134  ROS_WARN_NAMED("wo", "WO: Not all wheels have parameters specified (%lu/%i).", wheel_offset.size(), count);
135  }
136  }
137 
138  // Advertise RPM-s and distance-s
139  if (raw_send) {
140  rpm_pub = wo_nh.advertise<mavros_msgs::WheelOdomStamped>("rpm", 10);
141  dist_pub = wo_nh.advertise<mavros_msgs::WheelOdomStamped>("distance", 10);
142  }
143 
144  // Advertize topics
145  if (odom_mode != OM::NONE) {
146  if (twist_send)
147  twist_pub = wo_nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("velocity", 10);
148  else
149  odom_pub = wo_nh.advertise<nav_msgs::Odometry>("odom", 10);
150  }
151  // No-odometry warning
152  else
153  ROS_WARN_NAMED("wo", "WO: No odometry computations will be performed.");
154 
155  }
156 
158  {
159  return {
162  };
163  }
164 
165 private:
167 
172 
174  enum class OM {
175  NONE,
176  RPM,
177  DIST
178  };
180 
181  int count;
182  bool raw_send;
183  std::vector<Eigen::Vector2d> wheel_offset;
184  std::vector<double> wheel_radius;
185 
186  bool twist_send;
187  bool tf_send;
188  std::string frame_id;
189  std::string child_frame_id;
190  std::string tf_frame_id;
191  std::string tf_child_frame_id;
192  double vel_cov;
193 
196  std::vector<double> measurement_prev;
197 
199 
201  Eigen::Vector3d rpose;
202  Eigen::Vector3d rtwist;
203  Eigen::Matrix3d rpose_cov;
204  Eigen::Vector3d rtwist_cov;
205 
214  {
215  // Get initial yaw (from IMU)
216  // Check that IMU was already initialized
217  if (!yaw_initialized && m_uas->get_attitude_imu_enu()) {
219 
220  // Rotate current pose by initial yaw
221  Eigen::Rotation2Dd rot(yaw);
222  rpose.head(2) = rot * rpose.head(2); // x,y
223  rpose(2) += yaw; // yaw
224 
225  ROS_INFO_NAMED("wo", "WO: Initial yaw (deg): %f", yaw/M_PI*180.0);
226  yaw_initialized = true;
227  }
228 
229  // Orientation (only if we have initial yaw)
230  geometry_msgs::Quaternion quat;
231  if (yaw_initialized)
232  quat = tf2::toMsg(ftf::quaternion_from_rpy(0.0, 0.0, rpose(2)));
233 
234  // Twist
235  geometry_msgs::TwistWithCovariance twist_cov;
236  // linear
237  twist_cov.twist.linear.x = rtwist(0);
238  twist_cov.twist.linear.y = rtwist(1);
239  twist_cov.twist.linear.z = 0.0;
240  // angular
241  twist_cov.twist.angular.x = 0.0;
242  twist_cov.twist.angular.y = 0.0;
243  twist_cov.twist.angular.z = rtwist(2);
244  // covariance
245  ftf::EigenMapCovariance6d twist_cov_map(twist_cov.covariance.data());
246  twist_cov_map.setZero();
247  twist_cov_map.block<2, 2>(0, 0).diagonal() << rtwist_cov(0), rtwist_cov(1);
248  twist_cov_map.block<1, 1>(5, 5).diagonal() << rtwist_cov(2);
249 
250  // Publish twist
251  if (twist_send) {
252  auto twist_cov_t = boost::make_shared<geometry_msgs::TwistWithCovarianceStamped>();
253  // header
254  twist_cov_t->header.stamp = time;
255  twist_cov_t->header.frame_id = frame_id;
256  // twist
257  twist_cov_t->twist = twist_cov;
258  // publish
259  twist_pub.publish(twist_cov_t);
260  }
261  // Publish odometry (only if we have initial yaw)
262  else if (yaw_initialized) {
263  auto odom = boost::make_shared<nav_msgs::Odometry>();
264  // header
265  odom->header.stamp = time;
266  odom->header.frame_id = frame_id;
267  odom->child_frame_id = child_frame_id;
268  // pose
269  odom->pose.pose.position.x = rpose(0);
270  odom->pose.pose.position.y = rpose(1);
271  odom->pose.pose.position.z = 0.0;
272  odom->pose.pose.orientation = quat;
273  ftf::EigenMapCovariance6d pose_cov_map(odom->pose.covariance.data());
274  pose_cov_map.block<2, 2>(0, 0) << rpose_cov.block<2, 2>(0, 0);
275  pose_cov_map.block<2, 1>(0, 5) << rpose_cov.block<2, 1>(0, 2);
276  pose_cov_map.block<1, 2>(5, 0) << rpose_cov.block<1, 2>(2, 0);
277  pose_cov_map.block<1, 1>(5, 5) << rpose_cov.block<1, 1>(2, 2);
278  // twist
279  odom->twist = twist_cov;
280  // publish
281  odom_pub.publish(odom);
282  }
283 
284  // Publish TF (only if we have initial yaw)
285  if (tf_send && yaw_initialized) {
286  geometry_msgs::TransformStamped transform;
287  // header
288  transform.header.stamp = time;
289  transform.header.frame_id = tf_frame_id;
290  transform.child_frame_id = tf_child_frame_id;
291  // translation
292  transform.transform.translation.x = rpose(0);
293  transform.transform.translation.y = rpose(1);
294  transform.transform.translation.z = 0.0;
295  // rotation
296  transform.transform.rotation = quat;
297  // publish
298  m_uas->tf2_broadcaster.sendTransform(transform);
299  }
300  }
301 
317  void update_odometry_diffdrive(std::vector<double> distance, double dt)
318  {
319  double y0 = wheel_offset[0](1);
320  double y1 = wheel_offset[1](1);
321  double a = -wheel_offset[0](0);
322  double dy_inv = 1.0 / (y1 - y0);
323  double dt_inv = 1.0 / dt;
324 
325  // Rotation angle
326  double theta = (distance[1] - distance[0]) * dy_inv;
327  // Distance traveled by the projection of origin onto the axis connecting the wheels (Op)
328  double L = (y1 * distance[0] - y0 * distance[1]) * dy_inv;
329 
330  // Instantenous pose update in local (robot) coordinate system (vel*dt)
331  Eigen::Vector3d v(L, a*theta, theta);
332  // Instantenous local twist
333  rtwist = v * dt_inv;
334 
335  // Compute local pose update (approximate).
336  // In the book a=0 and |y0|=|y1|, additionally.
337  // dx = L*cos(theta/2) - a*theta*sin(theta/2)
338  // dy = L*sin(theta/2) + a*theta*cos(theta/2)
339  // Compute local pose update (exact)
340  // dx = a*(cos(theta)-1) + R*sin(theta)
341  // dy = a*sin(theta) - R*(cos(theta)-1)
342  // where R - rotation radius of Op around ICC (R = L/theta).
343  double cos_theta = std::cos(theta);
344  double sin_theta = std::sin(theta);
345  double p; // sin(theta)/theta
346  double q; // (1-cos(theta))/theta
347  if (std::abs(theta) > 1.e-5) {
348  p = sin_theta / theta;
349  q = (1.0 - cos_theta) / theta;
350  }
351  // Limits for theta -> 0
352  else {
353  p = 1.0;
354  q = 0.0;
355  }
356 
357  // Local pose update matrix
358  Eigen::Matrix3d M;
359  M << p, -q, 0,
360  q, p, 0,
361  0, 0, 1;
362 
363  // Local pose update
364  Eigen::Vector3d dpose = M * v;
365 
366  // Rotation by yaw
367  double cy = std::cos(rpose(2));
368  double sy = std::sin(rpose(2));
369  Eigen::Matrix3d R;
370  R << cy, -sy, 0,
371  sy, cy, 0,
372  0, 0, 1;
373 
374  // World pose
375  rpose += R * dpose;
376  rpose(2) = fmod(rpose(2), 2.0*M_PI); // Clamp to (-2*PI, 2*PI)
377 
378  // Twist errors (constant in time)
379  if (rtwist_cov(0) == 0.0) {
380  rtwist_cov(0) = vel_cov * (y0*y0 + y1*y1) * dy_inv*dy_inv; // vx_cov
381  rtwist_cov(1) = vel_cov * a*a * 2.0 * dy_inv*dy_inv + 0.001; // vy_cov (add extra error, otherwise vy_cov= 0 if a=0)
382  rtwist_cov(2) = vel_cov * 2.0 * dy_inv*dy_inv; // vyaw_cov
383  }
384 
385  // Pose errors (accumulated in time).
386  // Exact formulations respecting kinematic equations.
387  // dR/dYaw
388  Eigen::Matrix3d R_yaw;
389  R_yaw << -sy, -cy, 0,
390  cy, -sy, 0,
391  0, 0, 0;
392  // dYaw/dPose
393  Eigen::Vector3d yaw_pose(0, 0, 1);
394  // Jacobian by previous pose
395  Eigen::Matrix3d J_pose = Eigen::Matrix3d::Identity() + R_yaw * dpose * yaw_pose.transpose();
396 
397  // dL,dTheta / dL0,dL1
398  double L_L0 = y1 * dy_inv;
399  double L_L1 = -y0 * dy_inv;
400  double theta_L0 = -dy_inv;
401  double theta_L1 = dy_inv;
402  // dv/dMeasurement
403  Eigen::Matrix<double, 3, 2> v_meas;
404  v_meas << L_L0, L_L1,
405  a*theta_L0, a*theta_L1,
406  theta_L0, theta_L1;
407  // dTheta/dMeasurement
408  Eigen::Vector2d theta_meas(theta_L0, theta_L1);
409  // dM/dTheta
410  double px; // dP/dTheta
411  double qx; // dQ/dTheta
412  if (std::abs(theta) > 1.e-5) {
413  px = (theta*cos_theta - sin_theta) / (theta*theta);
414  qx = (theta*sin_theta - (1-cos_theta)) / (theta*theta);
415  }
416  // Limits for theta -> 0
417  else {
418  px = 0;
419  qx = 0.5;
420  }
421  // dM/dTheta
422  Eigen::Matrix3d M_theta;
423  M_theta << px, -qx, 0,
424  qx, px, 0,
425  0, 0, 0;
426  // Jacobian by measurement
427  Eigen::Matrix<double, 3, 2> J_meas = R * (M * v_meas + M_theta * v * theta_meas.transpose());
428 
429  // Measurement cov
430  double L0_cov = vel_cov * dt*dt;
431  double L1_cov = vel_cov * dt*dt;
432  Eigen::Matrix2d meas_cov;
433  meas_cov << L0_cov, 0,
434  0, L1_cov;
435 
436  // Update pose cov
437  rpose_cov = J_pose * rpose_cov * J_pose.transpose() + J_meas * meas_cov * J_meas.transpose();
438  }
439 
446  void update_odometry(std::vector<double> distance, double dt)
447  {
448  // Currently, only 2-wheels configuration implemented
449  int nwheels = std::min(2, (int)distance.size());
450  switch (nwheels)
451  {
452  // Differential drive robot.
453  case 2:
454  update_odometry_diffdrive(distance, dt);
455  break;
456  }
457  }
458 
466  void process_measurement(std::vector<double> measurement, bool rpm, ros::Time time, ros::Time time_pub)
467  {
468  // Initial measurement
469  if (time_prev == ros::Time(0)) {
470  count_meas = measurement.size();
471  measurement_prev.resize(count_meas);
472  count = std::min(count, count_meas); // don't try to use more wheels than we have
473  }
474  // Same time stamp (messages are generated by FCU more often than the wheel state updated)
475  else if (time == time_prev) {
476  return;
477  }
478  // # of wheels differs from the initial value
479  else if (measurement.size() != count_meas) {
480  ROS_WARN_THROTTLE_NAMED(10, "wo", "WO: Number of wheels in measurement (%lu) differs from the initial value (%i).", measurement.size(), count_meas);
481  return;
482  }
483  // Compute odometry
484  else {
485  double dt = (time - time_prev).toSec(); // Time since previous measurement (s)
486 
487  // Distance traveled by each wheel since last measurement.
488  // Reserve for at least 2 wheels.
489  std::vector<double> distance(std::max(2, count));
490  // Compute using RPM-s
491  if (rpm) {
492  for (int i = 0; i < count; i++) {
493  double RPM_2_SPEED = wheel_radius[i] * 2.0 * M_PI / 60.0; // RPM -> speed (m/s)
494  double rpm = 0.5 * (measurement[i] + measurement_prev[i]); // Mean RPM during last dt seconds
495  distance[i] = rpm * RPM_2_SPEED * dt;
496  }
497  }
498  // Compute using cumulative distances
499  else {
500  for (int i = 0; i < count; i++)
501  distance[i] = measurement[i] - measurement_prev[i];
502  }
503 
504  // Make distance of the 2nd wheel equal to that of the 1st one if requested or only one is available.
505  // This generalizes odometry computations for 1- and 2-wheels configurations.
506  if (count == 1)
507  distance[1] = distance[0];
508 
509  // Update odometry
510  update_odometry(distance, dt);
511 
512  // Publish odometry
513  publish_odometry(time_pub);
514  }
515 
516  // Time step
517  time_prev = time;
518  std::copy_n(measurement.begin(), measurement.size(), measurement_prev.begin());
519  }
520 
521  /* -*- message handlers -*- */
522 
529  void handle_rpm(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RPM &rpm)
530  {
531  // Get ROS timestamp of the message
533 
534  // Publish RPM-s
535  if (raw_send) {
536  auto rpm_msg = boost::make_shared<mavros_msgs::WheelOdomStamped>();
537 
538  rpm_msg->header.stamp = timestamp;
539  rpm_msg->data.resize(2);
540  rpm_msg->data[0] = rpm.rpm1;
541  rpm_msg->data[1] = rpm.rpm2;
542 
543  rpm_pub.publish(rpm_msg);
544  }
545 
546  // Process measurement
547  if (odom_mode == OM::RPM) {
548  std::vector<double> measurement{rpm.rpm1, rpm.rpm2};
549  process_measurement(measurement, true, timestamp, timestamp);
550  }
551  }
552 
559  void handle_wheel_distance(const mavlink::mavlink_message_t *msg, mavlink::common::msg::WHEEL_DISTANCE &wheel_dist)
560  {
561  // Check for bad wheels count
562  if (wheel_dist.count == 0)
563  return;
564 
565  // Get ROS timestamp of the message
566  ros::Time timestamp = m_uas->synchronise_stamp(wheel_dist.time_usec);
567  // Get internal timestamp of the message
568  ros::Time timestamp_int = ros::Time(wheel_dist.time_usec / 1000000UL, 1000UL * (wheel_dist.time_usec % 1000000UL));
569 
570  // Publish distances
571  if (raw_send) {
572  auto wheel_dist_msg = boost::make_shared<mavros_msgs::WheelOdomStamped>();
573 
574  wheel_dist_msg->header.stamp = timestamp;
575  wheel_dist_msg->data.resize(wheel_dist.count);
576  std::copy_n(wheel_dist.distance.begin(), wheel_dist.count, wheel_dist_msg->data.begin());
577 
578  dist_pub.publish(wheel_dist_msg);
579  }
580 
581  // Process measurement
582  if (odom_mode == OM::DIST) {
583  std::vector<double> measurement(wheel_dist.count);
584  std::copy_n(wheel_dist.distance.begin(), wheel_dist.count, measurement.begin());
585  process_measurement(measurement, false, timestamp_int, timestamp);
586  }
587  }
588 };
589 } // namespace extra_plugins
590 } // namespace mavros
591 
Eigen::Vector3d rpose
Robot origin 2D-state (SI units)
bool twist_send
send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry
OM odom_mode
odometry computation mode
int count_meas
number of wheels in measurements
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
std::string format(const std::string &fmt, Args...args)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
geometry_msgs::Quaternion get_attitude_orientation_enu()
void handle_rpm(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RPM &rpm)
Handle Ardupilot RPM MAVlink message. Message specification: http://mavlink.io/en/messages/ardupilotm...
ros::Time time_prev
timestamp of previous measurement
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
uint32_t offset
use wheel&#39;s cumulative distance
Eigen::Map< Eigen::Matrix< double, 6, 6, Eigen::RowMajor > > EigenMapCovariance6d
std::vector< Eigen::Vector2d > wheel_offset
wheel x,y offsets (m,NED)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW WheelOdometryPlugin()
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
std::string tf_child_frame_id
frame for TF and Pose
std::string frame_id
origin frame for topic headers
tf2_ros::TransformBroadcaster tf2_broadcaster
bool raw_send
send wheel&#39;s RPM and cumulative distance
std::vector< double > wheel_radius
wheel radiuses (m)
Eigen::Vector3d rtwist
twist (vx, vy, vyaw)
std::vector< double > measurement_prev
previous measurement
int count
requested number of wheels to compute odometry
void process_measurement(std::vector< double > measurement, bool rpm, ros::Time time, ros::Time time_pub)
Process wheel measurement.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Eigen::Matrix3d rpose_cov
pose error 1-var
void update_odometry_diffdrive(std::vector< double > distance, double dt)
Update odometry for differential drive robot. Odometry is computed for robot&#39;s origin (IMU)...
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool yaw_initialized
initial yaw initialized (from IMU)
void sendTransform(const geometry_msgs::TransformStamped &transform)
std::string child_frame_id
body-fixed frame for topic headers
B toMsg(const A &a)
void publish_odometry(ros::Time time)
Publish odometry. Odometry is computed from the very start but no pose info is published until we hav...
double vel_cov
wheel velocity measurement error 1-var (m/s)
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
Eigen::Vector3d rtwist_cov
twist error 1-var (vx_cov, vy_cov, vyaw_cov)
std::vector< HandlerInfo > Subscriptions
double quaternion_get_yaw(const Eigen::Quaterniond &q)
void handle_wheel_distance(const mavlink::mavlink_message_t *msg, mavlink::common::msg::WHEEL_DISTANCE &wheel_dist)
Handle WHEEL_DISTANCE MAVlink message. Message specification: https://mavlink.io/en/messages/common.html#WHEEL_DISTANCE.
ftf::StaticTF transform
static Time now()
void initialize(UAS &uas_) override
bool hasParam(const std::string &key) const
sensor_msgs::Imu::Ptr get_attitude_imu_enu()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void update_odometry(std::vector< double > distance, double dt)
Update odometry (currently, only 2-wheels differential configuration implemented). Odometry is computed for robot&#39;s origin (IMU).


mavros_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue Jun 1 2021 02:36:36