From 9f611a679dce1c02cdedbef52f2b4fcf4a544da5 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Fri, 13 Sep 2024 08:01:28 +0200 Subject: [PATCH 1/3] feat(controller-server): `publish_zero_velocity` parameter For optionally publishing a zero velocity command reference on goal exit. Publishing a zero velocity is not desired when we are following consecutive path segments that end with a velocity. Signed-off-by: Rein Appeldoorn --- .../nav2_controller/controller_server.hpp | 5 +++ nav2_controller/src/controller_server.cpp | 31 ++++++++++++------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 0f250dee148..1d0c92109cb 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -176,6 +176,10 @@ class ControllerServer : public nav2_util::LifecycleNode * @brief Calls velocity publisher to publish zero velocity */ void publishZeroVelocity(); + /** + * @brief Called on goal exit + */ + void onGoalExit(); /** * @brief Checks if goal is reached * @return true or false @@ -267,6 +271,7 @@ class ControllerServer : public nav2_util::LifecycleNode double failure_tolerance_; bool use_realtime_priority_; + bool publish_zero_velocity_; // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8ea2d9cfb1b..be1fdb66ad9 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -63,6 +63,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); + declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -128,6 +129,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) get_parameter("speed_limit_topic", speed_limit_topic); get_parameter("failure_tolerance", failure_tolerance_); get_parameter("use_realtime_priority", use_realtime_priority_); + get_parameter("publish_zero_velocity", publish_zero_velocity_); costmap_ros_->configure(); // Launch a thread to run the costmap node @@ -292,7 +294,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) */ costmap_ros_->deactivate(); - publishZeroVelocity(); + onGoalExit(); vel_publisher_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); @@ -470,7 +472,7 @@ void ControllerServer::computeControl() if (controllers_[current_controller_]->cancel()) { RCLCPP_INFO(get_logger(), "Cancellation was successful. Stopping the robot."); action_server_->terminate_all(); - publishZeroVelocity(); + onGoalExit(); return; } else { RCLCPP_INFO_THROTTLE( @@ -503,56 +505,56 @@ void ControllerServer::computeControl() } } catch (nav2_core::InvalidController & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::INVALID_CONTROLLER; action_server_->terminate_current(result); return; } catch (nav2_core::ControllerTFError & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::TF_ERROR; action_server_->terminate_current(result); return; } catch (nav2_core::NoValidControl & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::NO_VALID_CONTROL; action_server_->terminate_current(result); return; } catch (nav2_core::FailedToMakeProgress & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::FAILED_TO_MAKE_PROGRESS; action_server_->terminate_current(result); return; } catch (nav2_core::PatienceExceeded & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::PATIENCE_EXCEEDED; action_server_->terminate_current(result); return; } catch (nav2_core::InvalidPath & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::INVALID_PATH; action_server_->terminate_current(result); return; } catch (nav2_core::ControllerException & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::UNKNOWN; action_server_->terminate_current(result); return; } catch (std::exception & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::UNKNOWN; action_server_->terminate_current(result); @@ -561,7 +563,7 @@ void ControllerServer::computeControl() RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result"); - publishZeroVelocity(); + onGoalExit(); // TODO(orduno) #861 Handle a pending preemption and set controller name action_server_->succeeded_current(); @@ -729,6 +731,13 @@ void ControllerServer::publishZeroVelocity() velocity.header.frame_id = costmap_ros_->getBaseFrameID(); velocity.header.stamp = now(); publishVelocity(velocity); +} + +void ControllerServer::onGoalExit() +{ + if (publish_zero_velocity_) { + publishZeroVelocity(); + } // Reset the state of the controllers after the task has ended ControllerMap::iterator it; From 6e5e0c3740d99efef8c8db49fb6e94e4fecdc951 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Tue, 17 Sep 2024 08:16:21 +0200 Subject: [PATCH 2/3] processed comments Signed-off-by: Rein Appeldoorn --- nav2_controller/src/controller_server.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ee3aa673f1c..74b3f3707b3 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -300,7 +300,10 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) */ costmap_ros_->deactivate(); - onGoalExit(); + publishZeroVelocity(); + for (auto & controller : controllers_) { + controller.second->reset(); + } vel_publisher_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); From ea1580818155187f1910eaac821f8fd30e210b57 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 18 Sep 2024 08:07:00 +0200 Subject: [PATCH 3/3] comments Steve Signed-off-by: Rein Appeldoorn --- nav2_controller/src/controller_server.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 74b3f3707b3..ec9dc3d81b8 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -301,9 +301,6 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) costmap_ros_->deactivate(); publishZeroVelocity(); - for (auto & controller : controllers_) { - controller.second->reset(); - } vel_publisher_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); @@ -560,7 +557,7 @@ void ControllerServer::computeControl() return; } catch (nav2_core::ControllerTimedOut & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::CONTROLLER_TIMED_OUT; action_server_->terminate_current(result);