Compute twist on odom publish event

Author: Enrique Fernandez

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -134,6 +134,18 @@ namespace diff_drive_controller
     bool updateOpenLoop(const double linear, const double angular,
         const ros::Time &time);
 
+    /**
+     * \brief Update the odometry twist with the (internal) incremental pose,
+     * since the last update/call to this method; this resets the (internal)
+     * incremental pose
+     * \param[in] time Current time, used to compute the time step/increment,
+     *                 which is used to divide the (internal) incremental pose
+     *                 by dt and obtain the twist
+     * \return true if twist is actually updated; it won't be updated if the
+     *         time step/increment is very small, to avoid division by zero
+     */
+    bool updateTwist(const ros::Time& time);
+
     /**
      * \brief Heading getter
      * \return Heading [rad]
@@ -276,17 +288,12 @@ namespace diff_drive_controller
         const double v_l, const double v_r, const ros::Time& time);
 
     /**
-     * \brief Update the odometry twist with the previous and current odometry
-     * pose
-     * \param[in] p0   Previous odometry pose
-     * \param[in] p1   Current  odometry pose
-     * \param[in] v_l  Left  wheel velocity [rad/s]
-     * \param[in] v_r  Right wheel velocity [rad/s]
-     * \param[in] time Current time
-     * \return true if the odometry twist is actually updated
+     * \brief Updates the (internal) incremental odometry with latest left and
+     * right wheel position increments
+     * \param[in] dp_l  Left  wheel position increment [rad]
+     * \param[in] dp_r  Right wheel position increment [rad]
      */
-    bool updateTwist(const SE2& p0, const SE2& p1,
-        const double v_l, const double v_r, const ros::Time& time);
+    void updateIncrementalPose(const double dp_l, const double dp_r);
 
     /**
      * \brief Update the measurement covariance
@@ -303,19 +310,31 @@ namespace diff_drive_controller
     /// Current timestamp:
     ros::Time timestamp_;
 
+    /// Timestamp for last twist computed, ie. since when the (internal)
+    /// incremental pose has been computed:
+    ros::Time timestamp_twist_;
+
     /// Current pose:
     double x_;        //   [m]
     double y_;        //   [m]
     double heading_;  // [rad]
 
+    /// Current incremental pose:
+    double d_x_;    //   [m]
+    double d_y_;    //   [m]
+    double d_yaw_;  // [rad]
+
     /// Current velocity:
-    double v_x_;   //   [m/s]
-    double v_y_;   //   [m/s]
-    double v_yaw_; // [rad/s]
+    double v_x_;    //   [m/s]
+    double v_y_;    //   [m/s]
+    double v_yaw_;  // [rad/s]
 
     /// Pose covariance:
     PoseCovariance pose_covariance_;
 
+    /// Incremental Pose covariance:
+    PoseCovariance incremental_pose_covariance_;
+
     /// Twist (and minimum twist) covariance:
     TwistCovariance twist_covariance_;
     TwistCovariance minimum_twist_covariance_;

diff_drive_controller/src/diff_drive_controller.cpp

@@ -612,10 +612,19 @@ namespace diff_drive_controller
     }
 
     // Publish odometry message:
+    bool twist_updated_ = false;
+
     const ros::Duration half_period(0.5 * control_period);
     if (last_odom_publish_time_ + publish_period_ < time + half_period &&
         odom_pub_->trylock())
     {
+      // Update twist:
+      // Note that the twist must be computed at the same frequency that it gets
+      // published, because otherwise the code that uses it cannot apply the
+      // same period it was used to compute it.
+      odometry_.updateTwist(time);
+      twist_updated_ = true;
+
       last_odom_publish_time_ = time;
 
       // Populate odom message and publish:
@@ -642,6 +651,12 @@ namespace diff_drive_controller
         last_odom_tf_publish_time_ + publish_period_ < time + half_period &&
         tf_odom_pub_->trylock())
     {
+      // Note that the tf odometry doesn't need the twist, only the pose.
+      // In the current implementation the pose is computed on its own, ie. w/o
+      // using the (internal) incremental pose. Therefore, it's always up to
+      // date with every control cycle and can be published at any rate because
+      // it doesn't depend on any period.
+
       last_odom_tf_publish_time_ = time;
 
       // Populate tf odometry frame message and publish:

diff_drive_controller/src/odometry.cpp

@@ -58,12 +58,16 @@ namespace diff_drive_controller
 
   Odometry::Odometry(size_t velocity_rolling_window_size)
   : timestamp_(0.0)
+  , timestamp_twist_(0.0)
   , x_(0.0)
   , y_(0.0)
   , heading_(0.0)
   , v_x_(0.0)
   , v_y_(0.0)
   , v_yaw_(0.0)
+  , d_x_(0.0)
+  , d_y_(0.0)
+  , d_yaw_(0.0)
   , wheel_separation_(0.0)
   , left_wheel_radius_(0.0)
   , right_wheel_radius_(0.0)
@@ -86,19 +90,19 @@ namespace diff_drive_controller
     minimum_twist_covariance_.setIdentity();
     minimum_twist_covariance_ *= DEFAULT_MINIMUM_TWIST_COVARIANCE;
 
-    // There's no need to initialize the twist covariance because it's updated
-    // from scratch on each cycle, but it's safer to initialize it anyway:
     twist_covariance_ = minimum_twist_covariance_;
 
     pose_covariance_.setIdentity();
     pose_covariance_ *= DEFAULT_POSE_COVARIANCE;
+
+    incremental_pose_covariance_.setZero();
   }
 
   void Odometry::init(const ros::Time& time)
   {
     // Reset accumulators and timestamp:
     resetAccumulators();
-    timestamp_ = time;
+    timestamp_ = timestamp_twist_ = time;
   }
 
   bool Odometry::updateCloseLoop(
@@ -138,6 +142,7 @@ namespace diff_drive_controller
 
     /// Compute time step:
     const double dt = (time - timestamp_).toSec();
+    timestamp_ = time;
 
     /// Update pose and twist:
     return update(v_l * dt, v_r * dt, v_l, v_r, time);
@@ -166,72 +171,67 @@ namespace diff_drive_controller
     /// Safe new state:
     const SE2 p1(SE2::Scalar(heading_), SE2::Point(x_, y_));
 
-    /// Update twist:
-    return updateTwist(p0, p1, v_l, v_r, time);
+    /// Update incremental pose:
+    // @todo in principle there's no need to use v_l, v_r at all!
+    //updateIncrementalPose(v_l * dt, v_r * dt);
+    updateIncrementalPose(dp_l, dp_r);
+
+    return true;
   }
 
-  bool Odometry::updateTwist(const SE2& p0, const SE2& p1,
-      double v_l, double v_r, const ros::Time& time)
+  void Odometry::updateIncrementalPose(
+      const double dp_l, const double dp_r)
   {
-    /// We cannot estimate the speed with very small time intervals:
-    const double dt = (time - timestamp_).toSec();
-    if (dt < 0.0001)
-      return false;
+    /// Integrate incremental odometry pose:
+    IntegrateFunction::PoseJacobian J_pose;
+    IntegrateFunction::MeasJacobian J_meas;
+    (*integrate_fun_)(d_x_, d_y_, d_yaw_, dp_l, dp_r, J_pose, J_meas);
 
-    timestamp_ = time;
+    /// Update Measurement Covariance with the wheel joint position increments:
+    updateMeasCovariance(dp_l, dp_r);
 
-    /// Compute relative transformation:
-    const SE2 p = p0.inverse() * p1;
+    /// Update incremental pose covariance:
+    incremental_pose_covariance_ =
+        J_pose * incremental_pose_covariance_ * J_pose.transpose() +
+        J_meas * meas_covariance_ * J_meas.transpose();
+  }
 
-    /// 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();
+  bool Odometry::updateTwist(const ros::Time& time)
+  {
+    /// We cannot estimate the speed with very small time intervals:
+    const double dt = (time - timestamp_twist_).toSec();
+    if (dt < 0.0001)
+    {
+      return false;
+    }
 
-    v_x_   = t[0];
-    v_y_   = t[1];
-    v_yaw_ = p.so2().log();
+    timestamp_twist_ = time;
 
     /// 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);
+    const double f = 1.0 / dt;
+
+    v_x_acc_(d_x_ * f);
+    v_y_acc_(d_y_ * f);
+    v_yaw_acc_(d_yaw_ * f);
 
     v_x_   = bacc::rolling_mean(v_x_acc_);
     v_y_   = bacc::rolling_mean(v_y_acc_);
     v_yaw_ = bacc::rolling_mean(v_yaw_acc_);
 
-    /// Integrate odometry twist:
-    /// Note that this is done this way because it isn't trivial to compute the
-    /// Jacobians for the relative transformation between p0 and p1
-    const double dp_l = v_l * dt;
-    const double dp_r = v_r * dt;
-
-    IntegrateFunction::PoseJacobian J_dummy;
-    IntegrateFunction::MeasJacobian J_meas;
-    double x = 0.0, y = 0.0, yaw = 0.0;
-    (*integrate_fun_)(x, y, yaw, dp_l, dp_r, J_dummy, J_meas);
-
-    /// Include the Jacobian of dividing by dt, which is equivalent to divide
-    /// all the elements of the other Jacobian by dt:
-    J_meas /= dt;
-
-    /// Update Measurement Covariance with the wheel joint velocites:
-    updateMeasCovariance(dp_l, dp_r);
-
     /// Update twist covariance:
-    twist_covariance_ = J_meas * meas_covariance_ * J_meas.transpose();
+    IntegrateFunction::PoseJacobian J_twist =
+       IntegrateFunction::PoseJacobian::Identity() * f;
+    twist_covariance_ = J_twist * incremental_pose_covariance_ * J_twist.transpose();
 
     /// Add minimum (diagonal) covariance to avoid ill-conditioned covariance
     /// matrices, i.e. with a very large condition number, which would make
     /// inverse or Cholesky decomposition fail on many algorithms:
     twist_covariance_ += minimum_twist_covariance_;
 
+    /// Reset incremental pose and its covariance:
+    d_x_ = d_y_ = d_yaw_ = 0.0;
+    incremental_pose_covariance_.setZero();
+
     return true;
   }