feat(controller-server): publish_zero_velocity parameter (ros-navig…

…ation#4675) Backport of ros-navigation#4675 Signed-off-by: Rein Appeldoorn <[email protected]>
nobleo · Sep 24, 2024 · f72b994 · f72b994
1 parent 9f64098
commit f72b994
Showing 1 changed file with 24 additions and 11 deletions.
diff --git 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<nav2_costmap_2d::Costmap2DROS>(
@@ -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;