Improve BT navigation failure robustness and diagnostics#6245
Conversation
Audit and harden failure propagation in Behavior Tree navigation: reject bad-TF preemption goals instead of aborting active navigation, flush terminal BT status transitions, populate error codes/messages on BT action node failures, add explicit and rate-limited logging for recovery retries and preempt-without-pending-goal cases, and extend BT/system tests to cover these paths. No external ROS interfaces (actions, services, topics, params) or BT plugin base-class APIs are changed.
|
Please fill in the PR template properly and sign off on DCO. I also see that your PR doesn't build, which looks like your fork is very out of date and needs to be rebased or pulling in I will give this a short initial glance though |
There was a problem hiding this comment.
Remove the AI generated file.
| RCLCPP_ERROR(logger_, "Robot pose is not available."); | ||
| RCLCPP_WARN_THROTTLE( | ||
| logger_, *clock_, 2000, | ||
| "Robot pose is not available for NavigateToPose feedback. " | ||
| "This is often caused by delayed TF or intermittent localization."); |
There was a problem hiding this comment.
This should remain an Error. Additionally, you should remove the "This is often caused.." as this is a sophisticated issue that I would not want to lead users down unfruitful paths. I understand the reason to throttle though, but it should definitely not be throttled any lower than 1hz
| RCLCPP_ERROR(logger_, "Robot pose is not available."); | ||
| RCLCPP_WARN_THROTTLE( | ||
| logger_, *clock_, 2000, | ||
| "Robot pose is not available for NavigateThroughPoses feedback. " | ||
| "This is often caused by delayed TF or intermittent localization."); |
| if (!pending_goal) { | ||
| RCLCPP_WARN_THROTTLE( | ||
| logger_, *clock_, 2000, | ||
| "Preempt requested for %s, but no pending goal is available. Ignoring preempt.", | ||
| action_name_.c_str()); | ||
| } else { |
There was a problem hiding this comment.
Is this a real case you ran into?
| if (result == BT::NodeStatus::FAILURE) { | ||
| RCLCPP_WARN( | ||
| rclcpp::get_logger("BehaviorTreeEngine"), | ||
| "Behavior Tree completed with FAILURE status at root node '%s'.", | ||
| tree->rootNode() ? tree->rootNode()->name().c_str() : "<unknown>"); | ||
| } else if (result == BT::NodeStatus::IDLE) { | ||
| RCLCPP_ERROR( | ||
| rclcpp::get_logger("BehaviorTreeEngine"), | ||
| "Behavior Tree returned unexpected IDLE status at root node '%s'. Treating as failure.", | ||
| tree->rootNode() ? tree->rootNode()->name().c_str() : "<unknown>"); | ||
| } | ||
|
|
There was a problem hiding this comment.
In such cases, were there not logging / warnings anywhere otherwise listed that make this duplicate?
There was a problem hiding this comment.
Discussion point: in this file, I'm cautious to make exceeding retries as an "error" case. This could be used in cases where the recovery case is not really an 'error' and logging like this would imply its use. I agree with adding some debug logging though if you think its helpful. Maybe the error cases should either be infos or also debugs.
2 participants
Summary
Validation