fix(nav): survive controller_server crashes — respawn + lifecycle watchdog - #517
fix(nav): survive controller_server crashes — respawn + lifecycle watchdog#517DavidDobas wants to merge 4 commits into
Conversation
…ms and failure reasons Navigation trajectory: both Nav2 planner servers are namespaced, so the map widget's /plan subscription never received a route -- subscribe /navigation/plan + /mapfree/plan instead, and draw the same plan as a flat ribbon on the floor of the 3D sim view (cleared when the planner goes quiet, same staleness rule as the map). Agent panel: /brain/skill_status_update now carries the skill's call inputs, rendered as a compact k=v line under each skill run; failed runs render their reason expanded instead of hidden behind a chevron. navigate_to_position returns specific failure detail (unresolvable local goal, no path to goal, or distance remaining + recovery count) instead of the raw TaskResult.
See why navigation struggles, not just where it's headed. A new "follower" chip in the 3D view draws the MPPI controller's commanded velocity as an arrow on the robot (green forward / red reverse + turn indicator, from /cmd_vel_raw) and the path it's actively tracking (/transformed_global_plan) as an amber ribbon over the blue global plan -- a near-zero/oscillating arrow and a gap between the ribbons are the visible signature of a stuck follower. The 2D map gains the same velocity arrow. Enables MPPI `visualize` so the controller publishes its tracked path (costs controller CPU).
…me too The sim driver publishes /clock (extrapolated from the world server at wall rate, held while paused, monotonic); every other sim node runs use_sim_time:=true via a launch argument the sim tmux launcher passes (default false -- real robot unchanged). Pausing the world now freezes Nav2's progress checkers, BT timers and MPPI with it: a 25s mid-navigation freeze resumes with zero elapsed ROS time, no recoveries. TF-bearing publishers hold while paused so identical frozen stamps don't spam TF_REPEATED_DATA.
…F-aware overlays Everything nav-debug hides behind ?navdebug (plain sim view opens no rosbridge connection): the plan/follower ribbons and chips, a 5Hz HUD reporting what MPPI's critics are gated on (distance to goal, heading error to the carrot, costmap cost under/ahead, path occupancy, stall/oscillation detection), a freeze chip driving /virtual_mars/pause, and a set-goal chip (click the floor, drag the heading). navigate_to_position publishes its resolved goal on latched /nav/commanded_goal so UIs render the true target instead of the wiggling plan endpoint. Overlays and goal publishing go through AMCL's map->odom from /tf both ways, so localization drift can't shift what you see or where your click lands. Also duplicates the lidar-height map fix + failure details that shipped via
Greptile SummaryThis PR makes navigation and simulation more resilient and easier to debug. The main changes are:
Confidence Score: 4/5The sim pause and navigation overlay paths need fixes before merging.
node.py, mapWidget.js, rosbridgeController.ts, console.launch.py, uninavid.launch.py Important Files Changed
|
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| /** @param {any} msg geometry_msgs/PoseStamped — the skill's exact target */ | ||
| function onCommandedGoal(msg) { | ||
| const pos = msg?.pose?.position; | ||
| const q = msg?.pose?.orientation; | ||
| if (typeof pos?.x !== "number" || typeof pos?.y !== "number" || !q) return; | ||
| goalMarker = { x: pos.x, y: pos.y, yaw: Math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)) }; | ||
| goalIsCommanded = true; | ||
| armNavStale(); // the goal marks an active navigation; expire it like the route | ||
| draw(); | ||
| } |
There was a problem hiding this comment.
/nav/commanded_goal can be published with frame_id: "odom" for local goals, but this handler ignores the frame and draws raw coordinates on the /map grid. During local navigation, the green goal marker is shifted by the current map↔odom offset and shows the operator the target in the wrong place.
| @@ -179,15 +209,45 @@ export function createMap(root, opts = {}) { | |||
| } | |||
| if (pts.length) { | |||
| plan = pts; | |||
| // Fallback goal marker: the route's end. Only used until the skill's | |||
| // exact target arrives on /nav/commanded_goal (goalIsCommanded) — the | |||
| // endpoint wiggles with every replan, the commanded goal doesn't. Still | |||
| // needed for navigations that bypass the skill (e.g. map clicks routed | |||
| // straight to bt_navigator). | |||
| if (!goalIsCommanded) { | |||
| const end = poses[poses.length - 1]?.pose; | |||
| const q = end?.orientation; | |||
| if (q) { | |||
| const yaw = Math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)); | |||
| goalMarker = { x: pts[pts.length - 1].x, y: pts[pts.length - 1].y, yaw }; | |||
| } | |||
There was a problem hiding this comment.
This handler now receives both /navigation/plan and /mapfree/plan, but it still renders every path directly on the map grid. The mapfree planner publishes odom-frame paths, so local-navigation routes and fallback goal endpoints appear shifted on the 2D map instead of matching the robot's actual route.
| publishGoalPose(worldX: number, worldY: number, worldYaw: number): void { | ||
| // P_map = R(yaw)·P_world + t (world == odom in sim). | ||
| const t = this.#mapToOdom; | ||
| const c = t ? Math.cos(t.yaw) : 1; | ||
| const s = t ? Math.sin(t.yaw) : 0; | ||
| const x = t ? worldX * c - worldY * s + t.x : worldX; | ||
| const y = t ? worldX * s + worldY * c + t.y : worldY; | ||
| const yaw = t ? worldYaw + t.yaw : worldYaw; | ||
| this.#send({ | ||
| op: "publish", | ||
| topic: "/goal_pose", | ||
| msg: { | ||
| header: { stamp: { sec: 0, nanosec: 0 }, frame_id: "map" }, |
There was a problem hiding this comment.
If the navdebug set-goal chip is used before the first map -> odom TF arrives, #mapToOdom is null and this publishes world/odom click coordinates with frame_id: "map". Nav2 then interprets the pose in the wrong frame and can drive to a different location than the clicked point.
| from launch_ros.actions import Node | ||
| from launch_ros.parameter_descriptions import ParameterValue | ||
|
|
||
|
|
||
| def generate_launch_description() -> LaunchDescription: | ||
| # ROS time source: false on the real robot (no /clock); the sim launcher | ||
| # passes true so this node follows the sim driver's /clock. | ||
| use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="false") | ||
| use_sim_time = {"use_sim_time": ParameterValue(LaunchConfiguration("use_sim_time"), value_type=bool)} |
There was a problem hiding this comment.
This launch file now imports launch_ros.parameter_descriptions.ParameterValue, but the innate_console manifest only declares rclpy and std_msgs. A clean install has no package-level dependency that pulls in launch_ros, so ros2 launch innate_console console.launch.py can fail while parsing the launch file.
| @@ -27,15 +28,20 @@ def generate_launch_description() -> LaunchDescription: | |||
| description="Velocity topic UniNavid commands should publish to", | |||
| ) | |||
|
|
|||
| # ROS time source: false on the real robot (no /clock); the sim launcher | |||
| # passes true so this node follows the sim driver's /clock. | |||
| use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="false") | |||
| use_sim_time = {"use_sim_time": ParameterValue(LaunchConfiguration("use_sim_time"), value_type=bool)} | |||
There was a problem hiding this comment.
This launch file now imports launch_ros.parameter_descriptions.ParameterValue, but innate_uninavid does not declare launch_ros in its manifest. On a clean install, rosdep may not install that dependency and ros2 launch innate_uninavid uninavid.launch.py can fail before starting the node.
|
Wrong head branch (created from the debug branch by mistake) — superseded by the PR from fix/nav-controller-crash-resilience. |
1 participant
Problem
controller_servercrashes in the field and nothing brings it back — navigation is silently dead until reboot. Observed live twice in one sim session, same signature both times:Trigger: repeated progress-checker aborts (the robot stuck/rotating near clutter — a state this stack reaches routinely). After the crash, every navigation turns into recovery theater: the BT times out waiting for
follow_pathto acknowledge, cycles clear/spin/backup/wait, and aborts — indistinguishable from "navigation mysteriously broken."Root-cause investigation (summary)
SimpleActionServer: the control loop runs in a detachedstd::asyncthread and callsterminate_current()on failure while the executor thread concurrently handles preempts/cancels/expiry on the same goal handles.mode_manager.py'sskip_cleanup_nodes("Zenoh RMW crashes during TF unsubscription in cleanup", planner "Segfaults during costmap cleanup").ros-humble-nav2-controller 1.1.20), samermw_zenoh_cpp 0.1.8, same abort-generating behaviors. An upstream issue with the reproduction recipe is the path to a true fix; this PR makes the crash survivable in the meantime (and for whatever the next such bug is).Fix
Two parts, both generic:
respawn=True, respawn_delay=2.0oncontroller_server(navigation.launch.py) — launch brings the process back.mode_manager— a respawned lifecycle node comes backUNCONFIGUREDand useless. A 5 s watchdog checks the current mode's nodes and re-drives anythingUNCONFIGUREDto where the mode needs it (ACTIVE, orINACTIVEfor configure-only nodes). It arms only after the first successful mode startup (never races bringup) and yields to in-flight mode/map operations via the existing_mode_change_lock. It heals any respawn-configured nav node, not just the controller.Verification (live, full digital twin)
Simulated the exact crash (
SIGABRTtocontroller_server) during an active session:A crash that previously cost the rest of the session now costs ~6 seconds and leaves a loud log line.
Real-robot testing notes
pkill -ABRT -f nav2_controllermid-session, then watch for the watchdog log lines and send a goal.UNCONFIGUREDnodes of the current mode, so normal mode switches, map switches and bringup are untouched (they hold_mode_change_lock, which the watchdog defers to).