diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index dba72182fe..1a13eb00c1 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -103,6 +103,11 @@ class RotationShimController : public nav2_core::Controller */ void setSpeedLimit(const double & speed_limit, const bool & percentage) override; + /** + * @brief Reset the state of the controller + */ + void reset() override; + protected: /** * @brief Finds the point on the path that is roughly the sampling diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 2abddb10a3..1d8e3d5da6 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -388,6 +388,12 @@ void RotationShimController::setSpeedLimit(const double & speed_limit, const boo primary_controller_->setSpeedLimit(speed_limit, percentage); } +void RotationShimController::reset() +{ + last_angular_vel_ = std::numeric_limits::max(); + primary_controller_->reset(); +} + rcl_interfaces::msg::SetParametersResult RotationShimController::dynamicParametersCallback(std::vector parameters) {