Feature request

Description

Currently, the onGoalExit() method in nav2_controller::ControllerServer only publishes a zero velocity if publish_zero_velocity is set to true. However, this conditional publishing is applied even in the case of controller failures and exceptions such as:

• NO_VALID_CONTROL
• INVALID_PATH
• TF_ERROR
• PATIENCE_EXCEEDED
• FAILED_TO_MAKE_PROGRESS
• etc.

This can lead to potentially unsafe conditions: if publish_zero_velocity is set to false, and a controller error occurs, no stop command is sent to the robot, and the robot might continue to move based on the last velocity command.

Expected behavior

A zero velocity command should always be published in cases of:

• controller failure
• goal cancellation
• exception during execution

regardless of the value of the publish_zero_velocity parameter.

The parameter publish_zero_velocity should only control stop behavior in normal successful goal completions.

Actual behavior

When publish_zero_velocity := false, and a controller exception is thrown or an error result is returned, the robot does not receive a zero velocity command because onGoalExit() skips publishZeroVelocity().

Proposed solution

Refactor onGoalExit() to accept a boolean flag that forces zero velocity publication in failure scenarios:

void ControllerServer::onGoalExit(bool force_stop)
{
  if (publish_zero_velocity_ || force_stop) {
    publishZeroVelocity();
  }

  // Reset controller state
  for (auto & controller : controllers_) {
    controller.second->reset();
  }
}

This behavior aligns better with safety expectations in robotics systems: a robot should always receive a stop command on abort, failure, or exception. Users who disable publish_zero_velocity for custom controllers might reasonably assume that failures will still trigger a stop for safety reasons — which is currently not the case.

This change will help prevent unintended robot motion and aligns the system behavior with intuitive safety-critical expectations.