-
Notifications
You must be signed in to change notification settings - Fork 1.3k
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
base: main
Are you sure you want to change the base?
fix: handle transition failures in planner/controller/smoother servers #4697
Conversation
Signed-off-by: Kemal Bektas <[email protected]>
Codecov ReportAttention: Patch coverage is
Flags with carried forward coverage won't be shown. Click here to find out more.
|
There was a problem hiding this 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):
"Failed to create waypoint_task_executor. Exception: %s", e.what()); return nav2_util::CallbackReturn::FAILURE; return nav2_util::CallbackReturn::FAILURE; return nav2_util::CallbackReturn::FAILURE; return nav2_util::CallbackReturn::FAILURE; return nav2_util::CallbackReturn::FAILURE; return nav2_util::CallbackReturn::FAILURE; return nav2_util::CallbackReturn::FAILURE; return nav2_util::CallbackReturn::FAILURE;
Not a requirement, but this file has a number of throw
s which I think should instead be FAILURE
s 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."); | |
} |
@@ -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); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think L248 also needs one
@@ -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(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think you forgot to remove this line
Also, can activate
throw? If not, then we can remove this code change
@@ -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(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ditto here
{ | ||
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(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ditto
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: