Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Conversation

bektaskemal
Copy link
Contributor


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

Copy link

codecov bot commented Sep 30, 2024

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

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


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

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!

nav2_controller/src/controller_server.cpp Show resolved Hide resolved
nav2_controller/src/controller_server.cpp Show resolved Hide resolved
nav2_planner/src/planner_server.cpp Show resolved Hide resolved
nav2_smoother/src/nav2_smoother.cpp Show resolved Hide resolved
@SteveMacenski
Copy link
Member

@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
Copy link
Member

Continued in #4708

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants