Adds support for replanning when a progress error is raised - #39
Conversation
This PR adds support for replanning when a progress error is raised. We also add similar nav2 support for handling such progress errors. Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
|
cc: @SamuelFoo |
|
One integration issue exposed by this replanning flow is that However, this is outside the changes in this PR. |
| if let Ok(inner_nav_client) = inner_nav_clients.get(target.agent) { | ||
| if let Some(active_goal) = inner_nav_client.goal() { | ||
| if active_goal.id() != &target.safe_zone_id { | ||
| debug!( | ||
| "[{:?}] Aborted goal is stale, not retrying", | ||
| if let Ok(publisher) = plan_error_publishers.get(target.agent) { | ||
| let plan_error = ros_env::rmf_prototype_msgs::msg::PlanError { | ||
| error: ros_env::rmf_prototype_msgs::msg::Error { | ||
| code: ros_env::rmf_prototype_msgs::msg::PlanError::CODE_PATH_BLOCKED, | ||
| message: format!( | ||
| "Nav2 goal aborted for agent {:?}: path blocked within safe zone", | ||
| target.agent.index() | ||
| ); | ||
| return Err(result); | ||
| } | ||
| ), | ||
| parameters: String::new(), | ||
| }, | ||
| plan_id: handle.request.safe_zone_id.plan_id.clone(), | ||
| }; | ||
| if let Err(e) = publisher.publisher.publish(plan_error) { | ||
| error!( | ||
| "Failed to publish PlanError for agent {:?}: {:?}", | ||
| target.agent.index(), | ||
| e | ||
| ); | ||
| } else { | ||
| warn!( | ||
| "[{:?}] Goal aborted by Nav2! Published PlanError CODE_PATH_BLOCKED to ~/plan/error.", | ||
| target.agent.index() | ||
| ); | ||
| } | ||
| } |
There was a problem hiding this comment.
Maybe we should retain the active-goal check before publishing CODE_PATH_BLOCKED? When a replan produces a newer SafeZone, the Nav2 bridge cancels the current goal and submits the new incremental target. The superseded goal may still report Aborted; treating that result as current triggers another replan and causes oscillation.
I made an integration that addresses this by marking replaced goals as superseded and checking the full SafeZoneId: SamuelFoo@f0da686
3 participants
New feature implementation
Implemented feature
This PR adds support for replanning when a progress error is raised. We also add similar nav2 support for handling such progress errors.
Implementation description
Rather than repeatedly retrying a location, this PR adds support for replanning when an error message is caught.
GenAI Use
We follow OSRA's policy on GenAI tools
Generated-by: