Repository navigation
fix(nav2_integration): add sequence-numbered costmap handshake and diect goal preemption - #73
Conversation
…rect goal preemption Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
703f5a5 to
817674c
Compare
| let Some(amcl_pose_msg) = amcl_pose_sub.subscriber.take_data() else { | ||
| continue; | ||
| } | ||
| }; |
There was a problem hiding this comment.
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.
Fixed bug
This PR fixes a synchronization
rmf_nav2_traffic, the spatio temporal costmap layer and nav2. Previously it was possible for the costmap to arrive after the nav2 goal. This would lead to frequent replanning.Fix applied
The current fix is far from ideal, but it essentially involves publishing a new
std_msgs/Headeracknowledging receipt of the costmap before sending the next goal.GenAI Use
We follow OSRA's policy on GenAI tools
Generated-by: Gemini 4 Argon