feat(controller-server): publish_zero_velocity parameter jazzy back…

…port (#4687)

* feat(controller-server): `publish_zero_velocity` parameter (#4675)

Backport of #4675

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>

* Update nav2_controller/src/controller_server.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Rein Appeldoorn <reinzor@gmail.com>

---------

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
Signed-off-by: Rein Appeldoorn <reinzor@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

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,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;