Feat/navigate through nodes#75
Open
afonsofonbraga wants to merge 33 commits into
Open
Conversation
…order Move the logic that filters already-visited node_states during stitch orders from a post-update block into the _update_state call itself. This avoids a redundant state update and simplifies the control flow.
Re-add the STATUS_ABORTED check that was accidentally removed in 8f164e2. Extract _handle_navigation_failure() as a shared method used by both _navigate_to_node_result_callback and the NTN callback.
The result callback's while loop consumed ALL released edges, including ones added by a stitch order that arrived during navigation. This caused the controller to skip nodes the robot hadn't physically reached. Track the last sequence_id sent to the handler and stop consuming edges/nodes beyond that boundary.
Both NTN tests were calling _navigate_to_node_result_callback instead of _navigate_through_nodes_result_callback.
- Remove default value for nav_through_nodes.handler parameter - Only check nav_through_nodes driving state when rejecting goals
- Only create the action client for the active navigation mode - Simplify cancel order to branch on enable_nav_through_nodes - Simplify _is_navigation_active to check only the active mode - Skip order processing retry while errors are active - Clear current node goal on navigation failure - Remove stale goal re-dispatch logic - Remove redundant canceling_order check in NTN result callback
Replace independent _get_drivable_edges() and _get_drivable_nodes() with a single _get_drivable_segment() that walks edge-node pairs together, guaranteeing len(edges) == len(nodes). Each edge is matched to its destination node via sequence_id + 1 (VDA5050 §6.1.1). The segment stops on the first HARD/SOFT action from either the edge or its destination node (§6.2.2), or when a non-released edge/node is encountered.
…ption - Add _nav_through_nodes_goal_pending flag to prevent goal rejection spam between send_goal_async and response callback - Fix feedback callback to consume all intermediate nodes with while loop instead of processing only one node per callback - Guard _is_navigation_active() to check pending flag
…support Add ExtendNavigation.srv for extending in-flight navigation goals with new edges/nodes. The request includes a stitch reference node (nodes[0]) for continuity validation, followed by the new target nodes. Extend the NavThroughNodes base class with: - setupExtendNavigationService() to create the service server - extendNavigationCallback() to validate and append new edges/nodes - virtual onNavigationExtended() hook for subclass-specific handling - ExtendNavigation type alias and service member
When a STITCH order arrives while navigation is active, the controller now calls the ExtendNavigation service to append the new segment in-flight instead of waiting for the current goal to finish. - Create ExtendNavigation service client in _configure_service_clients() - Filter new released edges/nodes beyond the last known sequence_id - Send stitch node (order.nodes[0]) as reference for handler validation - Update _nav_through_nodes_last_seq on success; log warning on failure and let the normal dispatch handle it when the current goal finishes
Move the nav_to_node_ and nav_through_nodes_ null checks from the execute_* functions into the handle_accepted callbacks so the goal is rejected before spawning the execution thread.
Move setupExtendNavigationService() and extendNavigationCallback() from the vehicle-specific NavThroughNodesHandler (ROBOTICS2-core) into the reusable NavThroughNodes base class in ros_amr_interop. These methods contain generic service setup and request validation logic that is not vehicle-specific, so they belong in the base class alongside the existing adapter.cpp and utils.cpp sources.
- Dedup node_states by sequence_id using a dict (order version wins on conflict) to prevent duplicates when the stitch node appears in both the current state and the incoming order. - Fix action_states: preserve existing statuses (FINISHED/RUNNING) for stitch node actions instead of resetting them to WAITING. The old [:-len()] slice had two bugs: [:-0] wiped all states, and even with n>0 it removed edge actions instead of node actions. - Improve stitch-while-navigating comments: explain the ExtendNavigation request structure, stitch node significance, and fallback behavior. - Update error guard comment in _on_active_order to clarify controller vs adapter error lifecycle. - Add publish_now=True to _process_node state update. - Clean up local variable underscore prefixes (_goal_handle, _send_goal_future, etc.) to follow Python conventions. - Remove resolved TODO about in-flight goal extension.
- Add ExtendNavigation mock service fixture to conftest.py. - test_stitch_preserves_action_statuses: verify that stitching preserves existing FINISHED/RUNNING action statuses instead of resetting them. - test_stitch_no_actions_on_stitch_node: regression test for the [:-0] bug — stitching with zero actions on the stitch node must not wipe existing action_states. - test_stitch_while_navigating: verify that a stitch arriving mid- navigation calls ExtendNavigation instead of sending a new goal. - test_skipped_feedback_nodes: verify that skipped intermediate feedback nodes are individually consumed by the result callback. - Update existing stitch test assertions for node_states dedup fix.
…rent state When merging action states in STITCH mode, deduplication was done against actions on order.nodes[0] (the stitch node only). This is too narrow when the incoming order includes the full graph (e.g. nodes 0..5), as actions from nodes 0..1 would be added twice. Fix: deduplicate against all action IDs already in current_state.action_states.
- Add docs/source/nav_through_nodes.md covering: - Handler implementation steps (configure/execute/cancel) - ExtendNavigation in-flight stitching feature - setupExtendNavigationService() and onNavigationExtended() usage - Quantillion NavThroughNodesHandler as reference implementation - Add nav_through_nodes to Sphinx toctree - Update README: list NavThroughNodes handler and link to new doc
- Remove misleading log message from STITCH path when no newly released segments are present (unreleased segments wait for TMS release; the earlier info log already captures the count) - Add Quantillion Technologies copyright to nav_through_nodes.hpp
|
|
||
| // Append only the new edges and target nodes (skip stitch node) | ||
| edges_msg_.insert(edges_msg_.end(), request->edges.begin(), request->edges.end()); | ||
| nodes_msg_.insert(nodes_msg_.end(), request->nodes.begin() + 1, request->nodes.end()); |
Member
There was a problem hiding this comment.
execute() reads these vectors on a detached thread while these inserts mutate them on the executor thread.
Add a mutex around reset() + this block, locked snapshot accessors for execute().
| response->success = false; | ||
| response->message = "Stitch node mismatch: expected node_id='" | ||
| + nodes_msg_.back().node_id + "' seq=" | ||
| + std::to_string(nodes_msg_.back().sequence_id) |
Member
There was a problem hiding this comment.
You may back() on an empty array, which is undefined behavior.
| if self._is_navigation_active(): | ||
| self._navigate_to_node_goal_handle.cancel_goal_async() | ||
| if self._enable_nav_through_nodes: | ||
| self._navigate_through_nodes_goal_handle.cancel_goal_async() |
Member
There was a problem hiding this comment.
_is_navigation_active() returns True on _nav_through_nodes_goal_pending alone, so this handle can still be None (goal sent, not yet accepted).
A cancelOrder in that window calls None.cancel_goal_async(), leaving the cancel action stuck RUNNING. Guard for None here, and in _navigate_through_nodes_goal_response_callback cancel the just-accepted goal if _canceling_order(), so cancellation still completes.
| # Do not retry navigation while any error is active — controller | ||
| # errors (e.g. navigation failure) require a cancelOrder to clear; | ||
| # adapter errors self-resolve when the adapter stops reporting them. | ||
| if len(self._current_state.errors) > 0: |
Member
There was a problem hiding this comment.
This halts the order on any error, including WARNING-level ones -- e.g. a rejected stray order update (_reject_order emits a WARNING that persists until the next accepted order) will freeze a valid in-flight order. It's also not flag-gated, so it changes the legacy nav_to_node path. Scope it to FATAL:
if any(error.error_level == VDAError.FATAL for error in self._current_state.errors):| req = ExtendNavigation.Request() | ||
| req.edges = new_edges | ||
| req.nodes = [order.nodes[0]] + new_nodes | ||
| future = self._extend_nav_svc_cli.call_async(req) |
Member
There was a problem hiding this comment.
Unlike every other client, _extend_nav_svc_cli is created (line 318) without a wait_for_service loop. If the service isn't up when a stitch arrives, call_async returns a future that never resolves and _nav_through_nodes_last_seq never advances. Guard with service_is_ready() and fall back to the existing post-goal dispatch (which already re-picks the released segment).
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Hi,
Finally we finished our navigate through nodes and it has been running for a while in our system.
Now it is ready for review.
I also added some documentation to explain a bit how the custom handler was developed.
I also included a flag to enable/disable this feature to keep backwards compatibility