fix: reset encoders after each new launch of hardware interface

ros-mobile-robots · Apr 30, 2021 · 555ffb9 · 555ffb9
1 parent afe7fbd
commit 555ffb9
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 2 deletions.
diff --git a/diffbot_base/include/diffbot_base/diffbot_hw_interface.h b/diffbot_base/include/diffbot_base/diffbot_hw_interface.h
@@ -96,7 +96,7 @@ namespace diffbot_base
          *
          * \param timeout Minimum time to wait for receiving encoder ticks
          */
-        bool isReceivingEncoderTicks(const ros::Duration &timeout=ros::Duration(1)) const;
+        bool isReceivingEncoderTicks(const ros::Duration &timeout=ros::Duration(1));
 
         /** \brief Helper for debugging a joint's state */
         virtual void printState();
@@ -178,6 +178,9 @@ namespace diffbot_base
         ros::Publisher pub_left_motor_value_;
         ros::Publisher pub_right_motor_value_;
 
+        // Declare publisher to reset the wheel encoders
+	// used during first launch of hardware interface to avoid large difference in encoder ticks from a previous run
+        ros::Publisher pub_reset_encoders_;
         // Declare subscriber for the wheel encoders
         // This subscriber receives the encoder ticks in the custom diffbot_msgs/Encoder message
         ros::Subscriber sub_encoder_ticks_;

diff --git a/diffbot_base/src/diffbot_hw_interface.cpp b/diffbot_base/src/diffbot_hw_interface.cpp
@@ -5,6 +5,7 @@
 
 //#include <std_msgs/Float32.h>
 #include <std_msgs/Int32.h>
+#include <std_msgs/Empty.h>
 
 #include <iomanip>
 
@@ -42,6 +43,8 @@ namespace diffbot_base
         pub_left_motor_value_ = nh_.advertise<std_msgs::Int32>("motor_left", 10);
         pub_right_motor_value_ = nh_.advertise<std_msgs::Int32>("motor_right", 10);
 
+        // Setup publisher to reset wheel encoders (used during first launch of the hardware interface)
+        pub_reset_encoders_ = nh_.advertise<std_msgs::Empty>("reset", 10);
         // Setup subscriber for the wheel encoders
         sub_encoder_ticks_ = nh_.subscribe("encoder_ticks", 10, &DiffBotHWInterface::encoderTicksCallback, this);
 
@@ -212,7 +215,7 @@ namespace diffbot_base
         ROS_INFO_STREAM(std::endl << ss.str());
     }
 
-    bool DiffBotHWInterface::isReceivingEncoderTicks(const ros::Duration &timeout) const
+    bool DiffBotHWInterface::isReceivingEncoderTicks(const ros::Duration &timeout)
     {
         ROS_INFO("Get number of encoder ticks publishers");
 
@@ -233,6 +236,10 @@ namespace diffbot_base
             ROS_INFO_STREAM("Number of encoder ticks publishers: " << num_publishers);
         }
 
+        ROS_INFO("Publish /reset to encoders");
+        std_msgs::Empty msg;
+        pub_reset_encoders_.publish(msg);
+
         return (num_publishers > 0);
     }