Fix odometry twist computation

diff_drive_controller/CMakeLists.txt

@@ -9,13 +9,16 @@ find_package(catkin REQUIRED COMPONENTS
  urdf
 )
 
+find_package(sophus REQUIRED)
+
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${PROJECT_NAME}
 )
 
 include_directories(
  include ${catkin_INCLUDE_DIRS}
+ include ${sophus_INCLUDE_DIRS}
 )
 
 add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -43,6 +43,7 @@
 #define ODOMETRY_H_
 
 #include <ros/time.h>
+#include <sophus/se2.hpp>
 #include <boost/accumulators/accumulators.hpp>
 #include <boost/accumulators/statistics/stats.hpp>
 #include <boost/accumulators/statistics/rolling_mean.hpp>
@@ -63,6 +64,9 @@ namespace diff_drive_controller
  /// Integration function, used to integrate the odometry:
  typedef boost::function<void(double, double)> IntegrationFunction;
 
+ /// SO(2) and SE(2) Lie Groups:
+ typedef Sophus::SE2d SE2;
+
  /**
  * \brief Constructor
  * Timestamp will get the current time value
@@ -91,8 +95,19 @@ namespace diff_drive_controller
  * \param linear Linear velocity [m/s]
  * \param angular Angular velocity [rad/s]
  * \param time Current time
+ * \return true if the odometry is actually updated
+ */
+ bool updateOpenLoop(double linear, double angular, const ros::Time &time);
+
+ /**
+ * \brief Update the odometry twist with the previous and current odometry
+ * pose
+ * \param p0 Previous odometry pose
+ * \param p1 Current odometry pose
+ * \param time Current time
+ * \return true if the odometry twist is actually updated
  */
- void updateOpenLoop(double linear, double angular, const ros::Time &time);
+ bool updateTwist(const SE2& p0, const SE2& p1, const ros::Time& time);
 
  /**
  * \brief heading getter
@@ -122,21 +137,30 @@ namespace diff_drive_controller
  }
 
  /**
- * \brief linear velocity getter
- * \return linear velocity [m/s]
+ * \brief x velocity getter
+ * \return x velocity [m/s]
+ */
+ double getVx() const
+ {
+ return v_x_;
+ }
+
+ /**
+ * \brief y velocity getter
+ * \return y velocity [m/s]
  */
- double getLinear() const
+ double getVy() const
  {
- return linear_;
+ return v_y_;
  }
 
  /**
- * \brief angular velocity getter
- * \return angular velocity [rad/s]
+ * \brief yaw velocity getter
+ * \return yaw velocity [rad/s]
  */
- double getAngular() const
+ double getVyaw() const
  {
- return angular_;
+ return v_yaw_;
  }
 
  /**
@@ -186,8 +210,9 @@ namespace diff_drive_controller
  double heading_; // [rad]
 
  /// Current velocity:
- double linear_; // [m/s]
- double angular_; // [rad/s]
+ double v_x_; // [m/s]
+ double v_y_; // [m/s]
+ double v_yaw_; // [rad/s]
 
  /// Wheel kinematic parameters [m]:
  double wheel_separation_;
@@ -199,8 +224,9 @@ namespace diff_drive_controller
 
  /// Rolling mean accumulators for the linar and angular velocities:
  size_t velocity_rolling_window_size_;
- RollingMeanAcc linear_acc_;
- RollingMeanAcc angular_acc_;
+ RollingMeanAcc v_x_acc_;
+ RollingMeanAcc v_y_acc_;
+ RollingMeanAcc v_yaw_acc_;
 
  /// Integration funcion, used to integrate the odometry:
  IntegrationFunction integrate_fun_;

diff_drive_controller/package.xml

@@ -15,6 +15,8 @@
 
  <buildtool_depend>catkin</buildtool_depend>
 
+ <build_depend>sophus</build_depend>
+
  <depend>controller_interface</depend>
  <depend>nav_msgs</depend>
  <depend>realtime_tools</depend>

diff_drive_controller/src/diff_drive_controller.cpp

@@ -350,8 +350,9 @@ namespace diff_drive_controller{
  odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
  odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
  odom_pub_->msg_.pose.pose.orientation = orientation;
- odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
- odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
+ odom_pub_->msg_.twist.twist.linear.x = odometry_.getVx();
+ odom_pub_->msg_.twist.twist.linear.y = odometry_.getVy();
+ odom_pub_->msg_.twist.twist.angular.z = odometry_.getVyaw();
  odom_pub_->unlockAndPublish();
  }
 
@@ -618,7 +619,6 @@ namespace diff_drive_controller{
  (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
  (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
  (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
- odom_pub_->msg_.twist.twist.linear.y = 0;
  odom_pub_->msg_.twist.twist.linear.z = 0;
  odom_pub_->msg_.twist.twist.angular.x = 0;
  odom_pub_->msg_.twist.twist.angular.y = 0;

diff_drive_controller/src/odometry.cpp

@@ -41,6 +41,8 @@
 
 #include <diff_drive_controller/odometry.h>
 
+#include <Eigen/Core>
+
 #include <boost/bind.hpp>
 
 namespace diff_drive_controller
@@ -52,15 +54,17 @@ namespace diff_drive_controller
  , x_(0.0)
  , y_(0.0)
  , heading_(0.0)
- , linear_(0.0)
- , angular_(0.0)
+ , v_x_(0.0)
+ , v_y_(0.0)
+ , v_yaw_(0.0)
  , wheel_separation_(0.0)
  , wheel_radius_(0.0)
  , left_wheel_old_pos_(0.0)
  , right_wheel_old_pos_(0.0)
  , velocity_rolling_window_size_(velocity_rolling_window_size)
- , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
- , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+ , v_x_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+ , v_y_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+ , v_yaw_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
  {
  }
@@ -90,36 +94,70 @@ namespace diff_drive_controller
  const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
  const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
 
+ /// Safe current state:
+ const SE2 p0(SE2::Scalar(heading_), SE2::Point(x_, y_));
+
  /// Integrate odometry:
  integrate_fun_(linear, angular);
 
+ /// Update twist:
+ const SE2 p1(SE2::Scalar(heading_), SE2::Point(x_, y_));
+
+ return updateTwist(p0, p1, time);
+ }
+
+ bool Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
+ {
+ /// Safe current state:
+ const SE2 p0(SE2::Scalar(heading_), SE2::Point(x_, y_));
+
+ /// Integrate odometry:
+ const double dt = (time - timestamp_).toSec();
+ timestamp_ = time;
+ integrate_fun_(linear * dt, angular * dt);
+
+ /// Update twist:
+ const SE2 p1(SE2::Scalar(heading_), SE2::Point(x_, y_));
+
+ return updateTwist(p0, p1, time);
+ }
+
+ bool Odometry::updateTwist(const SE2& p0, const SE2& p1, const ros::Time& time)
+ {
  /// We cannot estimate the speed with very small time intervals:
  const double dt = (time - timestamp_).toSec();
  if (dt < 0.0001)
  return false; // Interval too small to integrate with
 
  timestamp_ = time;
 
- /// Estimate speeds using a rolling mean to filter them out:
- linear_acc_(linear/dt);
- angular_acc_(angular/dt);
+ /// Compute relative transformation:
+ const SE2 p = p0.inverse() * p1;
 
- linear_ = bacc::rolling_mean(linear_acc_);
- angular_ = bacc::rolling_mean(angular_acc_);
+ /// Retrieve rotation and translation:
+ /// Note that we don't use the log from SE(2) because we didn't use exp
+ /// to create p0 and p1.
+ /// So instead of:
+ ///
+ /// const SE2::Tangent v = p.log();
+ ///
+ /// we use the following:
+ const SE2::ConstTranslationReference t = p.translation();
 
- return true;
- }
+ v_x_ = t[0];
+ v_y_ = t[1];
+ v_yaw_ = p.so2().log();
 
- void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
- {
- /// Save last linear and angular velocity:
- linear_ = linear;
- angular_ = angular;
+ /// Estimate speeds using a rolling mean to filter them out:
+ v_x_acc_(v_x_/dt);
+ v_y_acc_(v_y_/dt);
+ v_yaw_acc_(v_yaw_/dt);
 
- /// Integrate odometry:
- const double dt = (time - timestamp_).toSec();
- timestamp_ = time;
- integrate_fun_(linear * dt, angular * dt);
+ v_x_ = bacc::rolling_mean(v_x_acc_);
+ v_y_ = bacc::rolling_mean(v_y_acc_);
+ v_yaw_ = bacc::rolling_mean(v_yaw_acc_);
+
+ return true;
  }
 
  void Odometry::setWheelParams(double wheel_separation, double wheel_radius)
@@ -167,8 +205,9 @@ namespace diff_drive_controller
 
  void Odometry::resetAccumulators()
  {
- linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
- angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+ v_x_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+ v_y_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+ v_yaw_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
  }
 
 } // namespace diff_drive_controller