From f72b99444ae4dd30581a196888c4bde337e919d6 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Tue, 24 Sep 2024 14:31:10 +0200 Subject: [PATCH 1/2] feat(controller-server): `publish_zero_velocity` parameter (#4675) Backport of https://github.com/ros-navigation/navigation2/pull/4675 Signed-off-by: Rein Appeldoorn --- nav2_controller/src/controller_server.cpp | 35 ++++++++++++++++------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8ea2d9cfb1b..02c12cc537b 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( @@ -292,7 +293,17 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) */ costmap_ros_->deactivate(); - publishZeroVelocity(); + geometry_msgs::msg::TwistStamped velocity; + velocity.twist.angular.x = 0; + velocity.twist.angular.y = 0; + velocity.twist.angular.z = 0; + velocity.twist.linear.x = 0; + velocity.twist.linear.y = 0; + velocity.twist.linear.z = 0; + velocity.header.frame_id = costmap_ros_->getBaseFrameID(); + velocity.header.stamp = now(); + publishVelocity(velocity); + vel_publisher_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); @@ -719,16 +730,18 @@ void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & void ControllerServer::publishZeroVelocity() { - geometry_msgs::msg::TwistStamped velocity; - velocity.twist.angular.x = 0; - velocity.twist.angular.y = 0; - velocity.twist.angular.z = 0; - velocity.twist.linear.x = 0; - velocity.twist.linear.y = 0; - velocity.twist.linear.z = 0; - velocity.header.frame_id = costmap_ros_->getBaseFrameID(); - velocity.header.stamp = now(); - publishVelocity(velocity); + if (get_parameter("publish_zero_velocity").as_bool()) { + geometry_msgs::msg::TwistStamped velocity; + velocity.twist.angular.x = 0; + velocity.twist.angular.y = 0; + velocity.twist.angular.z = 0; + velocity.twist.linear.x = 0; + velocity.twist.linear.y = 0; + velocity.twist.linear.z = 0; + velocity.header.frame_id = costmap_ros_->getBaseFrameID(); + velocity.header.stamp = now(); + publishVelocity(velocity); + } // Reset the state of the controllers after the task has ended ControllerMap::iterator it; From d976db1c25820c3cd1886ced4df40f9b73f23113 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 25 Sep 2024 07:49:04 +0200 Subject: [PATCH 2/2] Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski Signed-off-by: Rein Appeldoorn --- nav2_controller/src/controller_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 02c12cc537b..4f2636beeea 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -293,6 +293,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) */ costmap_ros_->deactivate(); + // Always publish a zero velocity when deactivating the controller server geometry_msgs::msg::TwistStamped velocity; velocity.twist.angular.x = 0; velocity.twist.angular.y = 0;