Skip to content

fix(nav2_integration): add sequence-numbered costmap handshake and diect goal preemption #73

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.

Sign up for GitHub

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

Open
arjo129 wants to merge 1 commit into from
Open

fix(nav2_integration): add sequence-numbered costmap handshake and diect goal preemption #73

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions bevy_ros2/src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,10 @@ impl<T: MessageIDL + Debug> RosSubscription<T> {
pub fn data_callback(&self) -> Option<T> {
self.data.lock().unwrap().as_ref().cloned()
}

pub fn take_data(&self) -> Option<T> {
self.data.lock().unwrap().take()
}
}

// Template for creating ROS 2 publishers
Expand Down
48 changes: 28 additions & 20 deletions nav2_integration/rmf_nav2_traffic/src/agent.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
use crate::{
navigation_server::CurrentNavigationRequest,
inner_navigation_client::InnerNavigationClient,
navigation_server::{AgentPose, CurrentNavigationRequest},
safe_zone::{
build_robot_status, CurrentSafeZone, LatestProgress, ProgressPublisher, ProgressTracker,
RobotStatusPublisher,
Expand Down Expand Up @@ -97,9 +98,8 @@ pub struct Nav2AgentPlugin {}
impl Plugin for Nav2AgentPlugin {
fn build(&self, app: &mut App) {
app.init_resource::<DiscoveryPublisher>()
.add_systems(PreUpdate, update_amcl_pose)
.add_observer(create_amcl_pose_subscriber)
.add_observer(publish_discovery);
.add_systems(PreUpdate, (publish_discovery, update_amcl_pose))
.add_observer(create_amcl_pose_subscriber);
}
}

Expand Down Expand Up @@ -133,7 +133,9 @@ fn create_amcl_pose_subscriber(
}

fn update_amcl_pose(
mut commands: Commands,
mut agents: Query<(
Entity,
&mut Nav2Agent,
&mut AmclPose,
&AmclPoseSubscription,
Expand All @@ -144,9 +146,11 @@ fn update_amcl_pose(
Option<&ProgressTracker>,
Option<&CurrentSafeZone>,
Option<&CurrentNavigationRequest>,
Option<&InnerNavigationClient>,
)>,
) {
for (
entity,
mut agent,
mut amcl_pose,
amcl_pose_sub,
Expand All @@ -157,15 +161,20 @@ fn update_amcl_pose(
progress_tracker,
current_safe_zone,
current_nav_req,
inner_nav_client,
) in agents.iter_mut()
{
if let Some(amcl_pose_msg) = amcl_pose_sub.subscriber.data_callback() {
amcl_pose.0 = amcl_pose_msg.clone();
agent.localized = true;
}
if !agent.localized {
let Some(amcl_pose_msg) = amcl_pose_sub.subscriber.take_data() else {
continue;
}
};
Comment on lines +167 to +169

@SamuelFoo SamuelFoo Oct 26, 2026 •
edited
Loading

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

I think skipping the status update here when there is no new AMCL pose may leave a stationary robot waiting indefinitely.

take_data() consumes the latest pose, so this path stops publishing status until another pose arrives. AMCL may not publish a new pose while the robot is stationary.

For example, A stops at a crossing to wait for B. B clears it and sends its status, but the executor recomputes A's release on A's own status callback. Without a new pose, A may keep waiting even though the crossing is clear.

Could we keep publishing status from the cached pose while preserving its timestamp? Alternatively, the executor could recompute A’s release when B’s progress changes. The continuous ground-truth pose stream in #74 may hide this, so it would be worth checking with AMCL.

amcl_pose.0 = amcl_pose_msg;
agent.localized = true;

let pose = PoseStamped {
header: amcl_pose.0.header.clone(),
pose: amcl_pose.0.pose.pose.clone(),
};
commands.entity(entity).insert(AgentPose(pose.clone()));

let mut odom = Odometry::default();
odom.header = amcl_pose.0.header.clone();
Expand All @@ -174,11 +183,9 @@ fn update_amcl_pose(
odom.twist = TwistWithCovariance::default();
let _ = odom_pub.publisher.publish(odom);

let current_pose = Some(amcl_pose.xy_yaw());
if let (Some(tracker), Some(ref mut latest_prog)) =
(progress_tracker, latest_progress.as_mut())
{
if let (Some(tracker), Some(latest_prog)) = (progress_tracker, latest_progress.as_mut()) {
if tracker.plan_id.is_some() {
let current_pose = Some(amcl_pose.xy_yaw());
let updated_prog = tracker.compute_progress(current_pose, latest_prog.0.as_ref());
if latest_prog.0.as_ref() != Some(&updated_prog) {
if let Some(prog_pub) = progress_pub {
Expand All @@ -190,11 +197,9 @@ fn update_amcl_pose(
}

if let Some(status_pub) = status_pub {
let pose = PoseStamped {
header: amcl_pose.0.header.clone(),
pose: amcl_pose.0.pose.pose.clone(),
};
let mode = if current_nav_req.is_some() {
let mode = if current_nav_req.is_some()
|| inner_nav_client.is_some_and(|c| c.goal().is_some())
{
RobotStatus::MODE_NAVIGATING
} else {
RobotStatus::MODE_IDLE
Expand All @@ -208,10 +213,13 @@ fn update_amcl_pose(
}

fn publish_discovery(
_trigger: Trigger<OnAdd, Nav2Agent>,
added: Query<(), Added<Nav2Agent>>,
publisher: Res<DiscoveryPublisher>,
agents: Query<&Nav2Agent>,
) {
if added.is_empty() {
return;
}
let mut msg = ParticipantList::default();
for agent in agents.iter() {
msg.participants.push(Participant {
Expand Down
115 changes: 60 additions & 55 deletions nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ use ros_env::{
builtin_interfaces::msg::Time as RosTime,
geometry_msgs::msg::{Point, Pose, PoseStamped, Quaternion},
nav2_msgs::action::{NavigateToPose, NavigateToPose_Feedback, NavigateToPose_Goal},
rmf_prototype_msgs::msg::SafeZoneId,
rmf_prototype_msgs::msg::{RobotStatus, SafeZoneId},
std_msgs::msg::Header,
};
use std::{future::Future, sync::Arc};
Expand Down Expand Up @@ -426,6 +426,7 @@ fn await_external_cancellation(
}
}

#[allow(dead_code)]
enum CheckExistingGoalResult {
ReplanAndCancel(CancelInnerNavigation),
NoExistingGoal(InnerNavigationRequest),
Expand Down Expand Up @@ -470,16 +471,13 @@ fn check_existing_goal(
}

if replan_and_cancel {
// Do not treat cancellation of this goal as a new blockage.
// Do not treat preemption of this goal as a new blockage.
// Send the new goal directly so Nav2's SimpleActionServer smoothly
// preempts the active goal in accept_pending_goal() without calling
// terminate_all() (which would abort a pending new goal if cancel() is
// still in flight).
existing_goal.mark_superseded();
let client = existing_goal.client().clone();
return Ok(CheckExistingGoalResult::ReplanAndCancel(
CancelInnerNavigation {
agent: request.agent,
new_request: Some(request),
cancel_client: client,
},
));
return Ok(CheckExistingGoalResult::NoExistingGoal(request));
}

// There is an existing goal but the new request is duplicate or outdated,
Expand Down Expand Up @@ -745,58 +743,65 @@ fn process_navigation_result(
plan_error_publishers: Query<&PlanErrorPublisher>,
mut progress_query: Query<(
&ProgressTracker,
&mut LatestProgress,
&ProgressPublisher,
Option<&RobotStatusPublisher>,
Option<&AmclPose>,
&RobotStatusPublisher,
&mut LatestProgress,
Option<&CurrentSafeZone>,
Option<&AmclPose>,
)>,
) -> Result<InnerNavigationRequest, InnerNavigationResult> {
match result {
Ok(ref success) => {
let agent = success.handle.request.agent;
if let Ok((
tracker,
mut latest_prog,
prog_pub,
status_pub,
amcl_pose,
current_safe_zone,
)) = progress_query.get_mut(agent)
{
if tracker.plan_id.as_ref() == Some(&success.handle.request.safe_zone_id.plan_id) {
let current_pose = amcl_pose.map(|p| p.xy_yaw());
if !tracker.is_target_pose_satisfied(current_pose) {
warn!(
"[{:?}] Nav2 reported goal succeeded before target pose was satisfied (current={:?}); retrying goal {:?}.",
agent.index(),
current_pose,
success.handle.request.safe_zone_id,
);
return Ok(success.handle.request.clone());
}
if let Some(updated_prog) = tracker.mark_target_reached(latest_prog.0.as_ref())
{
if latest_prog.0.as_ref() != Some(&updated_prog) {
let _ = prog_pub.publisher.publish(updated_prog.clone());
latest_prog.0 = Some(updated_prog.clone());
if let Some(status_pub) = status_pub {
let pose = amcl_pose
.map(|p| PoseStamped {
header: p.0.header.clone(),
pose: p.0.pose.pose.clone(),
})
.unwrap_or_default();
let sz_id = current_safe_zone
.and_then(|csz| csz.0.as_ref().map(|sz| &sz.id));
let status_msg = build_robot_status(
pose,
ros_env::rmf_prototype_msgs::msg::RobotStatus::MODE_NAVIGATING,
Some(&updated_prog),
sz_id,
);
let _ = status_pub.publisher.publish(status_msg);
let target = &success.handle.request;
let is_current = inner_nav_clients
.get(target.agent)
.ok()
.and_then(|c| c.goal().as_ref())
.is_some_and(|goal| goal.id() == &target.safe_zone_id && !goal.is_superseded());
if is_current {
if let Ok((
tracker,
progress_pub,
status_pub,
mut latest_progress,
current_safe_zone,
amcl_pose,
)) = progress_query.get_mut(target.agent)
{
if tracker.plan_id.as_ref() == Some(&target.safe_zone_id.plan_id) {
let current_pose = amcl_pose.map(|p| p.xy_yaw());
if !tracker.is_target_pose_satisfied(current_pose) {
warn!(
"[{:?}] Nav2 reported goal succeeded before target pose was satisfied (current={:?}); retrying goal {:?}.",
target.agent.index(),
current_pose,
target.safe_zone_id,
);
return Ok(target.clone());
}
if let Some(reached_prog) =
tracker.mark_target_reached(latest_progress.0.as_ref())
{
if latest_progress.0.as_ref() != Some(&reached_prog) {
let _ = progress_pub.publisher.publish(reached_prog.clone());
latest_progress.0 = Some(reached_prog.clone());
}
let pose = amcl_pose
.map(|p| PoseStamped {
header: p.0.header.clone(),
pose: p.0.pose.pose.clone(),
})
.unwrap_or_default();
let sz_id = current_safe_zone
.and_then(|csz| csz.0.as_ref().map(|sz| &sz.id))
.or(Some(&target.safe_zone_id));
let status_msg = build_robot_status(
pose,
RobotStatus::MODE_NAVIGATING,
Some(&reached_prog),
sz_id,
);
let _ = status_pub.publisher.publish(status_msg);
}
}
}
Expand Down
8 changes: 6 additions & 2 deletions nav2_integration/rmf_nav2_traffic/src/main.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,9 +1,13 @@
use bevy::{log::LogPlugin, prelude::*};
use bevy::{app::ScheduleRunnerPlugin, log::LogPlugin, prelude::*};
use nav2_traffic::*;
use std::time::Duration;

fn main() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, LogPlugin::default()));
app.add_plugins((
MinimalPlugins.set(ScheduleRunnerPlugin::run_loop(Duration::from_millis(20))),
LogPlugin::default(),
));
app.add_plugins(Nav2TrafficPlugin::default());
app.run();
}
Loading
Loading