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

bektaskemal · 2024-09-30T13:43:42Z

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

Signed-off-by: Kemal Bektas <[email protected]>

codecov · 2024-09-30T14:12:32Z

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 · 2024-10-01T22:28:50Z

nav2_controller/src/controller_server.cpp

@@ -206,6 +208,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
      RCLCPP_FATAL(
        get_logger(),
        "Failed to create controller. Exception: %s", ex.what());
+      on_cleanup(state);


I think L248 also needs one

SteveMacenski · 2024-10-01T22:29:46Z

nav2_controller/src/controller_server.cpp

@@ -265,6 +268,15 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
  ControllerMap::iterator it;
  for (it = controllers_.begin(); it != controllers_.end(); ++it) {
    it->second->activate();


I think you forgot to remove this line

Also, can activate throw? If not, then we can remove this code change

SteveMacenski · 2024-10-01T22:30:20Z

nav2_planner/src/planner_server.cpp

@@ -191,7 +192,15 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)

  PlannerMap::iterator it;
  for (it = planners_.begin(); it != planners_.end(); ++it) {
-    it->second->activate();
+    try {
+      it->second->activate();


SteveMacenski · 2024-10-01T22:30:53Z

nav2_smoother/src/nav2_smoother.cpp

 {
  RCLCPP_INFO(get_logger(), "Activating");

  plan_publisher_->on_activate();
  SmootherMap::iterator it;
  for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
-    it->second->activate();
+    try {
+      it->second->activate();


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

0ae7c08

Signed-off-by: Kemal Bektas <[email protected]>

SteveMacenski requested changes Oct 1, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

bektaskemal commented Sep 30, 2024

codecov bot commented Sep 30, 2024 •

edited

Loading

SteveMacenski left a comment

SteveMacenski Oct 1, 2024

SteveMacenski Oct 1, 2024

SteveMacenski Oct 1, 2024

SteveMacenski Oct 1, 2024

	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.");
	}

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

Are you sure you want to change the base?

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

Conversation

bektaskemal commented Sep 30, 2024

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

codecov bot commented Sep 30, 2024 • edited Loading

Codecov Report

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Oct 1, 2024

Choose a reason for hiding this comment

SteveMacenski Oct 1, 2024

Choose a reason for hiding this comment

SteveMacenski Oct 1, 2024

Choose a reason for hiding this comment

SteveMacenski Oct 1, 2024

Choose a reason for hiding this comment

codecov bot commented Sep 30, 2024 •

edited

Loading