add service to reset odometry

ros-controls · May 12, 2021 · a9d5c21 · a9d5c21
1 parent 9bbdedd
commit a9d5c21
Show file tree

Hide file tree

Showing 4 changed files with 42 additions and 0 deletions.
diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
@@ -53,6 +53,7 @@
 #include <realtime_tools/realtime_buffer.h>
 #include <realtime_tools/realtime_publisher.h>
 #include <tf/tfMessage.h>
+#include <std_srvs/Trigger.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::ServiceServer reset_odometry_srv_;
     Odometry odometry_;
 
     /// Controller state publisher
@@ -249,6 +251,11 @@ namespace diff_drive_controller{
      */
     void cmdVelCallback(const geometry_msgs::Twist& command);
 
+    /**
+     * \brief Reset Odometry internal state callback
+     */
+    bool resetOdometryCallback(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res);
+
     /**
      * \brief Get the wheel names from a wheel param
      * \param [in]  controller_nh Controller node handler

diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h
@@ -153,6 +153,11 @@ namespace diff_drive_controller
      */
     void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
 
+    /**
+     * \brief Reset Odometry internal state
+     */
+    void resetInternalState();
+
   private:
 
     /// Rolling mean accumulator and window:
@@ -199,6 +204,8 @@ namespace diff_drive_controller
     double left_wheel_old_pos_;
     double right_wheel_old_pos_;
 
+    bool first_data_;
+
     /// Rolling mean accumulators for the linar and angular velocities:
     size_t velocity_rolling_window_size_;
     RollingMeanAcc linear_acc_;

diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -353,6 +353,7 @@ namespace diff_drive_controller{
     }
 
     sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this);
+    reset_odometry_srv_ = controller_nh.advertiseService("/reset_odometry", &DiffDriveController::resetOdometryCallback, this);
 
     // Initialize dynamic parameters
     DynamicParams dynamic_params;
@@ -831,4 +832,12 @@ namespace diff_drive_controller{
     }
   }
 
+  bool DiffDriveController::resetOdometryCallback(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
+  {
+    brake();
+    odometry_.resetInternalState();
+    res.success = true;
+    return true;
+  }
+
 } // namespace diff_drive_controller
diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp
@@ -59,6 +59,7 @@ namespace diff_drive_controller
   , right_wheel_radius_(0.0)
   , left_wheel_old_pos_(0.0)
   , right_wheel_old_pos_(0.0)
+  , first_data_(true)
   , 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)
@@ -75,6 +76,15 @@ namespace diff_drive_controller
 
   bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
   {
+    if(first_data_)
+    {
+      left_wheel_old_pos_  = left_pos  * left_wheel_radius_;
+      right_wheel_old_pos_ = right_pos * right_wheel_radius_;
+      timestamp_ = time;
+      first_data_ = false;
+      return true;
+    }
+
     /// Get current wheel joint positions:
     const double left_wheel_cur_pos  = left_pos  * left_wheel_radius_;
     const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
@@ -173,4 +183,13 @@ namespace diff_drive_controller
     angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
   }
 
+  void Odometry::resetInternalState()
+  {
+    resetAccumulators();
+    x_ = 0.0;
+    y_ = 0.0;
+    heading_ = 0.0;
+    first_data_ = true;
+  }
+
 } // namespace diff_drive_controller