fix: handle transition failures in planner/controller/smoother servers

[bektaskemal]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4678
Primary OS tested on	Ubuntu
Robotic platform tested on	Sim
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Cleans up if configure fails, deactivates if activate fails.

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

Attention: Patch coverage is 43.47826% with 13 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_controller/src/controller_server.cpp	33.33%	6 Missing ⚠️
nav2_planner/src/planner_server.cpp	42.85%	4 Missing ⚠️
nav2_smoother/src/nav2_smoother.cpp	57.14%	3 Missing ⚠️

Flag	Coverage Δ
	89.45% <43.47%> (-0.10%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
nav2_smoother/src/nav2_smoother.cpp	96.29% <57.14%> (-1.82%)	⬇️
nav2_planner/src/planner_server.cpp	90.43% <42.85%> (-1.14%)	⬇️
nav2_controller/src/controller_server.cpp	83.24% <33.33%> (-1.25%)	⬇️

... and 5 files with indirect coverage changes

[SteveMacenski]

A couple more I think we should do to complete this task (lines to add the deactivate or cleanup to):

• navigation2/nav2_waypoint_follower/src/waypoint_follower.cpp

  Line 125 in 3233c84

  "Failed to create waypoint_task_executor. Exception: %s", e.what());

• navigation2/nav2_bt_navigator/src/bt_navigator.cpp

  Line 136 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

• navigation2/nav2_bt_navigator/src/bt_navigator.cpp

  Line 149 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

• navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp

  Line 63 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

• navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp

  Line 93 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

• navigation2/nav2_docking/opennav_docking/src/docking_server.cpp

  Line 93 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

• navigation2/nav2_behaviors/src/behavior_server.cpp

  Line 94 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

• navigation2/nav2_collision_monitor/src/collision_detector_node.cpp

  Line 63 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

• navigation2/nav2_velocity_smoother/src/velocity_smoother.cpp

  Line 147 in 3233c84

  return nav2_util::CallbackReturn::FAILURE;

---

Not a requirement, but this file has a number of throws which I think should instead be FAILUREs with your added lifecycle cleanup

navigation2/nav2_velocity_smoother/src/velocity_smoother.cpp

Lines 77 to 128 in 3233c84

	for (unsigned int i = 0; i != 3; i++) {
	if (max_decels_[i] > 0.0) {
	throw std::runtime_error(
	"Positive values set of deceleration! These should be negative to slow down!");
	}
	if (max_accels_[i] < 0.0) {
	throw std::runtime_error(
	"Negative values set of acceleration! These should be positive to speed up!");
	}
	if (min_velocities_[i] > 0.0) {
	throw std::runtime_error(
	"Positive values set of min_velocities! These should be negative!");
	}
	if (max_velocities_[i] < 0.0) {
	throw std::runtime_error(
	"Negative values set of max_velocities! These should be positive!");
	}
	if (min_velocities_[i] > max_velocities_[i]) {
	throw std::runtime_error(
	"Min velocities are higher than max velocities!");
	}
	}
	// Get feature parameters
	declare_parameter_if_not_declared(node, "odom_topic", rclcpp::ParameterValue("odom"));
	declare_parameter_if_not_declared(node, "odom_duration", rclcpp::ParameterValue(0.1));
	declare_parameter_if_not_declared(
	node, "deadband_velocity", rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
	declare_parameter_if_not_declared(node, "velocity_timeout", rclcpp::ParameterValue(1.0));
	node->get_parameter("odom_topic", odom_topic_);
	node->get_parameter("odom_duration", odom_duration_);
	node->get_parameter("deadband_velocity", deadband_velocities_);
	node->get_parameter("velocity_timeout", velocity_timeout_dbl);
	velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl);
	if (max_velocities_.size() != 3 || min_velocities_.size() != 3 ||
	max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3)
	{
	throw std::runtime_error(
	"Invalid setting of kinematic and/or deadband limits!"
	" All limits must be size of 3 representing (x, y, theta).");
	}
	// Get control type
	if (feedback_type == "OPEN_LOOP") {
	open_loop_ = true;
	} else if (feedback_type == "CLOSED_LOOP") {
	open_loop_ = false;
	odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration_, odom_topic_);
	} else {
	throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP.");
	}

. If you'd be open to doing that as well, that would be a great help!

[SteveMacenski]

@bektaskemal thanks for getting the ball rolling on this -- I'll finish up the changes from here since I kind of gave you a laundry list

[SteveMacenski]

Continued in #4708