Add service to reset odometry

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -52,6 +52,7 @@
 #include <pluginlib/class_list_macros.hpp>
 #include <realtime_tools/realtime_buffer.h>
 #include <realtime_tools/realtime_publisher.h>
+#include <std_msgs/Bool.h>
 #include <tf/tfMessage.h>
 
 namespace diff_drive_controller{
@@ -142,6 +143,7 @@ namespace diff_drive_controller{
     /// Odometry related:
     std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
     std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
+    ros::Subscriber sub_reset_odometry_;
     Odometry odometry_;
 
     /// Controller state publisher
@@ -188,6 +190,9 @@ namespace diff_drive_controller{
     /// Publish wheel data:
     bool publish_wheel_joint_controller_state_;
 
+    /// Reset odometry state
+    bool reset_odometry_;
+
     // A struct to hold dynamic parameters
     // set from dynamic_reconfigure server
     struct DynamicParams
@@ -249,6 +254,11 @@ namespace diff_drive_controller{
      */
     void cmdVelCallback(const geometry_msgs::Twist& command);
 
+    /**
+     * \brief Reset Odometry internal state callback
+     */
+    void resetOdometryCallback(const std_msgs::Bool::ConstPtr& msg);
+
     /**
      * \brief Get the wheel names from a wheel param
      * \param [in]  controller_nh Controller node handler

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -153,6 +153,14 @@ namespace diff_drive_controller
      */
     void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
 
+    /**
+     * \brief Reset Odometry internal state
+    * \param left_pos  Left  wheel position [rad] in which the system will be reset to
+     * \param right_pos Right wheel position [rad] in which the system will be reset to
+     * \param time      Current time in which the system will be reset to
+     */
+    void resetInternalState(double left_pos, double right_pos, const ros::Time &time);
+
   private:
 
     /// Rolling mean accumulator and window:
@@ -199,6 +207,8 @@ namespace diff_drive_controller
     double left_wheel_old_pos_;
     double right_wheel_old_pos_;
 
+    bool reset_odometry_;
+
     /// Rolling mean accumulators for the linar and angular velocities:
     size_t velocity_rolling_window_size_;
     RollingMeanAcc linear_acc_;

diff_drive_controller/src/diff_drive_controller.cpp

@@ -159,6 +159,7 @@ namespace diff_drive_controller{
     , wheel_joints_size_(0)
     , publish_cmd_(false)
     , publish_wheel_joint_controller_state_(false)
+    , reset_odometry_(false)
   {
   }
 
@@ -353,6 +354,7 @@ namespace diff_drive_controller{
     }
 
     sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this);
+    sub_reset_odometry_ = controller_nh.subscribe("reset_odometry", 1, &DiffDriveController::resetOdometryCallback, this);
 
     // Initialize dynamic parameters
     DynamicParams dynamic_params;
@@ -421,6 +423,13 @@ namespace diff_drive_controller{
       right_pos /= wheel_joints_size_;
 
       // Estimate linear and angular velocity using joint information
+      if(reset_odometry_)
+      {
+        brake();
+        odometry_.resetInternalState(left_pos, right_pos, time);
+        reset_odometry_ = false;
+      }
+
       odometry_.update(left_pos, right_pos, time);
     }
 
@@ -831,4 +840,9 @@ namespace diff_drive_controller{
     }
   }
 
+  void DiffDriveController::resetOdometryCallback(const std_msgs::Bool::ConstPtr& msg)
+  {
+    reset_odometry_ = msg->data;
+  }
+
 } // namespace diff_drive_controller

diff_drive_controller/src/odometry.cpp

@@ -173,4 +173,15 @@ namespace diff_drive_controller
     angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
   }
 
+  void Odometry::resetInternalState(double left_pos, double right_pos, const ros::Time &time)
+  {
+    resetAccumulators();
+    x_ = 0.0;
+    y_ = 0.0;
+    heading_ = 0.0;
+    left_wheel_old_pos_  = left_pos  * left_wheel_radius_;
+    right_wheel_old_pos_ = right_pos * right_wheel_radius_;
+    timestamp_ = time;
+  }
+
 } // namespace diff_drive_controller