From ef1330f37ff37b9f75027a16cdc18f0ad10287af Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 25 Sep 2024 20:40:34 +0200 Subject: [PATCH] feat(controller-server): `publish_zero_velocity` parameter jazzy backport (#4687) * feat(controller-server): `publish_zero_velocity` parameter (#4675) Backport of https://github.com/ros-navigation/navigation2/pull/4675 Signed-off-by: Rein Appeldoorn * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski Signed-off-by: Rein Appeldoorn --------- Signed-off-by: Rein Appeldoorn Signed-off-by: Rein Appeldoorn Co-authored-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 36 ++++++++++++++++------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8ea2d9cfb1..4f2636beee 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,18 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) */ costmap_ros_->deactivate(); - publishZeroVelocity(); + // 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; + 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 +731,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;