From 555ffb999d805aaa1398ec40d5c3095e308ed193 Mon Sep 17 00:00:00 2001 From: Franz Pucher Date: Fri, 30 Apr 2021 19:20:38 +0200 Subject: [PATCH] fix: reset encoders after each new launch of hardware interface --- diffbot_base/include/diffbot_base/diffbot_hw_interface.h | 5 ++++- diffbot_base/src/diffbot_hw_interface.cpp | 9 ++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/diffbot_base/include/diffbot_base/diffbot_hw_interface.h b/diffbot_base/include/diffbot_base/diffbot_hw_interface.h index 95e2abaa..ead49e0b 100644 --- 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 index 77700740..6e0eb039 100644 --- a/diffbot_base/src/diffbot_hw_interface.cpp +++ b/diffbot_base/src/diffbot_hw_interface.cpp @@ -5,6 +5,7 @@ //#include #include +#include #include @@ -42,6 +43,8 @@ namespace diffbot_base pub_left_motor_value_ = nh_.advertise("motor_left", 10); pub_right_motor_value_ = nh_.advertise("motor_right", 10); + // Setup publisher to reset wheel encoders (used during first launch of the hardware interface) + pub_reset_encoders_ = nh_.advertise("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); }