From 1231672e5b65969c9bbe5f5bf73d3fda064354fa Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Costa Silva Date: Thu, 29 Apr 2021 16:58:19 +0200 Subject: [PATCH 1/2] add service to reset odometry --- .../diff_drive_controller.h | 7 +++++++ .../include/diff_drive_controller/odometry.h | 7 +++++++ .../src/diff_drive_controller.cpp | 9 +++++++++ diff_drive_controller/src/odometry.cpp | 19 +++++++++++++++++++ 4 files changed, 42 insertions(+) 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 index 806246418..4b60b6a04 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -52,6 +52,7 @@ #include #include #include +#include #include namespace diff_drive_controller{ @@ -142,6 +143,7 @@ namespace diff_drive_controller{ /// Odometry related: std::shared_ptr > odom_pub_; std::shared_ptr > 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 index 39a5eb820..02983e106 100644 --- 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 index 60bf846fa..1a52008fc 100644 --- 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 index d086d5ccf..d34cb7222 100644 --- 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 From e660d57350d18ed4db72b7c714e430815ef77d74 Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Costa Silva Date: Mon, 23 Aug 2021 10:58:53 +0200 Subject: [PATCH 2/2] expose srv as a topic to avoid blocking the controller --- .../diff_drive_controller.h | 9 ++++++--- .../include/diff_drive_controller/odometry.h | 7 +++++-- .../src/diff_drive_controller.cpp | 17 +++++++++++------ diff_drive_controller/src/odometry.cpp | 16 ++++------------ 4 files changed, 26 insertions(+), 23 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 index 4b60b6a04..b9d9cfccd 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include namespace diff_drive_controller{ @@ -143,7 +143,7 @@ namespace diff_drive_controller{ /// Odometry related: std::shared_ptr > odom_pub_; std::shared_ptr > tf_odom_pub_; - ros::ServiceServer reset_odometry_srv_; + ros::Subscriber sub_reset_odometry_; Odometry odometry_; /// Controller state publisher @@ -190,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 @@ -254,7 +257,7 @@ namespace diff_drive_controller{ /** * \brief Reset Odometry internal state callback */ - bool resetOdometryCallback(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res); + void resetOdometryCallback(const std_msgs::Bool::ConstPtr& msg); /** * \brief Get the wheel names from a wheel param diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h index 02983e106..19ea60434 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.h +++ b/diff_drive_controller/include/diff_drive_controller/odometry.h @@ -155,8 +155,11 @@ namespace diff_drive_controller /** * \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(); + void resetInternalState(double left_pos, double right_pos, const ros::Time &time); private: @@ -204,7 +207,7 @@ namespace diff_drive_controller double left_wheel_old_pos_; double right_wheel_old_pos_; - bool first_data_; + bool reset_odometry_; /// Rolling mean accumulators for the linar and angular velocities: size_t velocity_rolling_window_size_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1a52008fc..b109851b8 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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,7 +354,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); + sub_reset_odometry_ = controller_nh.subscribe("reset_odometry", 1, &DiffDriveController::resetOdometryCallback, this); // Initialize dynamic parameters DynamicParams dynamic_params; @@ -422,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); } @@ -832,12 +840,9 @@ namespace diff_drive_controller{ } } - bool DiffDriveController::resetOdometryCallback(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) + void DiffDriveController::resetOdometryCallback(const std_msgs::Bool::ConstPtr& msg) { - brake(); - odometry_.resetInternalState(); - res.success = true; - return true; + reset_odometry_ = msg->data; } } // namespace diff_drive_controller diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index d34cb7222..878026b33 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -59,7 +59,6 @@ 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) @@ -76,15 +75,6 @@ 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_; @@ -183,13 +173,15 @@ namespace diff_drive_controller angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); } - void Odometry::resetInternalState() + void Odometry::resetInternalState(double left_pos, double right_pos, const ros::Time &time) { resetAccumulators(); x_ = 0.0; y_ = 0.0; heading_ = 0.0; - first_data_ = true; + left_wheel_old_pos_ = left_pos * left_wheel_radius_; + right_wheel_old_pos_ = right_pos * right_wheel_radius_; + timestamp_ = time; } } // namespace diff_drive_controller