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