Merge pull request #32 from clearpathrobotics/compute_twist_on_publish

Compute twist on publish

diff_drive_controller/CMakeLists.txt

@@ -83,6 +83,9 @@ if (CATKIN_ENABLE_TESTING)
     ${catkin_INCLUDE_DIRS}
     ${EIGEN_INCLUDE_DIRS})
 
+  catkin_add_gtest(odometry_test test/odometry_test.cpp)
+  target_link_libraries(odometry_test ${PROJECT_NAME})
+
   add_executable(diffbot test/diffbot.cpp)
   target_link_libraries(diffbot ${catkin_LIBRARIES})
 

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/include/diff_drive_controller/rigid_body_motion.h

@@ -0,0 +1,105 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Clearpath Robotics, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the PAL Robotics nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Enrique Fernández
+ */
+
+#ifndef RIGID_BODY_MOTION_H_
+#define RIGID_BODY_MOTION_H_
+
+#include <Eigen/Dense>
+
+#include <limits>
+
+namespace diff_drive_controller
+{
+
+/**
+ * \brief Compute rigid body motion in SE(2) using element-wise equations.
+ * \param[in, out] x   Pose x component
+ * \param[in, out] y   Pose y component
+ * \param[in, out] yaw Pose yaw component
+ * \param[in] v_x   Velocity/Twist x   component
+ * \param[in] v_y   Velocity/Twist y   component
+ * \param[in] v_yaw Velocity/Twist yaw component
+ * \param[in] dt Time step
+ */
+static void integrate_motion(double& x, double &y, double &yaw,
+    const double v_x, const double v_y, const double v_yaw,
+    const double dt)
+{
+  const double cos_yaw = std::cos(yaw);
+  const double sin_yaw = std::sin(yaw);
+
+  x   += (v_x * cos_yaw - v_y * sin_yaw) * dt;
+  y   += (v_x * sin_yaw + v_y * cos_yaw) * dt;
+  yaw += v_yaw * dt;
+}
+
+/**
+ * \brief Compute rigid body motion in SE(2) using element-wise equations.
+ * Also computes the Jacobians wrt the pose and the velocity/twist.
+ * \param[in, out] x   Pose x component
+ * \param[in, out] y   Pose y component
+ * \param[in, out] yaw Pose yaw component
+ * \param[in] v_x   Velocity/Twist x   component
+ * \param[in] v_y   Velocity/Twist y   component
+ * \param[in] v_yaw Velocity/Twist yaw component
+ * \param[in] dt Time step
+ */
+static void integrate_motion(double& x, double &y, double &yaw,
+    const double v_x, const double v_y, const double v_yaw,
+    const double dt,
+    Eigen::Matrix3d& J_pose, Eigen::Matrix3d& J_twist)
+{
+  const double cos_yaw = std::cos(yaw);
+  const double sin_yaw = std::sin(yaw);
+
+  x   += (v_x * cos_yaw - v_y * sin_yaw) * dt;
+  y   += (v_x * sin_yaw + v_y * cos_yaw) * dt;
+  yaw += v_yaw * dt;
+
+  J_pose << 1.0, 0.0, (-v_x * sin_yaw - v_y * cos_yaw) * dt,
+            0.0, 1.0, ( v_x * cos_yaw - v_y * sin_yaw) * dt,
+            0.0, 0.0,                                   1.0;
+
+  J_twist << cos_yaw * dt, -sin_yaw * dt, 0.0,
+             sin_yaw * dt,  cos_yaw * dt, 0.0,
+                      0.0,           0.0,  dt;
+}
+
+}
+
+#endif /* RIGID_BODY_MOTION_H_ */

diff_drive_controller/package.xml

@@ -46,6 +46,7 @@
   <!--Tests-->
   <build_depend>rostest</build_depend>
 
+  <test_depend>gtest</test_depend>
   <test_depend>std_srvs</test_depend>
   <test_depend>controller_manager</test_depend>
   <test_depend>control_toolbox</test_depend>

diff_drive_controller/src/diff_drive_controller.cpp

@@ -620,6 +620,12 @@ namespace diff_drive_controller
     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);
+
       last_odom_publish_time_ = time;
 
       // Populate odom message and publish:
@@ -646,6 +652,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: