From 8a7b026f8014653529ac6faf7fb1c58489edb5d0 Mon Sep 17 00:00:00 2001 From: David Dobas Date: Wed, 8 Jul 2026 16:24:07 -0700 Subject: [PATCH 1/4] feat(sim): render the Nav2 plan in the 3D view + map, show skill params 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. --- .../brain_client/nodes/brain_client_node.py | 3 + .../brain_client/nodes/skills_server.py | 10 +-- .../brain_client/transport/chat.py | 3 + sim/viewer/src/physics/rosbridgeController.ts | 63 +++++++++++++++---- sim/viewer/src/scene.ts | 63 +++++++++++++++++++ sim/viewer/src/simSession.ts | 48 +++++++++++--- webapp/css/app.css | 9 +++ webapp/js/agent/agentPanel.js | 38 +++++++++-- webapp/js/constants.js | 9 ++- webapp/js/map/mapWidget.js | 7 ++- 10 files changed, 219 insertions(+), 34 deletions(-) diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py index 15b1edf50..1dd81521f 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py @@ -381,6 +381,7 @@ def _on_manual_skill_event(self, msg: String) -> None: primitive_name = payload.get("skill_name") or self._skill_name_for_id(skill_id) primitive_id = payload.get("primitive_id") or f"manual_{skill_id}_{int(time.time() * 1000)}" reason = payload.get("reason") + inputs = payload.get("inputs") self.get_logger().info(f"Manual skill event: {status} {primitive_name} ({skill_id})") if self.state.is_brain_active: @@ -398,6 +399,7 @@ def _on_manual_skill_event(self, msg: String) -> None: status=status, skill_id=skill_id, reason=reason, + inputs=inputs, ) self.chat.history.append( { @@ -408,6 +410,7 @@ def _on_manual_skill_event(self, msg: String) -> None: "primitiveId": primitive_id, "skillId": skill_id, **({"failureReason": reason} if reason else {}), + **({"inputs": inputs} if inputs else {}), } ) diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py index bae9c404d..e6d01f591 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py @@ -322,7 +322,7 @@ def execute_callback(self, goal_handle): name = self._skill_display_name(skill_type) run_id = uuid.uuid4().hex - self._publish_skill_status(run_id, skill_type, name, "running") + self._publish_skill_status(run_id, skill_type, name, "running", inputs=inputs) try: if self.catalog.get_code_skill(skill_type) is not None: result = self._execute_code_skill(goal_handle, skill_type, inputs) @@ -341,12 +341,12 @@ def execute_callback(self, goal_handle): except Exception as e: # A 'running' broadcast went out above — a terminal status MUST # follow or every client shows this skill as active forever. - self._publish_skill_status(run_id, skill_type, name, "failed", str(e) or "internal error") + self._publish_skill_status(run_id, skill_type, name, "failed", str(e) or "internal error", inputs=inputs) raise finally: self._release_skill_slot() status, reason = self._terminal_skill_status(result) - self._publish_skill_status(run_id, skill_type, name, status, reason) + self._publish_skill_status(run_id, skill_type, name, status, reason, inputs=inputs) return result @staticmethod @@ -377,7 +377,7 @@ def _terminal_skill_status(result) -> tuple[str, str | None]: return "failed", result.message or None def _publish_skill_status( - self, run_id: str, skill_type: str, name: str, status: str, reason: str | None = None + self, run_id: str, skill_type: str, name: str, status: str, reason: str | None = None, inputs: dict | None = None ) -> None: payload = { "primitive_name": name, @@ -389,6 +389,8 @@ def _publish_skill_status( } if reason: payload["reason"] = reason + if inputs: + payload["inputs"] = inputs self._skill_status_pub.publish(String(data=json.dumps(payload))) # ================= execution ================= diff --git a/ros2_ws/src/brain/brain_client/brain_client/transport/chat.py b/ros2_ws/src/brain/brain_client/brain_client/transport/chat.py index ad347e47c..c7f0edfc6 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/transport/chat.py +++ b/ros2_ws/src/brain/brain_client/brain_client/transport/chat.py @@ -54,6 +54,7 @@ def publish_task_status( status: str, skill_id: str | None = None, reason: str | None = None, + inputs: dict | None = None, ) -> None: """Publish a local task-status update for the controller-app UI.""" payload = { @@ -66,6 +67,8 @@ def publish_task_status( } if reason: payload["reason"] = reason + if inputs: + payload["inputs"] = inputs self._task_status_pub.publish(String(data=json.dumps(payload))) def history_json(self) -> str: diff --git a/sim/viewer/src/physics/rosbridgeController.ts b/sim/viewer/src/physics/rosbridgeController.ts index b6e455429..e47aab3d1 100644 --- a/sim/viewer/src/physics/rosbridgeController.ts +++ b/sim/viewer/src/physics/rosbridgeController.ts @@ -1,19 +1,29 @@ -// /scan overlay source for the webapp's SimSession: subscribe to the robot's -// lidar over rosbridge and emit world-frame hit points. Pose/joints for the -// 3D view come from the world server's observer stream (worldStateController) -// -- lidar stays here deliberately: it is a robot sensor, so the robot's own -// pipeline is the honest source for the debug overlay. Read-only -- +// Robot-telemetry overlay source for the webapp's SimSession: subscribe to +// the robot's lidar and Nav2's planned path over rosbridge and emit +// world-frame points. Pose/joints for the 3D view come from the world +// server's observer stream (worldStateController) -- these stay here +// deliberately: they are robot software outputs, so the robot's own +// pipeline is the honest source for the overlays. Read-only -- // teleop/commands go through the webapp's own rosbridge client. // // Speaks the rosbridge JSON protocol directly (subscribe/publish) -- no -// roslib dependency for two message types. +// roslib dependency for a few message types. // base_laser mount relative to base_link (mars.urdf base_laser_joint). const LASER_OFFSET = { x: -0.0764, z: 0.17165 }; +// Both Nav2 planner servers are namespaced (mars_nav launch); only the +// active one publishes, so both feed the same handler. Kept in sync by hand +// with PLAN_TOPICS in webapp/js/constants.js -- the viewer is a separately +// built bundle, so the constant can't be imported across that boundary. +const PLAN_TOPICS = ["/navigation/plan", "/mapfree/plan"]; + export class RosbridgePhysicsController { /** World-frame lidar hit points [x0,y0,z0, x1,...], null-range rays skipped. */ onScan?: (points: Float32Array) => void; + /** Planned-path ground points [x0,y0, x1,y1, ...]; empty = plan cleared. */ + onPlan?: (points: Float32Array) => void; + #scanEnabled = false; #url: string; #ws!: WebSocket; @@ -44,12 +54,12 @@ export class RosbridgePhysicsController { ws.onopen = () => { this.#everOpened = true; this.#retryMs = 500; - // /odom only anchors the scan overlay, so a mild throttle is fine here - // (the 3D view's pose comes from the world state stream, not rosbridge). - // queue_length 1: latest-wins -- a hop buffering more than the newest - // sample converts load hiccups into permanent lag. - this.#send({ op: "subscribe", topic: "/odom", type: "nav_msgs/msg/Odometry", throttle_rate: 100, queue_length: 1 }); - this.#send({ op: "subscribe", topic: "/scan", type: "sensor_msgs/msg/LaserScan", throttle_rate: 150, queue_length: 1 }); + // The planner republishes ~1Hz while driving; queue_length 1 keeps it + // latest-wins like the other feeds. + for (const topic of PLAN_TOPICS) { + this.#send({ op: "subscribe", topic, type: "nav_msgs/msg/Path", throttle_rate: 250, queue_length: 1 }); + } + if (this.#scanEnabled) this.#subscribeScan(); this.#resolveOpen(); }; ws.onerror = () => { @@ -69,6 +79,23 @@ export class RosbridgePhysicsController { await this.#open; } + /** Start the /scan + /odom feeds (the lidar overlay is opt-in; the plan + * overlay is always on). Idempotent; survives reconnects via #connect. */ + enableScan(): void { + if (this.#scanEnabled) return; + this.#scanEnabled = true; + this.#subscribeScan(); + } + + #subscribeScan(): void { + // /odom only anchors the scan overlay, so a mild throttle is fine here + // (the 3D view's pose comes from the world state stream, not rosbridge). + // queue_length 1: latest-wins -- a hop buffering more than the newest + // sample converts load hiccups into permanent lag. + this.#send({ op: "subscribe", topic: "/odom", type: "nav_msgs/msg/Odometry", throttle_rate: 100, queue_length: 1 }); + this.#send({ op: "subscribe", topic: "/scan", type: "sensor_msgs/msg/LaserScan", throttle_rate: 150, queue_length: 1 }); + } + dispose(): void { this.#disposed = true; this.#ws.close(); @@ -82,6 +109,18 @@ export class RosbridgePhysicsController { }; const { position, orientation } = odom.pose.pose; this.#pose = { x: position.x, y: position.y, yaw: 2 * Math.atan2(orientation.z, orientation.w) }; + } else if (msg.topic !== undefined && PLAN_TOPICS.includes(msg.topic) && this.onPlan) { + // Plan poses arrive in the map/odom frame; the sim's world frame is the + // map frame (ground-truth-seeded localization), so use them directly -- + // the same assumption the webapp's 2D map widget makes. + const path = msg.msg as { poses?: Array<{ pose: { position: { x: number; y: number } } }> }; + if (!Array.isArray(path.poses)) return; + const points = new Float32Array(path.poses.length * 2); + path.poses.forEach((p, i) => { + points[i * 2] = p.pose.position.x; + points[i * 2 + 1] = p.pose.position.y; + }); + this.onPlan(points); } else if (msg.topic === "/scan" && this.onScan) { const scan = msg.msg as { angle_min: number; angle_increment: number; range_max: number; ranges: number[] }; const { x, y, yaw } = this.#pose; diff --git a/sim/viewer/src/scene.ts b/sim/viewer/src/scene.ts index 2ed9a3e23..9b11d9e21 100644 --- a/sim/viewer/src/scene.ts +++ b/sim/viewer/src/scene.ts @@ -62,6 +62,7 @@ export class SimScene { private robotCameras = new Map(); private activeView: CameraView = "orbit"; private lidarPoints?: THREE.Points; + private planRibbon?: THREE.Mesh; private hullsGroup?: THREE.Group; private hullsPromise?: Promise; private hullsVisible = false; @@ -162,6 +163,68 @@ export class SimScene { if (this.lidarPoints) this.lidarPoints.visible = visible; } + // Planned-path ribbon: width in meters and lift above the floor mesh + // (enough to clear z-fighting, low enough to read as painted on it). + private static readonly PLAN_WIDTH = 0.06; + private static readonly PLAN_Z = 0.02; + + /** Show the Nav2 planned path as a flat ribbon on the floor. `points` is + * ground-frame [x0,y0, x1,y1, ...]; fewer than 2 points hides it. */ + setPlanPoints(points: Float32Array): void { + const n = points.length / 2; + if (n < 2) { + if (this.planRibbon) this.planRibbon.visible = false; + return; + } + if (!this.planRibbon) { + const material = new THREE.MeshBasicMaterial({ + color: 0x00b7ff, // matches the 2D map widget's route color + transparent: true, + opacity: 0.85, + depthWrite: false, + side: THREE.DoubleSide, + }); + this.planRibbon = new THREE.Mesh(new THREE.BufferGeometry(), material); + this.planRibbon.frustumCulled = false; + this.scene.add(this.planRibbon); + } + // Two vertices per path point, offset ±half-width along the averaged + // perpendicular of the adjacent segments -- a miter-less triangle strip. + const half = SimScene.PLAN_WIDTH / 2; + const vertices = new Float32Array(n * 2 * 3); + for (let i = 0; i < n; i++) { + const x = points[i * 2]; + const y = points[i * 2 + 1]; + const px = points[Math.max(0, i - 1) * 2]; + const py = points[Math.max(0, i - 1) * 2 + 1]; + const nx = points[Math.min(n - 1, i + 1) * 2]; + const ny = points[Math.min(n - 1, i + 1) * 2 + 1]; + const len = Math.hypot(nx - px, ny - py) || 1; + const perpX = -(ny - py) / len; + const perpY = (nx - px) / len; + vertices.set([x + perpX * half, y + perpY * half, SimScene.PLAN_Z], i * 6); + vertices.set([x - perpX * half, y - perpY * half, SimScene.PLAN_Z], i * 6 + 3); + } + const indices: number[] = []; + for (let i = 0; i < n - 1; i++) { + const a = i * 2; + indices.push(a, a + 1, a + 2, a + 1, a + 3, a + 2); + } + // Swap in a fresh geometry and dispose the old one: replacing attributes + // on a live geometry leaves the previous GPU buffers allocated until a + // dispose, and the planner republishes ~1Hz for a whole navigation. + const geometry = new THREE.BufferGeometry(); + geometry.setAttribute("position", new THREE.BufferAttribute(vertices, 3)); + geometry.setIndex(indices); + this.planRibbon.geometry.dispose(); + this.planRibbon.geometry = geometry; + this.planRibbon.visible = true; + } + + setPlanVisible(visible: boolean): void { + if (this.planRibbon) this.planRibbon.visible = visible; + } + /** * Wireframe overlay of the driver's collision hulls -- the same * apartment_collisions_v2 set mars_sim_driver collides against, lazily diff --git a/sim/viewer/src/simSession.ts b/sim/viewer/src/simSession.ts index 749b568a6..42e4aa37a 100644 --- a/sim/viewer/src/simSession.ts +++ b/sim/viewer/src/simSession.ts @@ -2,7 +2,8 @@ // simulated: same state shape and methods, no video pipeline. State comes // from the world server's ground-truth observer stream (~75Hz pose+joints), // played back with a short clamped interpolation; rosbridge remains only -// for the /scan debug overlay. Architecture: sim/README.md. +// for the Nav2 plan overlay and the /scan debug overlay. +// Architecture: sim/README.md. import type { SimScene } from "./scene"; import { RosbridgePhysicsController } from "./physics/rosbridgeController"; @@ -86,6 +87,14 @@ export class SimSession { #hullsOn = false; #overlaysDirty = false; + // Nav2 planned path (always-on overlay, shown while navigating). The + // planner republishes ~1Hz the whole time it's driving, so a lull means + // navigation ended -- same staleness rule as the webapp's 2D map. + #plan: Float32Array | null = null; + #planDirty = false; + #planStaleAt = 0; + static readonly #PLAN_STALE_S = 4; + #stateUrls: string[]; #rosUrl: string; @@ -135,6 +144,20 @@ export class SimSession { // directly (thumbnailCanvas below). this.#connectState(0); + this.#connectRosFeed(); + } + + /** Open the rosbridge feed for the always-on plan overlay; the scan feed + * piggybacks on the same connection when the lidar chip enables it. */ + #connectRosFeed(): void { + this.#scanFeed = new RosbridgePhysicsController(this.#rosUrl); + this.#scanFeed.onPlan = (points) => { + this.#plan = points; + this.#planDirty = true; + this.#planStaleAt = performance.now() / 1000 + SimSession.#PLAN_STALE_S; + }; + this.#scanFeed.init().catch((err) => console.warn("[sim-session] rosbridge overlays unavailable:", err)); + if (this.#lidarOn) this.setLidarVisible(true); // chip toggled before start() } /** Connect the state feed, falling through the URL candidates (direct @@ -187,6 +210,11 @@ export class SimSession { this.#controller = null; this.#scanFeed?.dispose(); this.#scanFeed = null; + // Drop buffered overlay data so a restart doesn't redraw a stale route/scan. + this.#plan = null; + this.#planDirty = false; + this.#scan = null; + this.#scanDirty = false; this.#started = false; this.#gotPose = false; this.#patch({ status: "idle", videoStream: null }); @@ -197,19 +225,17 @@ export class SimSession { this.#listeners.clear(); } - /** Toggle the /scan hit-point overlay (stage "lidar" chip). The rosbridge - * connection is opened on first use -- the 3D view itself never consumes - * robot telemetry. */ + /** Toggle the /scan hit-point overlay (stage "lidar" chip). The scan + * subscription starts on first use, on the plan overlay's connection. */ setLidarVisible(on: boolean): void { this.#lidarOn = on; this.#overlaysDirty = true; - if (on && this.#scanFeed === null) { - this.#scanFeed = new RosbridgePhysicsController(this.#rosUrl); + if (on && this.#scanFeed !== null && !this.#scanFeed.onScan) { this.#scanFeed.onScan = (points) => { this.#scan = points; this.#scanDirty = true; }; - this.#scanFeed.init().catch((err) => console.warn("[sim-session] scan overlay unavailable:", err)); + this.#scanFeed.enableScan(); } } @@ -304,6 +330,14 @@ export class SimSession { scene.setLidarPoints(this.#scan); scene.setLidarVisible(true); // first points may arrive after the toggle } + + if (this.#planDirty && this.#plan) { + this.#planDirty = false; + scene.setPlanPoints(this.#plan); + } else if (this.#plan && performance.now() / 1000 > this.#planStaleAt) { + this.#plan = null; // planner went quiet: navigation ended + scene.setPlanVisible(false); + } } /** Active, non-primary views whose PiP tiles need frames. */ diff --git a/webapp/css/app.css b/webapp/css/app.css index 1bb006b24..9ff10fe57 100644 --- a/webapp/css/app.css +++ b/webapp/css/app.css @@ -4856,6 +4856,15 @@ button { opacity: 0.6; } +/* The skill's call parameters (compact k=v line under the status row). */ +.chat-skill-params { + padding: 0 10px 6px; + font-size: 10px; + line-height: 1.4; + color: var(--muted); + word-break: break-word; +} + /* Collapsible failure reason / error trace. */ .chat-skill-detail { max-height: 0; diff --git a/webapp/js/agent/agentPanel.js b/webapp/js/agent/agentPanel.js index a067bd4b2..f89973d4e 100644 --- a/webapp/js/agent/agentPanel.js +++ b/webapp/js/agent/agentPanel.js @@ -273,11 +273,20 @@ export function createAgentPanel(root, rosClient, agentState) { snapIfAtBottom(wasAtBottom); } - /** @type {Map} */ + /** @type {Map} */ const skillRuns = new Map(); - /** @param {string} key @param {string} name @param {string} status @param {number} ts @param {string} [reason] */ - function addSkillRun(key, name, status, ts, reason) { + /** Compact one-line "k=v" rendering of a skill's call inputs. + * @param {any} inputs */ + function formatInputs(inputs) { + if (!inputs || typeof inputs !== "object" || Array.isArray(inputs)) return ""; + return Object.entries(inputs) + .map(([k, v]) => `${k}=${v !== null && typeof v === "object" ? JSON.stringify(v) : String(v)}`) + .join(" "); + } + + /** @param {string} key @param {string} name @param {string} status @param {number} ts @param {string} [reason] @param {string} [params] */ + function addSkillRun(key, name, status, ts, reason, params) { const wasAtBottom = atBottom(); const cls = ["running", "completed", "failed", "interrupted"].includes(status) ? status : "running"; // Reflect the running primitive in the Active Skill readout. @@ -307,13 +316,22 @@ export function createAgentPanel(root, rosClient, agentState) { head.append(tag, nameEl, statusEl); wrap.append(head); stream.appendChild(wrap); - run = { wrap, head, status: statusEl, hasDetail: false }; + run = { wrap, head, status: statusEl, hasDetail: false, hasParams: false }; skillRuns.set(key, run); } run.wrap.className = `chat-skill ${cls}`; if (run.hasDetail) run.wrap.classList.add("has-detail"); run.status.textContent = cls; + // The call parameters, shown from the "running" update onwards. + if (params && !run.hasParams) { + run.hasParams = true; + const paramsEl = document.createElement("div"); + paramsEl.className = "chat-skill-params mono"; + paramsEl.textContent = roundNums(params); + run.head.insertAdjacentElement("afterend", paramsEl); + } + // A failed run carries the failure reason / error — show it expanded // in-place (collapsible so a long trace can be tucked away). if (cls === "failed" && reason && !run.hasDetail) { @@ -413,7 +431,14 @@ export function createAgentPanel(root, rosClient, agentState) { const status = String(e?.taskStatus ?? ""); if (!name || !status) return; const key = String(e?.primitiveId ?? e?.skillId ?? name); - addSkillRun(key, name, status, ts, typeof e?.failureReason === "string" ? e.failureReason : ""); + addSkillRun( + key, + name, + status, + ts, + typeof e?.failureReason === "string" ? e.failureReason : "", + formatInputs(e?.inputs), + ); return; } const text = String(e?.text ?? ""); @@ -485,7 +510,8 @@ export function createAgentPanel(root, rosClient, agentState) { if (!name || !status) return; const key = String(payload?.primitive_id ?? payload?.skill_id ?? name); const reason = typeof payload?.reason === "string" ? payload.reason : ""; - addSkillRun(key, name, status, Number(payload?.timestamp) || Date.now() / 1000, reason); + const params = formatInputs(payload?.inputs); + addSkillRun(key, name, status, Number(payload?.timestamp) || Date.now() / 1000, reason, params); }); return { diff --git a/webapp/js/constants.js b/webapp/js/constants.js index 1a49b3eab..85267c58e 100644 --- a/webapp/js/constants.js +++ b/webapp/js/constants.js @@ -52,9 +52,14 @@ export const WEBSOCKET_STATUS_TOPIC = "/brain/websocket_status"; // Navigation map + odometry for the 2D map page. export const MAP_TOPIC = "/map"; // nav_msgs/OccupancyGrid export const ODOM_TOPIC = "/odom"; // nav_msgs/Odometry -export const PLAN_TOPIC = "/plan"; // nav_msgs/Path — the planner's route to the goal +// nav_msgs/Path — the planner's route to the goal. Both Nav2 planner servers +// are namespaced (mars_nav launch), so the route arrives on one of these +// depending on the active planner; there is no root /plan publisher. Kept in +// sync by hand with PLAN_TOPICS in sim/viewer/src/physics/rosbridgeController.ts +// (the sim viewer is a separately built bundle). +export const PLAN_TOPICS = ["/navigation/plan", "/mapfree/plan"]; // Click-to-navigate goal. Publishing a geometry_msgs/PoseStamped here kicks off -// planning; the resulting route streams back on PLAN_TOPIC. Same topic the sim +// planning; the resulting route streams back on PLAN_TOPICS. Same topic the sim // console's map view publishes to. export const GOAL_POSE_TOPIC = "/goal_pose"; // Stop all active navigation (std_srvs/Trigger) — cancels every NavigateToPose diff --git a/webapp/js/map/mapWidget.js b/webapp/js/map/mapWidget.js index fa64d1978..8c9ca8bfa 100644 --- a/webapp/js/map/mapWidget.js +++ b/webapp/js/map/mapWidget.js @@ -7,7 +7,7 @@ // is all a 2D map needs. import { ros } from "../rosClient.js"; -import { MAP_TOPIC, ODOM_TOPIC, PLAN_TOPIC, GOAL_POSE_TOPIC, CANCEL_NAVIGATION_SERVICE } from "../constants.js"; +import { MAP_TOPIC, ODOM_TOPIC, PLAN_TOPICS, GOAL_POSE_TOPIC, CANCEL_NAVIGATION_SERVICE } from "../constants.js"; // Wheel-zoom bounds (metres of real-world width shown). const MIN_ZOOM_M = 1; @@ -376,7 +376,8 @@ export function createMap(root, opts = {}) { const unsubMap = ros.subscribe(MAP_TOPIC, onMap, 250); const unsubOdom = ros.subscribe(ODOM_TOPIC, onOdom, 100); - const unsubPlan = ros.subscribe(PLAN_TOPIC, onPlan, 250, "nav_msgs/msg/Path"); + // Only the active planner publishes, so both feeds can share one handler. + const unsubPlans = PLAN_TOPICS.map((topic) => ros.subscribe(topic, onPlan, 250, "nav_msgs/msg/Path")); return { /** Swap to a saved zoom (e.g. when this widget reparents between thumbnail and full stage). */ @@ -391,7 +392,7 @@ export function createMap(root, opts = {}) { ro.disconnect(); unsubMap(); unsubOdom(); - unsubPlan(); + for (const unsub of unsubPlans) unsub(); canvas.remove(); controls.remove(); }, From b1d8fcac35b24588bad40aada7983609d6812358 Mon Sep 17 00:00:00 2001 From: David Dobas Date: Wed, 8 Jul 2026 20:46:25 -0700 Subject: [PATCH 2/4] feat(sim): follower overlay -- MPPI command arrow + tracked-path ribbon 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). --- .../mars_bot/mars_nav/config/controller.yaml | 2 +- sim/viewer/src/physics/rosbridgeController.ts | 50 +++++- sim/viewer/src/scene.ts | 144 ++++++++++++++---- sim/viewer/src/simSession.ts | 61 ++++++++ sim/viewer/src/simStage.ts | 1 + webapp/js/constants.js | 4 + webapp/js/map/mapWidget.js | 53 ++++++- 7 files changed, 274 insertions(+), 41 deletions(-) diff --git a/ros2_ws/src/mars_bot/mars_nav/config/controller.yaml b/ros2_ws/src/mars_bot/mars_nav/config/controller.yaml index 4c71e3174..947c616be 100644 --- a/ros2_ws/src/mars_bot/mars_nav/config/controller.yaml +++ b/ros2_ws/src/mars_bot/mars_nav/config/controller.yaml @@ -63,7 +63,7 @@ # Algorithm parameters temperature: 0.3 # Trajectory selection temperature gamma: 0.015 # Smoothness vs energy trade-off - visualize: false # Set to true for debugging (slows performance) + visualize: true # publishes /trajectories + /transformed_global_plan for the sim follower overlay (costs controller CPU) regenerate_noises: false # Goal tolerances diff --git a/sim/viewer/src/physics/rosbridgeController.ts b/sim/viewer/src/physics/rosbridgeController.ts index e47aab3d1..4d0b09474 100644 --- a/sim/viewer/src/physics/rosbridgeController.ts +++ b/sim/viewer/src/physics/rosbridgeController.ts @@ -1,10 +1,11 @@ -// Robot-telemetry overlay source for the webapp's SimSession: subscribe to -// the robot's lidar and Nav2's planned path over rosbridge and emit -// world-frame points. Pose/joints for the 3D view come from the world -// server's observer stream (worldStateController) -- these stay here -// deliberately: they are robot software outputs, so the robot's own -// pipeline is the honest source for the overlays. Read-only -- -// teleop/commands go through the webapp's own rosbridge client. +// Robot-telemetry overlay source for the webapp's SimSession: subscribe over +// rosbridge to the robot's lidar, Nav2's planned path, and the MPPI +// controller's command + tracked path, and emit world-frame points. Pose/ +// joints for the 3D view come from the world server's observer stream +// (worldStateController) -- these stay here deliberately: they are robot +// software outputs, so the robot's own pipeline is the honest source for the +// overlays. Read-only -- teleop/commands go through the webapp's own +// rosbridge client. // // Speaks the rosbridge JSON protocol directly (subscribe/publish) -- no // roslib dependency for a few message types. @@ -23,7 +24,12 @@ export class RosbridgePhysicsController { onScan?: (points: Float32Array) => void; /** Planned-path ground points [x0,y0, x1,y1, ...]; empty = plan cleared. */ onPlan?: (points: Float32Array) => void; + /** MPPI's commanded body velocity (m/s, rad/s) from /cmd_vel_raw. */ + onCommand?: (vx: number, wz: number) => void; + /** Ground points of the path MPPI is tracking (transformed_global_plan). */ + onFollowerPath?: (points: Float32Array) => void; #scanEnabled = false; + #followerEnabled = false; #url: string; #ws!: WebSocket; @@ -60,6 +66,7 @@ export class RosbridgePhysicsController { this.#send({ op: "subscribe", topic, type: "nav_msgs/msg/Path", throttle_rate: 250, queue_length: 1 }); } if (this.#scanEnabled) this.#subscribeScan(); + if (this.#followerEnabled) this.#subscribeFollower(); this.#resolveOpen(); }; ws.onerror = () => { @@ -96,6 +103,23 @@ export class RosbridgePhysicsController { this.#send({ op: "subscribe", topic: "/scan", type: "sensor_msgs/msg/LaserScan", throttle_rate: 150, queue_length: 1 }); } + /** Start the follower overlay feeds (opt-in): the MPPI command and the path + * it's tracking. transformed_global_plan only publishes when the controller's + * `visualize` param is on (mars_nav controller.yaml). */ + enableFollower(): void { + if (this.#followerEnabled) return; + this.#followerEnabled = true; + this.#subscribeFollower(); + } + + #subscribeFollower(): void { + // /cmd_vel_raw is the controller's own output, pre-smoothing/mux -- the + // honest "what the follower commands". 20Hz raw; throttle to keep the + // arrow lively without flooding. + this.#send({ op: "subscribe", topic: "/cmd_vel_raw", type: "geometry_msgs/msg/Twist", throttle_rate: 100, queue_length: 1 }); + this.#send({ op: "subscribe", topic: "/transformed_global_plan", type: "nav_msgs/msg/Path", throttle_rate: 100, queue_length: 1 }); + } + dispose(): void { this.#disposed = true; this.#ws.close(); @@ -121,6 +145,18 @@ export class RosbridgePhysicsController { points[i * 2 + 1] = p.pose.position.y; }); this.onPlan(points); + } else if (msg.topic === "/cmd_vel_raw" && this.onCommand) { + const twist = msg.msg as { linear: { x: number }; angular: { z: number } }; + this.onCommand(twist.linear.x, twist.angular.z); + } else if (msg.topic === "/transformed_global_plan" && this.onFollowerPath) { + const path = msg.msg as { poses?: Array<{ pose: { position: { x: number; y: number } } }> }; + if (!Array.isArray(path.poses)) return; + const points = new Float32Array(path.poses.length * 2); + path.poses.forEach((p, i) => { + points[i * 2] = p.pose.position.x; + points[i * 2 + 1] = p.pose.position.y; + }); + this.onFollowerPath(points); } else if (msg.topic === "/scan" && this.onScan) { const scan = msg.msg as { angle_min: number; angle_increment: number; range_max: number; ranges: number[] }; const { x, y, yaw } = this.#pose; diff --git a/sim/viewer/src/scene.ts b/sim/viewer/src/scene.ts index 9b11d9e21..88bc7b2aa 100644 --- a/sim/viewer/src/scene.ts +++ b/sim/viewer/src/scene.ts @@ -63,6 +63,9 @@ export class SimScene { private activeView: CameraView = "orbit"; private lidarPoints?: THREE.Points; private planRibbon?: THREE.Mesh; + private followerRibbon?: THREE.Mesh; + private cmdLinearArrow?: THREE.ArrowHelper; + private cmdTurnArrow?: THREE.ArrowHelper; private hullsGroup?: THREE.Group; private hullsPromise?: Promise; private hullsVisible = false; @@ -163,34 +166,22 @@ export class SimScene { if (this.lidarPoints) this.lidarPoints.visible = visible; } - // Planned-path ribbon: width in meters and lift above the floor mesh - // (enough to clear z-fighting, low enough to read as painted on it). + // Ribbon widths (meters) and lift above the floor mesh (enough to clear + // z-fighting, low enough to read as painted on it). The follower ribbon + // sits a hair higher so it draws on top where it overlaps the plan. private static readonly PLAN_WIDTH = 0.06; private static readonly PLAN_Z = 0.02; + private static readonly FOLLOWER_WIDTH = 0.05; + private static readonly FOLLOWER_Z = 0.025; - /** Show the Nav2 planned path as a flat ribbon on the floor. `points` is - * ground-frame [x0,y0, x1,y1, ...]; fewer than 2 points hides it. */ - setPlanPoints(points: Float32Array): void { + /** Build a flat miter-less triangle-strip ribbon from ground-frame points + * [x0,y0, x1,y1, ...] at height `z`, or null for fewer than 2 points. */ + private static ribbonGeometry(points: Float32Array, width: number, z: number): THREE.BufferGeometry | null { const n = points.length / 2; - if (n < 2) { - if (this.planRibbon) this.planRibbon.visible = false; - return; - } - if (!this.planRibbon) { - const material = new THREE.MeshBasicMaterial({ - color: 0x00b7ff, // matches the 2D map widget's route color - transparent: true, - opacity: 0.85, - depthWrite: false, - side: THREE.DoubleSide, - }); - this.planRibbon = new THREE.Mesh(new THREE.BufferGeometry(), material); - this.planRibbon.frustumCulled = false; - this.scene.add(this.planRibbon); - } - // Two vertices per path point, offset ±half-width along the averaged - // perpendicular of the adjacent segments -- a miter-less triangle strip. - const half = SimScene.PLAN_WIDTH / 2; + if (n < 2) return null; + // Two vertices per point, offset ±half-width along the averaged + // perpendicular of the adjacent segments. + const half = width / 2; const vertices = new Float32Array(n * 2 * 3); for (let i = 0; i < n; i++) { const x = points[i * 2]; @@ -202,29 +193,118 @@ export class SimScene { const len = Math.hypot(nx - px, ny - py) || 1; const perpX = -(ny - py) / len; const perpY = (nx - px) / len; - vertices.set([x + perpX * half, y + perpY * half, SimScene.PLAN_Z], i * 6); - vertices.set([x - perpX * half, y - perpY * half, SimScene.PLAN_Z], i * 6 + 3); + vertices.set([x + perpX * half, y + perpY * half, z], i * 6); + vertices.set([x - perpX * half, y - perpY * half, z], i * 6 + 3); } const indices: number[] = []; for (let i = 0; i < n - 1; i++) { const a = i * 2; indices.push(a, a + 1, a + 2, a + 1, a + 3, a + 2); } - // Swap in a fresh geometry and dispose the old one: replacing attributes - // on a live geometry leaves the previous GPU buffers allocated until a - // dispose, and the planner republishes ~1Hz for a whole navigation. const geometry = new THREE.BufferGeometry(); geometry.setAttribute("position", new THREE.BufferAttribute(vertices, 3)); geometry.setIndex(indices); - this.planRibbon.geometry.dispose(); - this.planRibbon.geometry = geometry; - this.planRibbon.visible = true; + return geometry; + } + + /** Point `mesh` at a freshly built ribbon and dispose the old geometry + * (swapping attributes on a live geometry leaks GPU buffers, and these + * republish ~1Hz). Hides the mesh when there aren't enough points. */ + private updateRibbon(mesh: THREE.Mesh, points: Float32Array, width: number, z: number): void { + const geometry = SimScene.ribbonGeometry(points, width, z); + if (!geometry) { + mesh.visible = false; + return; + } + mesh.geometry.dispose(); + mesh.geometry = geometry; + mesh.visible = true; + } + + /** Show the Nav2 planned path as a flat ribbon on the floor. `points` is + * ground-frame [x0,y0, x1,y1, ...]; fewer than 2 points hides it. */ + setPlanPoints(points: Float32Array): void { + if (!this.planRibbon) { + const material = new THREE.MeshBasicMaterial({ + color: 0x00b7ff, // matches the 2D map widget's route color + transparent: true, + opacity: 0.85, + depthWrite: false, + side: THREE.DoubleSide, + }); + this.planRibbon = new THREE.Mesh(new THREE.BufferGeometry(), material); + this.planRibbon.frustumCulled = false; + this.scene.add(this.planRibbon); + } + this.updateRibbon(this.planRibbon, points, SimScene.PLAN_WIDTH, SimScene.PLAN_Z); } setPlanVisible(visible: boolean): void { if (this.planRibbon) this.planRibbon.visible = visible; } + /** Show the path the MPPI controller is actively tracking (Nav2's + * transformed_global_plan) as an amber ribbon over the blue plan -- the gap + * between the two is what the follower is struggling to close. */ + setFollowerPathPoints(points: Float32Array): void { + if (!this.followerRibbon) { + const material = new THREE.MeshBasicMaterial({ + color: 0xffb020, // amber -- distinct from the cyan global plan + transparent: true, + opacity: 0.9, + depthWrite: false, + side: THREE.DoubleSide, + }); + this.followerRibbon = new THREE.Mesh(new THREE.BufferGeometry(), material); + this.followerRibbon.frustumCulled = false; + this.scene.add(this.followerRibbon); + } + this.updateRibbon(this.followerRibbon, points, SimScene.FOLLOWER_WIDTH, SimScene.FOLLOWER_Z); + } + + /** Draw the controller's commanded velocity as arrows anchored to the robot: + * a forward/back arrow (green forward, red reverse) scaled by linear speed, + * and a side arrow marking turn direction scaled by yaw rate. Both are + * children of the robot root, so they inherit its live pose. */ + setCommandVelocity(vx: number, wz: number): void { + if (!this.cmdLinearArrow) { + // +X is forward, +Y is left in the robot frame (REP-103). + this.cmdLinearArrow = new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), new THREE.Vector3(0, 0, 0.12), 0.3, 0x33ff88, 0.09, 0.06); + this.cmdTurnArrow = new THREE.ArrowHelper(new THREE.Vector3(0, 1, 0), new THREE.Vector3(0, 0, 0.12), 0.2, 0xffd24a, 0.07, 0.05); + this.robotRoot.add(this.cmdLinearArrow, this.cmdTurnArrow); + } + const turn = this.cmdTurnArrow!; + // 1 m/s -> ~0.6 m arrow; hidden below a small deadband so a stalled + // controller reads as "no arrow", not a stub. + const linLen = Math.min(Math.abs(vx) * 0.6, 0.9); + if (linLen < 0.02) { + this.cmdLinearArrow.visible = false; + } else { + this.cmdLinearArrow.visible = true; + this.cmdLinearArrow.setDirection(new THREE.Vector3(Math.sign(vx), 0, 0)); + this.cmdLinearArrow.setLength(linLen, Math.min(0.09, linLen * 0.4), Math.min(0.06, linLen * 0.3)); + this.cmdLinearArrow.setColor(vx >= 0 ? 0x33ff88 : 0xff5544); + } + const turnLen = Math.min(Math.abs(wz) * 0.35, 0.5); + if (turnLen < 0.02) { + turn.visible = false; + } else { + turn.visible = true; + turn.setDirection(new THREE.Vector3(0, Math.sign(wz), 0)); // wz>0 = CCW = left + turn.setLength(turnLen, Math.min(0.07, turnLen * 0.4), Math.min(0.05, turnLen * 0.3)); + } + } + + setFollowerVisible(visible: boolean): void { + if (this.followerRibbon) this.followerRibbon.visible = visible; + // Arrows are re-driven by setCommandVelocity each tick while on; here we + // only need to force them off when the overlay is hidden. + if (!visible) { + if (this.cmdLinearArrow) this.cmdLinearArrow.visible = false; + if (this.cmdTurnArrow) this.cmdTurnArrow.visible = false; + } + } + /** * Wireframe overlay of the driver's collision hulls -- the same * apartment_collisions_v2 set mars_sim_driver collides against, lazily diff --git a/sim/viewer/src/simSession.ts b/sim/viewer/src/simSession.ts index 42e4aa37a..d46bffaaa 100644 --- a/sim/viewer/src/simSession.ts +++ b/sim/viewer/src/simSession.ts @@ -95,6 +95,17 @@ export class SimSession { #planStaleAt = 0; static readonly #PLAN_STALE_S = 4; + // Follower overlay (opt-in "follower" chip): the MPPI command arrow + the + // path it's tracking. Both go stale quickly once the controller stops. + #followerOn = false; + #followerDirty = false; + #cmd: { vx: number; wz: number } | null = null; + #cmdDirty = false; + #followerPath: Float32Array | null = null; + #followerPathDirty = false; + #followerStaleAt = 0; + static readonly #FOLLOWER_STALE_S = 1; + #stateUrls: string[]; #rosUrl: string; @@ -156,8 +167,19 @@ export class SimSession { this.#planDirty = true; this.#planStaleAt = performance.now() / 1000 + SimSession.#PLAN_STALE_S; }; + this.#scanFeed.onCommand = (vx, wz) => { + this.#cmd = { vx, wz }; + this.#cmdDirty = true; + this.#followerStaleAt = performance.now() / 1000 + SimSession.#FOLLOWER_STALE_S; + }; + this.#scanFeed.onFollowerPath = (points) => { + this.#followerPath = points; + this.#followerPathDirty = true; + this.#followerStaleAt = performance.now() / 1000 + SimSession.#FOLLOWER_STALE_S; + }; this.#scanFeed.init().catch((err) => console.warn("[sim-session] rosbridge overlays unavailable:", err)); if (this.#lidarOn) this.setLidarVisible(true); // chip toggled before start() + if (this.#followerOn) this.setFollowerVisible(true); } /** Connect the state feed, falling through the URL candidates (direct @@ -215,6 +237,10 @@ export class SimSession { this.#planDirty = false; this.#scan = null; this.#scanDirty = false; + this.#cmd = null; + this.#cmdDirty = false; + this.#followerPath = null; + this.#followerPathDirty = false; this.#started = false; this.#gotPose = false; this.#patch({ status: "idle", videoStream: null }); @@ -245,6 +271,15 @@ export class SimSession { this.#overlaysDirty = true; } + /** Toggle the follower overlay (stage "follower" chip): MPPI's command arrow + * + the path it's tracking. Subscribes on first enable; needs the + * controller's `visualize` param on for the tracked-path feed. */ + setFollowerVisible(on: boolean): void { + this.#followerOn = on; + this.#followerDirty = true; + if (on && this.#scanFeed !== null) this.#scanFeed.enableFollower(); + } + // WebRTC-specific surface: harmless no-ops in sim. setAudio(_on: boolean): void {} async getStats(): Promise { @@ -338,6 +373,32 @@ export class SimSession { this.#plan = null; // planner went quiet: navigation ended scene.setPlanVisible(false); } + + if (this.#followerDirty) { + this.#followerDirty = false; + scene.setFollowerVisible(this.#followerOn); + } + if (this.#followerOn) { + const stale = performance.now() / 1000 > this.#followerStaleAt; + if (stale) { + // Controller stopped commanding: clear the arrow and tracked path. + if (this.#cmd || this.#followerPath) { + this.#cmd = null; + this.#followerPath = null; + scene.setCommandVelocity(0, 0); + scene.setFollowerPathPoints(new Float32Array(0)); + } + } else { + if (this.#cmdDirty && this.#cmd) { + this.#cmdDirty = false; + scene.setCommandVelocity(this.#cmd.vx, this.#cmd.wz); + } + if (this.#followerPathDirty && this.#followerPath) { + this.#followerPathDirty = false; + scene.setFollowerPathPoints(this.#followerPath); + } + } + } } /** Active, non-primary views whose PiP tiles need frames. */ diff --git a/sim/viewer/src/simStage.ts b/sim/viewer/src/simStage.ts index b5eba7a26..0531a8530 100644 --- a/sim/viewer/src/simStage.ts +++ b/sim/viewer/src/simStage.ts @@ -59,6 +59,7 @@ export function createSimStage(parent: HTMLElement, session: SimSession): { audi }; addChip("lidar", (on) => session.setLidarVisible(on)); addChip("collisions", (on) => session.setCollisionHullsVisible(on)); + addChip("follower", (on) => session.setFollowerVisible(on)); debugStack.appendChild(chips); // Loading overlay: spinner + staged label instead of a black canvas until diff --git a/webapp/js/constants.js b/webapp/js/constants.js index 85267c58e..6b9e5f1d1 100644 --- a/webapp/js/constants.js +++ b/webapp/js/constants.js @@ -58,6 +58,10 @@ export const ODOM_TOPIC = "/odom"; // nav_msgs/Odometry // sync by hand with PLAN_TOPICS in sim/viewer/src/physics/rosbridgeController.ts // (the sim viewer is a separately built bundle). export const PLAN_TOPICS = ["/navigation/plan", "/mapfree/plan"]; +// The MPPI controller's raw commanded velocity (geometry_msgs/Twist), before +// smoothing and the teleop/skills/nav mux. Shown as a velocity arrow so a +// stalled/oscillating follower is visible against the plan. +export const CMD_VEL_RAW_TOPIC = "/cmd_vel_raw"; // Click-to-navigate goal. Publishing a geometry_msgs/PoseStamped here kicks off // planning; the resulting route streams back on PLAN_TOPICS. Same topic the sim // console's map view publishes to. diff --git a/webapp/js/map/mapWidget.js b/webapp/js/map/mapWidget.js index 8c9ca8bfa..1b6748725 100644 --- a/webapp/js/map/mapWidget.js +++ b/webapp/js/map/mapWidget.js @@ -7,7 +7,7 @@ // is all a 2D map needs. import { ros } from "../rosClient.js"; -import { MAP_TOPIC, ODOM_TOPIC, PLAN_TOPICS, GOAL_POSE_TOPIC, CANCEL_NAVIGATION_SERVICE } from "../constants.js"; +import { MAP_TOPIC, ODOM_TOPIC, PLAN_TOPICS, CMD_VEL_RAW_TOPIC, GOAL_POSE_TOPIC, CANCEL_NAVIGATION_SERVICE } from "../constants.js"; // Wheel-zoom bounds (metres of real-world width shown). const MIN_ZOOM_M = 1; @@ -50,6 +50,10 @@ export function createMap(root, opts = {}) { let pose = null; /** @type {Array<{ x: number, y: number }> | null} world-frame plan points */ let plan = null; + /** @type {{ vx: number, wz: number } | null} latest follower command (body frame) */ + let cmd = null; + /** @type {ReturnType | undefined} clears the arrow when the controller goes quiet */ + let cmdStaleTimer; // Last draw's grid→canvas placement, so pointer handlers can invert it. /** @type {{ ox: number, oy: number, scale: number } | null} */ @@ -188,6 +192,22 @@ export function createMap(root, opts = {}) { draw(); } + /** @param {any} msg geometry_msgs/Twist — the follower's commanded velocity */ + function onCmd(msg) { + const vx = msg?.linear?.x; + const wz = msg?.angular?.z; + if (typeof vx !== "number" || typeof wz !== "number") return; + cmd = { vx, wz }; + // /cmd_vel_raw is published only while a controller is active; a lull means + // it stopped, so drop the arrow after a short quiet period. + clearTimeout(cmdStaleTimer); + cmdStaleTimer = setTimeout(() => { + cmd = null; + draw(); + }, 500); + draw(); + } + function draw() { if (!ctx) return; ctx.fillStyle = "#0a0a0c"; @@ -269,6 +289,34 @@ export function createMap(root, opts = {}) { ctx.moveTo(px, py); ctx.lineTo(px + Math.cos(pose.yaw) * rad * 2.4, py - Math.sin(pose.yaw) * rad * 2.4); ctx.stroke(); + + // Follower command: an arrow from the robot showing the commanded linear + // velocity (green forward, red reverse), length ~1.5 s of travel. A + // near-zero or flipping arrow is the visible signature of a stuck + // controller. Canvas y is flipped vs world y, hence -sin. + if (cmd && Math.abs(cmd.vx) > 0.01) { + const pxPerM = view.scale / grid.resolution; + const len = Math.max(-1.2, Math.min(1.2, cmd.vx * 1.5)) * pxPerM; + const dir = cmd.vx >= 0 ? pose.yaw : pose.yaw + Math.PI; + const ex = px + Math.cos(dir) * Math.abs(len); + const ey = py - Math.sin(dir) * Math.abs(len); + ctx.strokeStyle = cmd.vx >= 0 ? "#33ff88" : "#ff5544"; + ctx.lineWidth = 3 * dpr(); + ctx.lineCap = "round"; + ctx.beginPath(); + ctx.moveTo(px, py); + ctx.lineTo(ex, ey); + ctx.stroke(); + // Arrowhead. + const head = 6 * dpr(); + ctx.beginPath(); + ctx.moveTo(ex, ey); + ctx.lineTo(ex - Math.cos(dir - 0.4) * head, ey + Math.sin(dir - 0.4) * head); + ctx.moveTo(ex, ey); + ctx.lineTo(ex - Math.cos(dir + 0.4) * head, ey + Math.sin(dir + 0.4) * head); + ctx.stroke(); + ctx.lineCap = "butt"; + } } } @@ -378,6 +426,7 @@ export function createMap(root, opts = {}) { const unsubOdom = ros.subscribe(ODOM_TOPIC, onOdom, 100); // Only the active planner publishes, so both feeds can share one handler. const unsubPlans = PLAN_TOPICS.map((topic) => ros.subscribe(topic, onPlan, 250, "nav_msgs/msg/Path")); + const unsubCmd = ros.subscribe(CMD_VEL_RAW_TOPIC, onCmd, 100, "geometry_msgs/msg/Twist"); return { /** Swap to a saved zoom (e.g. when this widget reparents between thumbnail and full stage). */ @@ -389,10 +438,12 @@ export function createMap(root, opts = {}) { }, destroy() { clearTimeout(navStaleTimer); + clearTimeout(cmdStaleTimer); ro.disconnect(); unsubMap(); unsubOdom(); for (const unsub of unsubPlans) unsub(); + unsubCmd(); canvas.remove(); controls.remove(); }, From 62dffbafdaeccd90e357e1c82bc26690547fe8e3 Mon Sep 17 00:00:00 2001 From: David Dobas Date: Thu, 9 Jul 2026 16:23:33 -0700 Subject: [PATCH 3/4] feat(sim): ROS sim time + world pause -- freezing stops navigation time 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. --- .../launch/brain_client.sim.launch.py | 9 +++ .../manipulation/launch/behavior.launch.py | 9 ++- .../innate_console/launch/console.launch.py | 10 ++- .../innate_uninavid/launch/uninavid.launch.py | 10 ++- .../mars_control/launch/app.sim.launch.py | 2 + .../mars_nav/launch/mode_manager.launch.py | 35 ++++++--- .../mars_nav/launch/navigation.launch.py | 17 ++++- .../launch/sim_rosbridge.launch.py | 2 + .../launch/sim_driver.launch.py | 6 +- .../mars_sim_driver/mars_sim_driver/node.py | 71 +++++++++++++++++-- .../mars_sim_driver/remote_world.py | 13 ++++ .../mars_sim_driver/world_server.py | 16 +++++ .../src/mars_bot/mars_sim_driver/package.xml | 1 + scripts/launch_sim_in_tmux.zsh | 10 +-- 14 files changed, 185 insertions(+), 26 deletions(-) diff --git a/ros2_ws/src/brain/brain_client/launch/brain_client.sim.launch.py b/ros2_ws/src/brain/brain_client/launch/brain_client.sim.launch.py index ba1b55bf0..0dbe05fcf 100644 --- a/ros2_ws/src/brain/brain_client/launch/brain_client.sim.launch.py +++ b/ros2_ws/src/brain/brain_client/launch/brain_client.sim.launch.py @@ -108,6 +108,12 @@ def generate_launch_description(): package="brain_client", executable="brain_client_node.py", name="brain_client_node", + # Sim runs on /clock (mars_sim_driver publishes it). A process-level + # --ros-args override, NOT a node parameter: these processes create + # extra nodes at runtime (BasicNavigator instances in skills and + # mobility) that a per-node parameters dict would never reach -- the + # global override applies to every node in the process. + arguments=["--ros-args", "-p", "use_sim_time:=true"], parameters=[ { "websocket_uri": LaunchConfiguration("websocket_uri"), @@ -166,6 +172,9 @@ def generate_launch_description(): executable="skills_server.py", name="skills_action_server", output="screen", + # Same process-level sim-time override as brain_client_node: + # skills create BasicNavigator nodes at runtime. + arguments=["--ros-args", "-p", "use_sim_time:=true"], parameters=[ { "image_topic": LaunchConfiguration("image_topic"), diff --git a/ros2_ws/src/brain/manipulation/launch/behavior.launch.py b/ros2_ws/src/brain/manipulation/launch/behavior.launch.py index 530e3fe67..5beeecf52 100644 --- a/ros2_ws/src/brain/manipulation/launch/behavior.launch.py +++ b/ros2_ws/src/brain/manipulation/launch/behavior.launch.py @@ -7,6 +7,7 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare from mars_bringup.config_loader import settings_params, workspace_skills_dir @@ -26,6 +27,11 @@ def generate_launch_description(): "log_level", default_value="info", description="Log level for the behavior server" ) + # 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)} + # Resolved here because ROS YAML can't expand $INNATE_OS_ROOT. Mirrors recorder.launch.py. data_directory = str(workspace_skills_dir()) @@ -38,6 +44,7 @@ def generate_launch_description(): parameters=[ LaunchConfiguration("manipulation_config"), {"data_directory": data_directory}, # env-resolved; beats the YAML fallback + use_sim_time, *settings_params(), # settings.yaml overrides, layered last ], arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")], @@ -46,4 +53,4 @@ def generate_launch_description(): respawn_delay=2.0, ) - return LaunchDescription([manipulation_config_arg, log_level_arg, behavior_server_node]) + return LaunchDescription([manipulation_config_arg, log_level_arg, use_sim_time_arg, behavior_server_node]) diff --git a/ros2_ws/src/cloud/innate_console/launch/console.launch.py b/ros2_ws/src/cloud/innate_console/launch/console.launch.py index e4463279f..83e600621 100644 --- a/ros2_ws/src/cloud/innate_console/launch/console.launch.py +++ b/ros2_ws/src/cloud/innate_console/launch/console.launch.py @@ -3,18 +3,26 @@ """Launch file for the innate_console node.""" from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration 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)} return LaunchDescription( [ + use_sim_time_arg, Node( package="innate_console", executable="console_node", name="innate_console", output="screen", - parameters=[], + parameters=[use_sim_time], ), ] ) diff --git a/ros2_ws/src/cloud/innate_uninavid/launch/uninavid.launch.py b/ros2_ws/src/cloud/innate_uninavid/launch/uninavid.launch.py index 0a6b148b5..f3a722954 100644 --- a/ros2_ws/src/cloud/innate_uninavid/launch/uninavid.launch.py +++ b/ros2_ws/src/cloud/innate_uninavid/launch/uninavid.launch.py @@ -9,6 +9,7 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from mars_bringup.config_loader import settings_params @@ -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)} + node = Node( package="innate_uninavid", executable="uninavid_node", name="uninavid_node", output="screen", - parameters=[LaunchConfiguration("params_file"), *settings_params()], + parameters=[LaunchConfiguration("params_file"), use_sim_time, *settings_params()], remappings=[ ("/cmd_vel", LaunchConfiguration("cmd_vel_topic")), ], ) - return LaunchDescription([params_arg, cmd_vel_topic_arg, node]) + return LaunchDescription([params_arg, cmd_vel_topic_arg, use_sim_time_arg, node]) diff --git a/ros2_ws/src/mars_bot/mars_control/launch/app.sim.launch.py b/ros2_ws/src/mars_bot/mars_control/launch/app.sim.launch.py index fa11c4cd1..d1a54242e 100644 --- a/ros2_ws/src/mars_bot/mars_control/launch/app.sim.launch.py +++ b/ros2_ws/src/mars_bot/mars_control/launch/app.sim.launch.py @@ -20,6 +20,8 @@ def generate_launch_description(): { "data_directory": data_directory, "default_hardware_revision": default_hardware_revision, + # Sim runs on /clock (mars_sim_driver publishes it). + "use_sim_time": True, }, *settings_params(), ], diff --git a/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py b/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py index 6f810ee53..c466eb333 100644 --- a/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py +++ b/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py @@ -6,9 +6,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from mars_bringup.config_loader import load_motion_limit_overrides, load_yaml_param_defaults @@ -62,14 +64,24 @@ def generate_launch_description(): nav_through_poses_bt_xml = os.path.join(share_dir, "config", "nav_through_poses.xml") nav_to_pose_mapfree_bt_xml = os.path.join(share_dir, "config", "nav_to_pose_mapfree.xml") # noqa: F841 + # ROS time source: false on the real robot (no /clock); the sim launcher + # passes true so the whole nav stack follows the sim driver's /clock and + # freezes with the world. Layered last in each node's parameters so it + # overrides the yamls' hardcoded use_sim_time: false. + 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)} + navigation_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([mars_nav_launch_dir, "/navigation.launch.py"]) + PythonLaunchDescriptionSource([mars_nav_launch_dir, "/navigation.launch.py"]), + launch_arguments={"use_sim_time": LaunchConfiguration("use_sim_time")}.items(), ) # mapfree_launch = IncludeLaunchDescription( # PythonLaunchDescriptionSource([mars_nav_launch_dir, '/mapfree_local_nav.launch.py']) # ) + # mapping.launch.py keeps its own use_sim_time default (true) -- threading + # ours through would change slam_toolbox's existing real-robot behavior. mapping_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([mars_nav_launch_dir, "/mapping.launch.py"]) ) @@ -84,7 +96,8 @@ def generate_launch_description(): { "slowdown_radius": 1.0, "min_speed_fraction": 0.3, - } + }, + use_sim_time, ], remappings=[ ("cmd_vel_in", "/cmd_vel_raw"), @@ -101,7 +114,7 @@ def generate_launch_description(): executable="velocity_smoother", name="velocity_smoother", output="screen", - parameters=[smoother_params_file, smoother_limit_overrides], + parameters=[smoother_params_file, smoother_limit_overrides, use_sim_time], # Nav2 output feeds the priority mux, not the base directly. remappings=[("cmd_vel", "/cmd_vel_scaled"), ("cmd_vel_smoothed", "/cmd_vel_nav")], arguments=["--ros-args", "--log-level", "warn"], @@ -114,6 +127,7 @@ def generate_launch_description(): executable="cmd_vel_mux.py", name="cmd_vel_mux", output="screen", + parameters=[use_sim_time], ) # Shared BT navigator node @@ -126,6 +140,7 @@ def generate_launch_description(): bt_navigator_params_file, {"default_nav_to_pose_bt_xml": nav_to_pose_bt_xml}, {"default_nav_through_poses_bt_xml": nav_through_poses_bt_xml}, + use_sim_time, ], remappings=expand_action_remappings( [ @@ -143,7 +158,7 @@ def generate_launch_description(): executable="behavior_server", name="behavior_server", output="screen", - parameters=[behavior_params_file], + parameters=[behavior_params_file, use_sim_time], arguments=["--ros-args", "--log-level", "warn"], remappings=[("cmd_vel", "cmd_vel_raw")], ) @@ -153,6 +168,7 @@ def generate_launch_description(): executable="dynamic_footprint", name="dynamic_footprint", output="screen", + parameters=[use_sim_time], ) # Mapfree planner node (runs in mapfree namespace) @@ -162,7 +178,7 @@ def generate_launch_description(): name="planner_server", namespace="mapfree", output="screen", - parameters=[planner_params_file, costmap_params_file], + parameters=[planner_params_file, costmap_params_file, use_sim_time], remappings=[ # TF remappings - critical for namespaced nodes ("tf", "/tf"), @@ -174,10 +190,13 @@ def generate_launch_description(): ) # Null map node for identity map->odom transform - null_map_node = Node(package="mars_nav", executable="null_map_node", name="null_map_node", output="screen") + null_map_node = Node( + package="mars_nav", executable="null_map_node", name="null_map_node", output="screen", parameters=[use_sim_time] + ) return LaunchDescription( [ + use_sim_time_arg, null_map_node, navigation_launch, # mapfree_launch, @@ -194,7 +213,7 @@ def generate_launch_description(): executable="mode_manager.py", name="mode_manager", output="screen", - parameters=[], + parameters=[use_sim_time], # nav2's BasicNavigator creates internal nodes that can share a # name; mute the benign "Publisher already registered" warning. arguments=["--ros-args", "--log-level", "rcl.logging_rosout:=ERROR"], diff --git a/ros2_ws/src/mars_bot/mars_nav/launch/navigation.launch.py b/ros2_ws/src/mars_bot/mars_nav/launch/navigation.launch.py index e90f3b845..290e082ab 100644 --- a/ros2_ws/src/mars_bot/mars_nav/launch/navigation.launch.py +++ b/ros2_ws/src/mars_bot/mars_nav/launch/navigation.launch.py @@ -28,6 +28,7 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from mars_bringup.config_loader import ( load_costmap_rewrites, load_motion_limit_overrides, @@ -75,13 +76,20 @@ def generate_launch_description(): "amcl_params_file", default_value=amcl_params_file, description="Full path to the AMCL parameters file" ) + # ROS time source: false on the real robot (no /clock); the sim launcher + # passes true (via mode_manager.launch.py) so the nav stack follows the + # sim driver's /clock and freezes with the world. Layered last so it + # overrides the yamls' hardcoded use_sim_time: false. + 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)} + # Create the map server node map_server_node = Node( package="nav2_map_server", executable="map_server", name="navigation_map_server", output="screen", - parameters=[{"yaml_filename": ""}], + parameters=[{"yaml_filename": ""}, use_sim_time], # nav2 boot chatter at WARN to keep `innate view` readable. arguments=["--ros-args", "--log-level", "warn"], ) @@ -92,7 +100,7 @@ def generate_launch_description(): executable="amcl", name="navigation_amcl", output="screen", - parameters=[LaunchConfiguration("amcl_params_file")], + parameters=[LaunchConfiguration("amcl_params_file"), use_sim_time], arguments=["--ros-args", "--log-level", "warn"], ) @@ -110,6 +118,7 @@ def generate_launch_description(): "auto_localize_timeout": 30.0, "max_score_threshold": 0.3, }, + use_sim_time, *settings_params(), ], ) @@ -121,7 +130,7 @@ def generate_launch_description(): name="planner_server", namespace="navigation", output="screen", - parameters=[planner_params_file, costmap_params_file], + parameters=[planner_params_file, costmap_params_file, use_sim_time], remappings=[ # TF remappings - critical for namespaced nodes ("tf", "/tf"), @@ -142,6 +151,7 @@ def generate_launch_description(): controller_params_file, costmap_params_file, load_motion_limit_overrides("mppi", defaults=load_yaml_param_defaults(controller_params_file)), + use_sim_time, ], remappings=[ ("cmd_vel", "cmd_vel_raw"), @@ -155,6 +165,7 @@ def generate_launch_description(): return LaunchDescription( [ amcl_params_arg, + use_sim_time_arg, # Nav2 lifecycle-managed nodes map_server_node, grid_localizer_node, diff --git a/ros2_ws/src/mars_bot/mars_sim_bringup/launch/sim_rosbridge.launch.py b/ros2_ws/src/mars_bot/mars_sim_bringup/launch/sim_rosbridge.launch.py index 08c7b26ec..d79e88ef2 100644 --- a/ros2_ws/src/mars_bot/mars_sim_bringup/launch/sim_rosbridge.launch.py +++ b/ros2_ws/src/mars_bot/mars_sim_bringup/launch/sim_rosbridge.launch.py @@ -16,6 +16,8 @@ def generate_launch_description(): { "port": 9090, "rosbridge_compatible": True, + # Sim runs on /clock (mars_sim_driver publishes it). + "use_sim_time": True, } ], # Disable Zenoh SHM for rws_server to prevent __pthread_tpp_change_priority diff --git a/ros2_ws/src/mars_bot/mars_sim_driver/launch/sim_driver.launch.py b/ros2_ws/src/mars_bot/mars_sim_driver/launch/sim_driver.launch.py index 6beb1f0b5..aa8ff7412 100644 --- a/ros2_ws/src/mars_bot/mars_sim_driver/launch/sim_driver.launch.py +++ b/ros2_ws/src/mars_bot/mars_sim_driver/launch/sim_driver.launch.py @@ -44,8 +44,11 @@ def generate_launch_description(): executable="robot_state_publisher", name="robot_state_publisher", output="screen", - parameters=[{"robot_description": urdf.read_text()}], + parameters=[{"robot_description": urdf.read_text(), "use_sim_time": True}], ), + # The driver is the /clock source and deliberately stays on wall + # time: its timers must keep ticking while the world is paused to + # publish the frozen clock and accept the unpause. Node( package="mars_sim_driver", executable="sim_driver", @@ -59,6 +62,7 @@ def generate_launch_description(): package="mars_sim_driver", executable="grid_localizer_sim", output="screen", + parameters=[{"use_sim_time": True}], ), ] ) diff --git a/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/node.py b/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/node.py index 772d679c4..5dee77714 100644 --- a/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/node.py +++ b/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/node.py @@ -50,7 +50,8 @@ from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, qos_profile_sensor_data from sensor_msgs.msg import CameraInfo, CompressedImage, Image, JointState, LaserScan, PointCloud2, PointField -from std_msgs.msg import Empty, Float64MultiArray, Int32, String +from rosgraph_msgs.msg import Clock +from std_msgs.msg import Bool, Empty, Float64MultiArray, Int32, String from std_srvs.srv import Trigger from tf2_ros import TransformBroadcaster @@ -65,6 +66,7 @@ MAIN_CAMERA_FPS = 7.5 # main_camera_driver: 15fps capture, JPEG every 2nd frame WRIST_CAMERA_FPS = 5.0 # arm_camera_driver: 30fps capture, JPEG every 6th frame DEPTH_FPS = 8.0 # stereo_depth_estimator max_fps +CLOCK_HZ = 100.0 # /clock rate: sim-time timers quantize to this ODOM_HZ = 30.0 # bringup.py odom_frequency SCAN_HZ = 6.0 # lidar.launch.py throttle JOINT_STATE_HZ = 30.0 @@ -118,6 +120,18 @@ def __init__(self) -> None: self._stream_interval = 0.1 # EMA of the stream's cadence (see _on_arm_commands) self._tf = TransformBroadcaster(self) + + # ROS sim time (sim-only): this node is the /clock source, so the rest + # of the stack can run use_sim_time and freeze cleanly with the world + # (paused world -> frozen /clock -> Nav2 timers/progress checks stop). + # This node itself stays on wall time -- its timers must keep ticking + # while paused to publish the frozen clock and accept the unpause. + self._clock_lock = threading.Lock() + self._clock_prev = 0.0 + self._clock_paused = False + self._pub_seen: dict[str, float] = {} # last stamp per paused publisher + self._clock_pub = self.create_publisher(Clock, "/clock", 10) + self.create_timer(1.0 / CLOCK_HZ, self._publish_clock) # State topics publish KEEP_LAST(1): a hop that buffers more than the # newest sample turns a slow consumer into permanent display lag. self._odom_pub = self.create_publisher(Odometry, "/odom", 1) @@ -164,6 +178,10 @@ def __init__(self) -> None: # Sim-only convenience (no real-robot equivalent): respawn at the # spawn pose, used by sim/viewer's Reset button in connected mode. self.create_subscription(Empty, "/virtual_mars/reset", self._on_reset, 10) + # Sim-only: freeze physics in place so an operator can inspect a + # navigation mid-flight (sim/viewer's ?navdebug "freeze" chip). The + # robot stack keeps running -- only the world stops advancing. + self.create_subscription(Bool, "/virtual_mars/pause", self._on_pause, 10) services = ReentrantCallbackGroup() # goto services block; don't starve timers if GotoJS is not None: @@ -258,6 +276,9 @@ def _on_reset(self, _msg: Empty) -> None: self._fail_active_traj() self.sim.reset() + def _on_pause(self, msg: Bool) -> None: + self.sim.set_paused(bool(msg.data)) + def _on_head_position(self, msg: Int32) -> None: deg = max(HEAD_MIN_DEG, min(HEAD_MAX_DEG, float(msg.data))) with self._lock: @@ -364,13 +385,47 @@ def _advance_trajectory(self) -> None: # --- outputs --- + def _sim_time_pair(self) -> tuple[float, bool]: + """Current sim time + paused flag. Extrapolates from the last world + state at wall rate (physics steps against the wall clock), holds while + paused, and never steps backwards -- /clock must be monotonic.""" + sim_t, paused, at = self.sim.clock_sample() + t = sim_t if paused else sim_t + (time.monotonic() - at) + with self._clock_lock: + t = max(t, self._clock_prev) + self._clock_prev = t + self._clock_paused = paused + return t, paused + + def _publish_clock(self) -> None: + t, _paused = self._sim_time_pair() + self._clock_pub.publish(Clock(clock=rclpy.time.Time(seconds=t).to_msg())) + + def _paused_repeat(self, key: str, t: float, paused: bool) -> bool: + """True when paused and `key` already published at this frozen time -- + TF consumers log TF_REPEATED_DATA for every identically-stamped + retransmit, so the TF-bearing publishers hold instead.""" + if not paused: + self._pub_seen.pop(key, None) + return False + if self._pub_seen.get(key) == t: + return True + self._pub_seen[key] = t + return False + def _stamp(self): - return self.get_clock().now().to_msg() + # Sim time, matching /clock -- NOT this node's (wall) clock: every + # other node runs use_sim_time, so stamps must live on that timeline. + t, _ = self._sim_time_pair() + return rclpy.time.Time(seconds=t).to_msg() def _publish_odom(self) -> None: with self._lock: x, y, yaw = self.sim.pose() - stamp = self._stamp() + t, paused = self._sim_time_pair() + if self._paused_repeat("odom", t, paused): + return + stamp = rclpy.time.Time(seconds=t).to_msg() # Like bringup.py: pose only, zero twist/covariance. odom = Odometry() @@ -394,13 +449,16 @@ def _publish_odom(self) -> None: self._tf.sendTransform(tf) def _publish_scan(self) -> None: + t, paused = self._sim_time_pair() + if self._paused_repeat("scan", t, paused): + return try: with self._lock: ranges = self.sim.lidar_scan(LIDAR_N_RAYS, LIDAR_RANGE_MAX) except (OSError, RuntimeError): return # server briefly away; skip this scan msg = LaserScan() - msg.header.stamp = self._stamp() + msg.header.stamp = rclpy.time.Time(seconds=t).to_msg() msg.header.frame_id = "base_laser" msg.angle_min = -math.pi msg.angle_max = math.pi - 2 * math.pi / LIDAR_N_RAYS @@ -561,10 +619,13 @@ def _publish_camera_info(self) -> None: self._caminfo_pub.publish(msg) def _publish_joint_states(self) -> None: + t, paused = self._sim_time_pair() + if self._paused_repeat("joints", t, paused): + return # robot_state_publisher TF: no identically-stamped repeats with self._lock: positions = self.sim.joint_positions() msg = JointState() - msg.header.stamp = self._stamp() + msg.header.stamp = rclpy.time.Time(seconds=t).to_msg() msg.name = [*ARM_JOINTS, "joint_head"] # same 7 as the real arm node msg.position = [positions[n] for n in msg.name] self._joint_states_pub.publish(msg) diff --git a/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/remote_world.py b/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/remote_world.py index 65d08cd20..c7b22cbbc 100644 --- a/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/remote_world.py +++ b/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/remote_world.py @@ -121,6 +121,13 @@ def _fresh_state(self) -> dict: def time(self) -> float: return float(self._fresh_state()["time"]) + def clock_sample(self) -> tuple[float, bool, float]: + """(sim_time, paused, monotonic_at_receipt) -- the driver's /clock + extrapolates from this between polls (sim runs at wall rate).""" + state = self._fresh_state() + with self._state_lock: + return float(state["time"]), bool(state.get("paused", False)), self._state_at + def pose(self) -> tuple[float, float, float]: return tuple(self._fresh_state()["pose"]) @@ -154,6 +161,12 @@ def reset(self) -> None: with self._state_lock: self._state_at = 0.0 + def set_paused(self, on: bool) -> None: + """Freeze/unfreeze physics stepping (sim-only debugging).""" + self._state_ch.call({"op": "pause", "on": bool(on)}) + with self._state_lock: + self._state_at = 0.0 # pick up the new paused flag immediately + # --- sensors --- def lidar_scan(self, n_rays: int, max_range: float) -> np.ndarray: diff --git a/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/world_server.py b/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/world_server.py index 21fd574c9..4ccb7306f 100644 --- a/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/world_server.py +++ b/ros2_ws/src/mars_bot/mars_sim_driver/mars_sim_driver/world_server.py @@ -85,6 +85,9 @@ def __init__(self, sim: VirtualMars): self.state_payload = "{}" self.state_seq = 0 self.state_cond = threading.Condition() + # Freeze physics in place (sim-only debugging: /virtual_mars/pause). + # Robot software keeps running against a world that stops advancing. + self.paused = False # --- physics (side thread; MuJoCo stepping is pure CPU) --- @@ -96,6 +99,13 @@ def physics_loop(self) -> None: start_wall = time.perf_counter() start_sim = self.sim.data.time while True: + if self.paused: + # Rebase the wall<->sim offset every tick while held, so + # resuming doesn't look like a huge backlog to catch up on. + start_wall = time.perf_counter() + start_sim = self.sim.data.time + time.sleep(0.01) + continue target = start_sim + (time.perf_counter() - start_wall) stepped = False with self.lock: @@ -220,6 +230,7 @@ def handle(self, req: dict) -> tuple[dict, bytes | None]: return { "ok": True, "time": sim_time, + "paused": self.paused, "pose": [x, y, yaw], "vel": [vx, vy, wz], "joints": joints, @@ -238,6 +249,11 @@ def handle(self, req: dict) -> tuple[dict, bytes | None]: with self.lock: self.sim.reset() return {"ok": True}, None + if op == "pause": + # Plain bool assignment: physics_loop reads it every tick, and a + # one-tick delay either way is invisible. + self.paused = bool(req.get("on", True)) + return {"ok": True, "paused": self.paused}, None if op == "lidar": with self.lock: ranges = self.sim.lidar_scan(int(req["n_rays"]), float(req["max_range"])) diff --git a/ros2_ws/src/mars_bot/mars_sim_driver/package.xml b/ros2_ws/src/mars_bot/mars_sim_driver/package.xml index c18b7cd1f..30f0d7ec0 100644 --- a/ros2_ws/src/mars_bot/mars_sim_driver/package.xml +++ b/ros2_ws/src/mars_bot/mars_sim_driver/package.xml @@ -17,6 +17,7 @@ sensor_msgs geometry_msgs nav_msgs + rosgraph_msgs tf2_ros mars_msgs robot_state_publisher diff --git a/scripts/launch_sim_in_tmux.zsh b/scripts/launch_sim_in_tmux.zsh index 70ee79cb0..226762f0e 100755 --- a/scripts/launch_sim_in_tmux.zsh +++ b/scripts/launch_sim_in_tmux.zsh @@ -120,7 +120,7 @@ settle_after_launch mkdir -p ~/innate-os/data/maps cp ~/innate-os/sim/assets/map/sim_apartment.* ~/innate-os/data/maps/ 2>/dev/null || true tmux new-window -t "$SESSION_NAME" -n nav-brain -tmux send-keys -t "${TMUX_TARGET_PREFIX}:nav-brain" "ros2 launch mars_nav mode_manager.launch.py" C-m +tmux send-keys -t "${TMUX_TARGET_PREFIX}:nav-brain" "ros2 launch mars_nav mode_manager.launch.py use_sim_time:=true" C-m echo "Started navigation system..." settle_after_launch # Split and run brain client @@ -139,17 +139,17 @@ echo "Started brain client..." # === Window 4: Behavior Server === tmux new-window -t "$SESSION_NAME" -n behavior -tmux send-keys -t "${TMUX_TARGET_PREFIX}:behavior" "ros2 launch manipulation behavior.launch.py" C-m +tmux send-keys -t "${TMUX_TARGET_PREFIX}:behavior" "ros2 launch manipulation behavior.launch.py use_sim_time:=true" C-m echo "Started behavior server..." # === Window 5: Arm IK === tmux new-window -t "$SESSION_NAME" -n arm-ik -tmux send-keys -t "${TMUX_TARGET_PREFIX}:arm-ik" "ros2 run mars_arm ik.py" C-m +tmux send-keys -t "${TMUX_TARGET_PREFIX}:arm-ik" "ros2 run mars_arm ik.py --ros-args -p use_sim_time:=true" C-m echo "Started arm IK..." # === Window 6: Vision Navigation Inference Client === tmux new-window -t "$SESSION_NAME" -n vision-nav -tmux send-keys -t "${TMUX_TARGET_PREFIX}:vision-nav" "ros2 launch innate_uninavid uninavid.launch.py cmd_vel_topic:=/cmd_vel" C-m +tmux send-keys -t "${TMUX_TARGET_PREFIX}:vision-nav" "ros2 launch innate_uninavid uninavid.launch.py cmd_vel_topic:=/cmd_vel use_sim_time:=true" C-m echo "Started vision navigation inference client..." settle_after_launch @@ -158,7 +158,7 @@ settle_after_launch # front door binds 443 (https) + 80 (http) inside the container — both exposed by # docker-compose.dev.yml — and proxies /ws to the sim rosbridge on 9090. tmux new-window -t "$SESSION_NAME" -n console-webapp -tmux send-keys -t "${TMUX_TARGET_PREFIX}:console-webapp" "ros2 launch innate_console console.launch.py" C-m +tmux send-keys -t "${TMUX_TARGET_PREFIX}:console-webapp" "ros2 launch innate_console console.launch.py use_sim_time:=true" C-m echo "Started console..." settle_after_launch tmux split-window -t "${TMUX_TARGET_PREFIX}:console-webapp" -h From dc678fbf1c19c5825ab464aa3c6d0fd7b7c5bde8 Mon Sep 17 00:00:00 2001 From: David Dobas Date: Thu, 9 Jul 2026 16:23:53 -0700 Subject: [PATCH 4/4] feat(sim): ?navdebug -- MPPI decision HUD, freeze + set-goal chips, TF-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 --- sim/viewer/src/navDebug.ts | 207 ++++++++++++++++ sim/viewer/src/physics/rosbridgeController.ts | 234 ++++++++++++++++-- sim/viewer/src/scene.ts | 71 ++++++ sim/viewer/src/simSession.ts | 233 ++++++++++++++--- sim/viewer/src/simStage.ts | 147 ++++++++++- webapp/js/constants.js | 4 + webapp/js/map/mapWidget.js | 55 +++- .../innate_skills/navigate_to_position.py | 10 + 8 files changed, 895 insertions(+), 66 deletions(-) create mode 100644 sim/viewer/src/navDebug.ts diff --git a/sim/viewer/src/navDebug.ts b/sim/viewer/src/navDebug.ts new file mode 100644 index 000000000..63f827be3 --- /dev/null +++ b/sim/viewer/src/navDebug.ts @@ -0,0 +1,207 @@ +// Why is the follower doing that? -- derived MPPI decision state for the +// ?navdebug HUD. +// +// MPPI publishes its command and its tracked path, but never the per-critic +// costs behind them. What it *does* expose is everything those critics are +// gated on: distance to goal, heading error to the path, and the local +// costmap. Each critic switches on/off at a fixed distance from the goal, so +// knowing the distance tells you which objectives are actually steering the +// robot at this instant -- usually more informative than the raw cost would be. +// +// The thresholds below MIRROR mars_nav/config/controller.yaml (InnateFollowPath). +// They are duplicated, not imported: the viewer is a separately built bundle +// with no access to the ROS param server. Keep them in sync by hand. + +/** Distance-to-goal (m) under which the path critics stop being applied + * (PathAlign/PathFollow/PathAngle `threshold_to_consider`). */ +const PATH_CRITICS_OFF_WITHIN = 0.6; +/** Distance-to-goal (m) under which GoalCritic starts being applied. */ +const GOAL_CRITIC_ON_WITHIN = 0.6; +/** Distance-to-goal (m) under which GoalAngleCritic starts (yaw settle). */ +const GOAL_ANGLE_ON_WITHIN = 0.3; +/** Distance-to-goal (m) under which PreferForwardCritic stops being applied. */ +const PREFER_FORWARD_OFF_WITHIN = 0.5; +/** PathAngleCritic only penalizes once heading error exceeds this (rad). */ +const PATH_ANGLE_MAX_ANGLE = 1.0; +/** PathAlignCritic disables itself when more of the path than this is occupied. */ +const PATH_ALIGN_MAX_OCCUPANCY = 0.05; +/** PathAngleCritic's carrot: this many path points ahead of the closest one. */ +const PATH_ANGLE_OFFSET = 4; + +/** Command magnitudes below which the controller is effectively commanding + * nothing (matches the arrow overlay's deadband in scene.ts). */ +export const STALL_VX = 0.033; +export const STALL_WZ = 0.057; + +// nav2 costmap cost semantics. +const COST_INSCRIBED = 253; // >= this: the robot's inscribed circle collides +const COST_LETHAL = 254; +const COST_UNKNOWN = 255; + +export interface Costmap { + resolution: number; + sizeX: number; + sizeY: number; + originX: number; + originY: number; + data: Uint8Array; +} + +export interface Pose { + x: number; + y: number; + yaw: number; +} + +/** Raw cost (0-255) at a world point, or null when outside the costmap. */ +export function costAt(cm: Costmap, x: number, y: number): number | null { + const col = Math.floor((x - cm.originX) / cm.resolution); + const row = Math.floor((y - cm.originY) / cm.resolution); + if (col < 0 || row < 0 || col >= cm.sizeX || row >= cm.sizeY) return null; + return cm.data[row * cm.sizeX + col] ?? null; +} + +/** Human label for a raw cost value. */ +export function costLabel(cost: number | null): string { + if (cost === null) return "off-map"; + if (cost === COST_UNKNOWN) return "unknown"; + if (cost >= COST_LETHAL) return "LETHAL"; + if (cost >= COST_INSCRIBED) return "INSCRIBED"; + if (cost > 0) return "inflated"; + return "free"; +} + +/** Fraction of the path's points sitting in inscribed-or-worse cells. This is + * what PathAlignCritic tests against max_path_occupancy_ratio before giving up + * on aligning the robot to a path that runs through obstacles. */ +export function pathOccupancyRatio(cm: Costmap, path: Float32Array): number { + const n = path.length / 2; + if (n === 0) return 0; + let blocked = 0; + for (let i = 0; i < n; i++) { + const cost = costAt(cm, path[i * 2], path[i * 2 + 1]); + if (cost !== null && cost >= COST_INSCRIBED && cost !== COST_UNKNOWN) blocked++; + } + return blocked / n; +} + +/** Worst cost encountered along the first `meters` of the path -- the thing + * that makes every forward rollout expensive when the corridor is tight. */ +export function maxCostAhead(cm: Costmap, path: Float32Array, meters: number): number | null { + const n = path.length / 2; + let travelled = 0; + let worst: number | null = null; + for (let i = 0; i < n && travelled <= meters; i++) { + if (i > 0) { + travelled += Math.hypot(path[i * 2] - path[(i - 1) * 2], path[i * 2 + 1] - path[(i - 1) * 2 + 1]); + } + const cost = costAt(cm, path[i * 2], path[i * 2 + 1]); + if (cost !== null && cost !== COST_UNKNOWN && (worst === null || cost > worst)) worst = cost; + } + return worst; +} + +/** Signed heading error (rad) from the robot to PathAngleCritic's carrot point: + * the angle it must turn through to face the path. A diff-drive can only fix + * this by rotating, which is why a straight path can still produce pure spin. */ +export function headingErrorToCarrot(pose: Pose, path: Float32Array): number | null { + const n = path.length / 2; + if (n === 0) return null; + // The tracked path is already pruned to start near the robot, so the carrot + // is simply a fixed offset along it. + const i = Math.min(PATH_ANGLE_OFFSET, n - 1); + const dx = path[i * 2] - pose.x; + const dy = path[i * 2 + 1] - pose.y; + if (Math.hypot(dx, dy) < 1e-4) return null; + const bearing = Math.atan2(dy, dx); + return Math.atan2(Math.sin(bearing - pose.yaw), Math.cos(bearing - pose.yaw)); +} + +export interface CriticState { + name: string; + active: boolean; + /** Why it's on or off, in the reader's terms. */ + note: string; +} + +/** Which MPPI objectives are actually steering the robot right now, given how + * far it is from the goal and whether the path is passable. */ +export function criticStates( + distToGoal: number | null, + headingErr: number | null, + occupancy: number | null, +): CriticState[] { + const d = distToGoal; + const near = (limit: number) => (d === null ? null : d < limit); + const pathCriticsOn = near(PATH_CRITICS_OFF_WITHIN) === false; + const alignKilled = occupancy !== null && occupancy > PATH_ALIGN_MAX_OCCUPANCY; + + const unknown = (name: string): CriticState => ({ name, active: false, note: "no goal" }); + if (d === null) return ["PathFollow", "PathAlign", "PathAngle", "Goal", "GoalAngle", "PreferFwd"].map(unknown); + + return [ + { + name: "PathFollow", + active: pathCriticsOn, + note: pathCriticsOn ? "pulling along path" : `off: within ${PATH_CRITICS_OFF_WITHIN}m of goal`, + }, + { + name: "PathAlign", + active: pathCriticsOn && !alignKilled, + note: alignKilled + ? `off: path ${(occupancy! * 100).toFixed(0)}% blocked (>${PATH_ALIGN_MAX_OCCUPANCY * 100}%)` + : pathCriticsOn + ? "holding robot on the line" + : `off: within ${PATH_CRITICS_OFF_WITHIN}m of goal`, + }, + { + name: "PathAngle", + active: pathCriticsOn && headingErr !== null && Math.abs(headingErr) > PATH_ANGLE_MAX_ANGLE, + note: + !pathCriticsOn + ? `off: within ${PATH_CRITICS_OFF_WITHIN}m of goal` + : headingErr === null + ? "no path" + : Math.abs(headingErr) > PATH_ANGLE_MAX_ANGLE + ? `FORCING TURN: heading err ${((headingErr * 180) / Math.PI).toFixed(0)}deg > ${((PATH_ANGLE_MAX_ANGLE * 180) / Math.PI).toFixed(0)}deg` + : `dormant: heading err ${((headingErr * 180) / Math.PI).toFixed(0)}deg`, + }, + { + name: "Goal", + active: d < GOAL_CRITIC_ON_WITHIN, + note: d < GOAL_CRITIC_ON_WITHIN ? "driving to goal point" : `off: >${GOAL_CRITIC_ON_WITHIN}m away`, + }, + { + name: "GoalAngle", + active: d < GOAL_ANGLE_ON_WITHIN, + note: d < GOAL_ANGLE_ON_WITHIN ? "SETTLING YAW (rotates in place)" : `off: >${GOAL_ANGLE_ON_WITHIN}m away`, + }, + { + name: "PreferFwd", + active: d > PREFER_FORWARD_OFF_WITHIN, + note: d > PREFER_FORWARD_OFF_WITHIN ? "penalizing reverse" : `off: within ${PREFER_FORWARD_OFF_WITHIN}m of goal`, + }, + ]; +} + +/** Tracks recent yaw-rate sign changes: a controller wedged between equally + * bad options flips wz every few frames instead of committing. */ +export class OscillationDetector { + #signs: number[] = []; + #lastSign = 0; + #flips = 0; + + push(wz: number): void { + const sign = Math.abs(wz) < STALL_WZ ? 0 : Math.sign(wz); + if (sign !== 0 && this.#lastSign !== 0 && sign !== this.#lastSign) this.#flips++; + if (sign !== 0) this.#lastSign = sign; + this.#signs.push(this.#flips); + if (this.#signs.length > 40) this.#signs.shift(); // ~4s at 10Hz + } + + /** Sign changes observed across the retained window. */ + get flipsInWindow(): number { + if (this.#signs.length < 2) return 0; + return this.#signs[this.#signs.length - 1] - this.#signs[0]; + } +} diff --git a/sim/viewer/src/physics/rosbridgeController.ts b/sim/viewer/src/physics/rosbridgeController.ts index 4d0b09474..f0bed8546 100644 --- a/sim/viewer/src/physics/rosbridgeController.ts +++ b/sim/viewer/src/physics/rosbridgeController.ts @@ -4,7 +4,8 @@ // joints for the 3D view come from the world server's observer stream // (worldStateController) -- these stay here deliberately: they are robot // software outputs, so the robot's own pipeline is the honest source for the -// overlays. Read-only -- teleop/commands go through the webapp's own +// overlays. Read-only except for the ?navdebug freeze chip (setWorldPaused +// below, sim-only) -- teleop and everything else go through the webapp's own // rosbridge client. // // Speaks the rosbridge JSON protocol directly (subscribe/publish) -- no @@ -19,17 +20,59 @@ const LASER_OFFSET = { x: -0.0764, z: 0.17165 }; // built bundle, so the constant can't be imported across that boundary. const PLAN_TOPICS = ["/navigation/plan", "/mapfree/plan"]; +// The router forwards goals to an internal action server; whichever of the two +// relays feedback, we take it. Same only-one-publishes trick as PLAN_TOPICS. +const NAV_FEEDBACK_TOPICS = ["/navigate_to_pose/_action/feedback", "/internal_navigate_to_pose/_action/feedback"]; + +/** nav2_msgs/Costmap payload, flattened for the debug HUD. */ +export interface RawCostmap { + resolution: number; + sizeX: number; + sizeY: number; + originX: number; + originY: number; + data: Uint8Array; +} + +/** rosbridge sends uint8[] as either a base64 string or a plain number array + * depending on the bridge implementation; accept both. */ +function decodeCostmapData(data: string | number[]): Uint8Array { + if (typeof data !== "string") return Uint8Array.from(data); + const binary = atob(data); + const out = new Uint8Array(binary.length); + for (let i = 0; i < binary.length; i++) out[i] = binary.charCodeAt(i); + return out; +} + export class RosbridgePhysicsController { /** World-frame lidar hit points [x0,y0,z0, x1,...], null-range rays skipped. */ onScan?: (points: Float32Array) => void; - /** Planned-path ground points [x0,y0, x1,y1, ...]; empty = plan cleared. */ - onPlan?: (points: Float32Array) => void; + /** Planned-path ground points [x0,y0, x1,y1, ...]; empty = plan cleared. + * `goal` is the plan's final pose -- the navigation target. (The action goal + * itself is sent as a service call, so it can't be subscribed to; the + * planner always plans all the way to it, so the path's end is the goal.) */ + onPlan?: (points: Float32Array, goal: { x: number; y: number; yaw: number } | null) => void; /** MPPI's commanded body velocity (m/s, rad/s) from /cmd_vel_raw. */ onCommand?: (vx: number, wz: number) => void; + /** The final velocity reaching the wheels, after smoothing + the mux. */ + onFinalCommand?: (vx: number, wz: number) => void; /** Ground points of the path MPPI is tracking (transformed_global_plan). */ onFollowerPath?: (points: Float32Array) => void; + /** Nav2's own progress feedback for the active NavigateToPose goal. */ + onNavFeedback?: (fb: { distanceRemaining: number; recoveries: number; navTimeS: number }) => void; + /** The exact goal navigate_to_position commanded, in its fixed frame + * ("map", or "odom" for local goals) -- the true target, unlike the + * plan endpoint which wiggles with every replan. */ + onCommandedGoal?: (goal: { x: number; y: number; yaw: number; frame: string }) => void; + /** The controller's local costmap (raw 0-255 costs). */ + onCostmap?: (cm: RawCostmap) => void; #scanEnabled = false; - #followerEnabled = false; + #navDebugEnabled = false; + // AMCL's map->odom: in sim, odom == the ground-truth world frame, so this is + // the world<->map bridge. Identity until the first TF arrives. Without it, + // map-frame plans/goals drawn as world coordinates shift by however far + // AMCL has drifted -- goals appear to "jump". + #mapToOdom: { x: number; y: number; yaw: number } | null = null; #url: string; #ws!: WebSocket; @@ -60,13 +103,8 @@ export class RosbridgePhysicsController { ws.onopen = () => { this.#everOpened = true; this.#retryMs = 500; - // The planner republishes ~1Hz while driving; queue_length 1 keeps it - // latest-wins like the other feeds. - for (const topic of PLAN_TOPICS) { - this.#send({ op: "subscribe", topic, type: "nav_msgs/msg/Path", throttle_rate: 250, queue_length: 1 }); - } if (this.#scanEnabled) this.#subscribeScan(); - if (this.#followerEnabled) this.#subscribeFollower(); + if (this.#navDebugEnabled) this.#subscribeNavDebug(); this.#resolveOpen(); }; ws.onerror = () => { @@ -103,21 +141,53 @@ export class RosbridgePhysicsController { this.#send({ op: "subscribe", topic: "/scan", type: "sensor_msgs/msg/LaserScan", throttle_rate: 150, queue_length: 1 }); } - /** Start the follower overlay feeds (opt-in): the MPPI command and the path - * it's tracking. transformed_global_plan only publishes when the controller's - * `visualize` param is on (mars_nav controller.yaml). */ - enableFollower(): void { - if (this.#followerEnabled) return; - this.#followerEnabled = true; - this.#subscribeFollower(); + /** Start every navigation-debug feed (?navdebug only): the planner's route, + * the MPPI command and tracked path, Nav2's progress feedback, and the local + * costmap. transformed_global_plan/trajectories only publish when the + * controller's `visualize` param is on (mars_nav controller.yaml). + * Idempotent; survives reconnects via #connect. */ + enableNavDebug(): void { + if (this.#navDebugEnabled) return; + this.#navDebugEnabled = true; + this.#subscribeNavDebug(); + } + + /** map-frame point -> world (= odom) frame. P_odom = R(-yaw)·(P_map - t). */ + #mapToWorld(x: number, y: number): [number, number] { + const t = this.#mapToOdom; + if (!t) return [x, y]; + const dx = x - t.x; + const dy = y - t.y; + const c = Math.cos(-t.yaw); + const s = Math.sin(-t.yaw); + return [dx * c - dy * s, dx * s + dy * c]; } - #subscribeFollower(): void { + #yawMapToWorld(yaw: number): number { + return this.#mapToOdom ? yaw - this.#mapToOdom.yaw : yaw; + } + + #subscribeNavDebug(): void { + // The planner republishes ~1Hz while driving; queue_length 1 keeps every + // feed latest-wins. + for (const topic of PLAN_TOPICS) { + this.#send({ op: "subscribe", topic, type: "nav_msgs/msg/Path", throttle_rate: 250, queue_length: 1 }); + } // /cmd_vel_raw is the controller's own output, pre-smoothing/mux -- the - // honest "what the follower commands". 20Hz raw; throttle to keep the - // arrow lively without flooding. + // honest "what the follower commands"; /cmd_vel is what survives the mux. this.#send({ op: "subscribe", topic: "/cmd_vel_raw", type: "geometry_msgs/msg/Twist", throttle_rate: 100, queue_length: 1 }); + this.#send({ op: "subscribe", topic: "/cmd_vel", type: "geometry_msgs/msg/Twist", throttle_rate: 100, queue_length: 1 }); this.#send({ op: "subscribe", topic: "/transformed_global_plan", type: "nav_msgs/msg/Path", throttle_rate: 100, queue_length: 1 }); + for (const topic of NAV_FEEDBACK_TOPICS) { + this.#send({ op: "subscribe", topic, throttle_rate: 200, queue_length: 1 }); + } + this.#send({ op: "subscribe", topic: "/nav/commanded_goal", type: "geometry_msgs/msg/PoseStamped", queue_length: 1 }); + // map->odom from AMCL rides /tf among the 30Hz odom transforms; no + // throttle (tiny messages) so we never starve on the rarer map->odom. + this.#send({ op: "subscribe", topic: "/tf", type: "tf2_msgs/msg/TFMessage", queue_length: 1 }); + // The costmap is the biggest payload here (a 6x6m @5cm grid); 2Hz is + // plenty to explain why forward rollouts are expensive. + this.#send({ op: "subscribe", topic: "/local_costmap/costmap_raw", type: "nav2_msgs/msg/Costmap", throttle_rate: 500, queue_length: 1 }); } dispose(): void { @@ -133,21 +203,52 @@ export class RosbridgePhysicsController { }; const { position, orientation } = odom.pose.pose; this.#pose = { x: position.x, y: position.y, yaw: 2 * Math.atan2(orientation.z, orientation.w) }; + } else if (msg.topic === "/tf") { + const tf = msg.msg as { + transforms?: Array<{ + header: { frame_id: string }; + child_frame_id: string; + transform: { translation: { x: number; y: number }; rotation: { z: number; w: number } }; + }>; + }; + for (const tr of tf.transforms ?? []) { + if (tr.header.frame_id === "map" && tr.child_frame_id === "odom") { + const { translation, rotation } = tr.transform; + this.#mapToOdom = { x: translation.x, y: translation.y, yaw: 2 * Math.atan2(rotation.z, rotation.w) }; + } + } } else if (msg.topic !== undefined && PLAN_TOPICS.includes(msg.topic) && this.onPlan) { - // Plan poses arrive in the map/odom frame; the sim's world frame is the - // map frame (ground-truth-seeded localization), so use them directly -- - // the same assumption the webapp's 2D map widget makes. - const path = msg.msg as { poses?: Array<{ pose: { position: { x: number; y: number } } }> }; + // Everything is drawn in the world frame (= odom in sim, since the + // driver's odometry is ground truth). Map-frame plans are transformed + // through AMCL's map->odom, so localization drift can't shift them. + const path = msg.msg as { + header?: { frame_id?: string }; + poses?: Array<{ pose: { position: { x: number; y: number }; orientation: { z: number; w: number } } }>; + }; if (!Array.isArray(path.poses)) return; + const inMap = path.header?.frame_id === "map"; const points = new Float32Array(path.poses.length * 2); path.poses.forEach((p, i) => { - points[i * 2] = p.pose.position.x; - points[i * 2 + 1] = p.pose.position.y; + const [x, y] = inMap ? this.#mapToWorld(p.pose.position.x, p.pose.position.y) : [p.pose.position.x, p.pose.position.y]; + points[i * 2] = x; + points[i * 2 + 1] = y; }); - this.onPlan(points); + const end = path.poses[path.poses.length - 1]; + const endYaw = end ? 2 * Math.atan2(end.pose.orientation.z, end.pose.orientation.w) : 0; + const goal = end + ? { + x: points[points.length - 2], + y: points[points.length - 1], + yaw: inMap ? this.#yawMapToWorld(endYaw) : endYaw, + } + : null; + this.onPlan(points, goal); } else if (msg.topic === "/cmd_vel_raw" && this.onCommand) { const twist = msg.msg as { linear: { x: number }; angular: { z: number } }; this.onCommand(twist.linear.x, twist.angular.z); + } else if (msg.topic === "/cmd_vel" && this.onFinalCommand) { + const twist = msg.msg as { linear: { x: number }; angular: { z: number } }; + this.onFinalCommand(twist.linear.x, twist.angular.z); } else if (msg.topic === "/transformed_global_plan" && this.onFollowerPath) { const path = msg.msg as { poses?: Array<{ pose: { position: { x: number; y: number } } }> }; if (!Array.isArray(path.poses)) return; @@ -157,6 +258,54 @@ export class RosbridgePhysicsController { points[i * 2 + 1] = p.pose.position.y; }); this.onFollowerPath(points); + } else if (msg.topic === "/nav/commanded_goal" && this.onCommandedGoal) { + const ps = msg.msg as { + header: { frame_id: string }; + pose: { position: { x: number; y: number }; orientation: { z: number; w: number } }; + }; + const rawYaw = 2 * Math.atan2(ps.pose.orientation.z, ps.pose.orientation.w); + const inMap = ps.header.frame_id === "map"; + const [x, y] = inMap ? this.#mapToWorld(ps.pose.position.x, ps.pose.position.y) : [ps.pose.position.x, ps.pose.position.y]; + this.onCommandedGoal({ + x, + y, + yaw: inMap ? this.#yawMapToWorld(rawYaw) : rawYaw, + frame: ps.header.frame_id, + }); + } else if (msg.topic !== undefined && NAV_FEEDBACK_TOPICS.includes(msg.topic) && this.onNavFeedback) { + const wrapper = msg.msg as { + feedback?: { + distance_remaining?: number; + number_of_recoveries?: number; + navigation_time?: { sec: number; nanosec: number }; + }; + }; + const fb = wrapper.feedback; + if (!fb) return; + this.onNavFeedback({ + distanceRemaining: fb.distance_remaining ?? 0, + recoveries: fb.number_of_recoveries ?? 0, + navTimeS: (fb.navigation_time?.sec ?? 0) + (fb.navigation_time?.nanosec ?? 0) / 1e9, + }); + } else if (msg.topic === "/local_costmap/costmap_raw" && this.onCostmap) { + const cm = msg.msg as { + metadata?: { + resolution: number; + size_x: number; + size_y: number; + origin: { position: { x: number; y: number } }; + }; + data?: string | number[]; + }; + if (!cm.metadata || cm.data === undefined) return; + this.onCostmap({ + resolution: cm.metadata.resolution, + sizeX: cm.metadata.size_x, + sizeY: cm.metadata.size_y, + originX: cm.metadata.origin.position.x, + originY: cm.metadata.origin.position.y, + data: decodeCostmapData(cm.data), + }); } else if (msg.topic === "/scan" && this.onScan) { const scan = msg.msg as { angle_min: number; angle_increment: number; range_max: number; ranges: number[] }; const { x, y, yaw } = this.#pose; @@ -174,6 +323,37 @@ export class RosbridgePhysicsController { } } + /** Freeze/unfreeze the simulated world (?navdebug freeze chip). Sim-only: + * mars_sim_driver holds physics stepping while the robot stack keeps running. */ + setWorldPaused(on: boolean): void { + this.#send({ op: "publish", topic: "/virtual_mars/pause", msg: { data: on } }); + } + + /** Send a navigation goal picked in WORLD coordinates (?navdebug set-goal + * chip) -- converted into AMCL's map frame before publishing on /goal_pose, + * the same route the webapp map's Set Goal uses. Zero stamp = "latest" to + * TF; never wall time, the sim runs on ROS sim time. */ + 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" }, + pose: { + position: { x, y, z: 0 }, + orientation: { x: 0, y: 0, z: Math.sin(yaw / 2), w: Math.cos(yaw / 2) }, + }, + }, + }); + } + #send(payload: object): void { if (this.#ws.readyState === WebSocket.OPEN) this.#ws.send(JSON.stringify(payload)); } diff --git a/sim/viewer/src/scene.ts b/sim/viewer/src/scene.ts index 88bc7b2aa..55551adbd 100644 --- a/sim/viewer/src/scene.ts +++ b/sim/viewer/src/scene.ts @@ -66,6 +66,8 @@ export class SimScene { private followerRibbon?: THREE.Mesh; private cmdLinearArrow?: THREE.ArrowHelper; private cmdTurnArrow?: THREE.ArrowHelper; + private goalMarker?: THREE.Group; + private goalPreview?: THREE.Group; private hullsGroup?: THREE.Group; private hullsPromise?: Promise; private hullsVisible = false; @@ -295,6 +297,75 @@ export class SimScene { } } + /** Ring + heading arrow on the floor, used for both the live goal marker + * and the set-goal placement preview. */ + private static makeGoalGroup(opacity: number): THREE.Group { + const group = new THREE.Group(); + const color = 0x00ff88; // matches the 2D map widget's goal dot + const ring = new THREE.Mesh( + // RingGeometry already lies in the XY plane -- our ground plane. + new THREE.RingGeometry(0.11, 0.15, 32), + new THREE.MeshBasicMaterial({ color, transparent: true, opacity, side: THREE.DoubleSide, depthWrite: false }), + ); + const heading = new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), new THREE.Vector3(0, 0, 0), 0.3, color, 0.08, 0.05); + heading.name = "heading"; + group.add(ring, heading); + group.frustumCulled = false; + return group; + } + + /** Mark the navigation target on the floor: a ring at the goal position with + * an arrow for the goal heading (what GoalAngleCritic settles onto). */ + setGoalMarker(x: number, y: number, yaw: number): void { + if (!this.goalMarker) { + this.goalMarker = SimScene.makeGoalGroup(0.9); + this.scene.add(this.goalMarker); + } + // Just above the ribbons so it stays legible where the path ends on it. + this.goalMarker.position.set(x, y, SimScene.FOLLOWER_Z + 0.005); + this.goalMarker.rotation.set(0, 0, yaw); + this.goalMarker.visible = true; + } + + setGoalVisible(visible: boolean): void { + if (this.goalMarker) this.goalMarker.visible = visible; + } + + /** Ghost marker following the cursor while placing a goal (set-goal chip). + * `yaw` null hides the heading arrow (position picked, heading not yet). */ + setGoalPreview(x: number, y: number, yaw: number | null): void { + if (!this.goalPreview) { + this.goalPreview = SimScene.makeGoalGroup(0.5); + this.scene.add(this.goalPreview); + } + this.goalPreview.position.set(x, y, SimScene.FOLLOWER_Z + 0.005); + this.goalPreview.rotation.set(0, 0, yaw ?? 0); + const heading = this.goalPreview.getObjectByName("heading"); + if (heading) heading.visible = yaw !== null; + this.goalPreview.visible = true; + } + + clearGoalPreview(): void { + if (this.goalPreview) this.goalPreview.visible = false; + } + + /** World-frame ground point (z=0 plane) under a pointer event, or null when + * the ray misses the ground (e.g. pointing at the sky). Uses the active + * render camera, so it works in orbit and robot views alike. */ + groundPoint(clientX: number, clientY: number): { x: number; y: number } | null { + const rect = this.renderer.domElement.getBoundingClientRect(); + const ndc = new THREE.Vector2( + ((clientX - rect.left) / rect.width) * 2 - 1, + -((clientY - rect.top) / rect.height) * 2 + 1, + ); + const cam = (this.activeView !== "orbit" ? this.robotCameras.get(this.activeView) : undefined) ?? this.camera; + const raycaster = new THREE.Raycaster(); + raycaster.setFromCamera(ndc, cam); + const hit = new THREE.Vector3(); + const ground = new THREE.Plane(new THREE.Vector3(0, 0, 1), 0); // z = 0 + return raycaster.ray.intersectPlane(ground, hit) ? { x: hit.x, y: hit.y } : null; + } + setFollowerVisible(visible: boolean): void { if (this.followerRibbon) this.followerRibbon.visible = visible; // Arrows are re-driven by setCommandVelocity each tick while on; here we diff --git a/sim/viewer/src/simSession.ts b/sim/viewer/src/simSession.ts index d46bffaaa..9964e1d7d 100644 --- a/sim/viewer/src/simSession.ts +++ b/sim/viewer/src/simSession.ts @@ -6,8 +6,45 @@ // Architecture: sim/README.md. import type { SimScene } from "./scene"; -import { RosbridgePhysicsController } from "./physics/rosbridgeController"; +import { RosbridgePhysicsController, type RawCostmap } from "./physics/rosbridgeController"; import { WorldStateController } from "./physics/worldStateController"; +import { + OscillationDetector, + costAt, + costLabel, + criticStates, + headingErrorToCarrot, + maxCostAhead, + pathOccupancyRatio, + STALL_VX, + STALL_WZ, + type CriticState, +} from "./navDebug"; + +/** Nav debugging (planner route, follower overlay, HUD) is opt-in per URL so + * the normal sim view stays uncluttered and pays no rosbridge cost. */ +export function navDebugEnabled(): boolean { + return new URLSearchParams(location.search).has("navdebug"); +} + +/** One frame of derived MPPI decision state for the ?navdebug HUD. */ +export interface NavDebugSnapshot { + cmd: { vx: number; wz: number } | null; + finalCmd: { vx: number; wz: number } | null; + stalled: boolean; + wzFlips: number; + /** The navigation target: the skill's commanded goal when available + * (frame + commanded=true), else the plan endpoint as a fallback. */ + goal: { x: number; y: number; yaw: number; frame?: string; commanded: boolean } | null; + distanceRemaining: number | null; + recoveries: number | null; + navTimeS: number | null; + costUnderRobot: string; + maxCostAhead: string; + pathOccupancy: number | null; + headingErrDeg: number | null; + critics: CriticState[]; +} /** PiP tile render size; square to match the webapp's .cam-tile. */ export const THUMB_W = 240; @@ -87,16 +124,26 @@ export class SimSession { #hullsOn = false; #overlaysDirty = false; - // Nav2 planned path (always-on overlay, shown while navigating). The - // planner republishes ~1Hz the whole time it's driving, so a lull means - // navigation ended -- same staleness rule as the webapp's 2D map. + // --- ?navdebug only (see navDebugEnabled) --- + #navDebug = navDebugEnabled(); + + // Nav2 planned path, shown while navigating. The planner republishes ~1Hz + // the whole time it's driving, so a lull means navigation ended -- same + // staleness rule as the webapp's 2D map. #plan: Float32Array | null = null; #planDirty = false; #planStaleAt = 0; static readonly #PLAN_STALE_S = 4; - - // Follower overlay (opt-in "follower" chip): the MPPI command arrow + the - // path it's tracking. Both go stale quickly once the controller stops. + /** Fallback navigation target: the plan's final pose. Cleared with the plan. */ + #goal: { x: number; y: number; yaw: number } | null = null; + #goalDirty = false; + /** The exact goal navigate_to_position commanded (preferred over #goal -- + * the plan endpoint wiggles with every replan, this is the true target). + * Cleared with the plan: the planner going quiet means navigation ended. */ + #commandedGoal: { x: number; y: number; yaw: number; frame: string } | null = null; + + // Follower overlay ("follower" chip): the MPPI command arrow + the path it's + // tracking. Both go stale quickly once the controller stops. #followerOn = false; #followerDirty = false; #cmd: { vx: number; wz: number } | null = null; @@ -106,6 +153,19 @@ export class SimSession { #followerStaleAt = 0; static readonly #FOLLOWER_STALE_S = 1; + // HUD inputs: everything else MPPI's decision is gated on. + #finalCmd: { vx: number; wz: number } | null = null; + #navFeedback: { distanceRemaining: number; recoveries: number; navTimeS: number } | null = null; + #costmap: RawCostmap | null = null; + #oscillation = new OscillationDetector(); + #lastPose: { x: number; y: number; yaw: number } | null = null; + + // Freeze chip: pauses the simulated world in place. The robot stack (Nav2, + // MPPI) keeps running against a world that stops advancing, so the planner + // and controller keep publishing and the overlays stay live -- just static. + // Unfreezing resumes exactly where it left off. + #frozen = false; + #stateUrls: string[]; #rosUrl: string; @@ -155,31 +215,61 @@ export class SimSession { // directly (thumbnailCanvas below). this.#connectState(0); - this.#connectRosFeed(); + if (this.#navDebug) this.#ensureRosFeed().enableNavDebug(); } - /** Open the rosbridge feed for the always-on plan overlay; the scan feed - * piggybacks on the same connection when the lidar chip enables it. */ - #connectRosFeed(): void { - this.#scanFeed = new RosbridgePhysicsController(this.#rosUrl); - this.#scanFeed.onPlan = (points) => { + /** The rosbridge connection is shared by the lidar overlay and the nav-debug + * feeds, and opened by whichever needs it first -- a plain sim view never + * opens it at all. */ + #ensureRosFeed(): RosbridgePhysicsController { + if (this.#scanFeed) return this.#scanFeed; + const feed = new RosbridgePhysicsController(this.#rosUrl); + this.#scanFeed = feed; + feed.onScan = (points) => { + this.#scan = points; + this.#scanDirty = true; + }; + feed.onPlan = (points, goal) => { this.#plan = points; this.#planDirty = true; this.#planStaleAt = performance.now() / 1000 + SimSession.#PLAN_STALE_S; + if (goal) { + this.#goal = goal; + this.#goalDirty = true; + } }; - this.#scanFeed.onCommand = (vx, wz) => { + feed.onCommand = (vx, wz) => { this.#cmd = { vx, wz }; this.#cmdDirty = true; + this.#oscillation.push(wz); this.#followerStaleAt = performance.now() / 1000 + SimSession.#FOLLOWER_STALE_S; }; - this.#scanFeed.onFollowerPath = (points) => { + feed.onFinalCommand = (vx, wz) => { + this.#finalCmd = { vx, wz }; + }; + feed.onFollowerPath = (points) => { this.#followerPath = points; this.#followerPathDirty = true; this.#followerStaleAt = performance.now() / 1000 + SimSession.#FOLLOWER_STALE_S; }; - this.#scanFeed.init().catch((err) => console.warn("[sim-session] rosbridge overlays unavailable:", err)); - if (this.#lidarOn) this.setLidarVisible(true); // chip toggled before start() - if (this.#followerOn) this.setFollowerVisible(true); + feed.onNavFeedback = (fb) => { + this.#navFeedback = fb; + }; + feed.onCommandedGoal = (goal) => { + this.#commandedGoal = goal; + this.#goalDirty = true; + }; + feed.onCostmap = (cm) => { + this.#costmap = cm; + }; + feed.init().catch((err) => console.warn("[sim-session] rosbridge overlays unavailable:", err)); + return feed; + } + + /** True when the ?navdebug flag is set: the stage shows the follower chip + * and the decision HUD. */ + get navDebug(): boolean { + return this.#navDebug; } /** Connect the state feed, falling through the URL candidates (direct @@ -235,12 +325,18 @@ export class SimSession { // Drop buffered overlay data so a restart doesn't redraw a stale route/scan. this.#plan = null; this.#planDirty = false; + this.#goal = null; + this.#goalDirty = false; + this.#commandedGoal = null; this.#scan = null; this.#scanDirty = false; this.#cmd = null; this.#cmdDirty = false; this.#followerPath = null; this.#followerPathDirty = false; + this.#finalCmd = null; + this.#navFeedback = null; + this.#costmap = null; this.#started = false; this.#gotPose = false; this.#patch({ status: "idle", videoStream: null }); @@ -251,18 +347,12 @@ export class SimSession { this.#listeners.clear(); } - /** Toggle the /scan hit-point overlay (stage "lidar" chip). The scan - * subscription starts on first use, on the plan overlay's connection. */ + /** Toggle the /scan hit-point overlay (stage "lidar" chip); opens the + * rosbridge connection on first use. */ setLidarVisible(on: boolean): void { this.#lidarOn = on; this.#overlaysDirty = true; - if (on && this.#scanFeed !== null && !this.#scanFeed.onScan) { - this.#scanFeed.onScan = (points) => { - this.#scan = points; - this.#scanDirty = true; - }; - this.#scanFeed.enableScan(); - } + if (on) this.#ensureRosFeed().enableScan(); } /** Toggle the collision-hull wireframe overlay (stage "collisions" chip). */ @@ -272,12 +362,33 @@ export class SimSession { } /** Toggle the follower overlay (stage "follower" chip): MPPI's command arrow - * + the path it's tracking. Subscribes on first enable; needs the - * controller's `visualize` param on for the tracked-path feed. */ + * + the path it's tracking. The feeds are already subscribed under ?navdebug; + * this only governs rendering. */ setFollowerVisible(on: boolean): void { this.#followerOn = on; this.#followerDirty = true; - if (on && this.#scanFeed !== null) this.#scanFeed.enableFollower(); + } + + /** Freeze/unfreeze the simulated world (stage "freeze" chip). Pauses physics + * in place: the robot halts, navigation stays exactly where it was, and the + * overlays hold. Unfreezing continues from that point. */ + setFrozen(on: boolean): void { + this.#frozen = on; + this.#ensureRosFeed().setWorldPaused(on); + } + + get frozen(): boolean { + return this.#frozen; + } + + /** Send a map-frame navigation goal (set-goal chip). */ + publishGoalPose(x: number, y: number, yaw: number): void { + this.#ensureRosFeed().publishGoalPose(x, y, yaw); + } + + /** Latest interpolated robot pose the scene is showing (world frame). */ + get robotPose(): { x: number; y: number; yaw: number } | null { + return this.#lastPose; } // WebRTC-specific surface: harmless no-ops in sim. @@ -346,7 +457,9 @@ export class SimSession { const x = a.x + (b.x - a.x) * u; const y = a.y + (b.y - a.y) * u; const dyaw = Math.atan2(Math.sin(b.yaw - a.yaw), Math.cos(b.yaw - a.yaw)); - scene.setPose(x, y, a.yaw + dyaw * u); + const yaw = a.yaw + dyaw * u; + scene.setPose(x, y, yaw); + this.#lastPose = { x, y, yaw }; const joints: Record = {}; for (const [name, va] of Object.entries(a.joints)) { @@ -366,12 +479,30 @@ export class SimSession { scene.setLidarVisible(true); // first points may arrive after the toggle } + if (!this.#navDebug) return; // plan + follower overlays are ?navdebug only + + // While the world is frozen, ROS time is stopped, so the planner and + // controller legitimately go quiet -- suspend staleness expiry (and let + // the timers re-arm on the first message after unfreeze). + if (this.#frozen) { + this.#planStaleAt = performance.now() / 1000 + SimSession.#PLAN_STALE_S; + this.#followerStaleAt = performance.now() / 1000 + SimSession.#FOLLOWER_STALE_S; + } + if (this.#planDirty && this.#plan) { this.#planDirty = false; scene.setPlanPoints(this.#plan); } else if (this.#plan && performance.now() / 1000 > this.#planStaleAt) { this.#plan = null; // planner went quiet: navigation ended + this.#goal = null; + this.#commandedGoal = null; scene.setPlanVisible(false); + scene.setGoalVisible(false); + } + const goal = this.#commandedGoal ?? this.#goal; + if (this.#goalDirty && goal) { + this.#goalDirty = false; + scene.setGoalMarker(goal.x, goal.y, goal.yaw); } if (this.#followerDirty) { @@ -401,6 +532,46 @@ export class SimSession { } } + /** Derived MPPI decision state for the ?navdebug HUD. MPPI never publishes + * its per-critic costs, so instead we report what those critics are gated + * on -- distance to goal, heading error, and costmap occupancy -- which is + * what actually decides whether it drives, turns, or sits still. + * + * Stays live while the world is frozen: the robot stack keeps publishing + * against the paused world, so the numbers simply stop changing. */ + get navDebugSnapshot(): NavDebugSnapshot | null { + if (!this.#navDebug) return null; + const stale = performance.now() / 1000 > this.#followerStaleAt; + const cmd = stale ? null : this.#cmd; + const path = stale ? null : this.#followerPath; + const cm = this.#costmap; + const pose = this.#lastPose; + + const occupancy = cm && path ? pathOccupancyRatio(cm, path) : null; + const headingErr = pose && path ? headingErrorToCarrot(pose, path) : null; + const distance = stale ? null : (this.#navFeedback?.distanceRemaining ?? null); + + return { + cmd, + finalCmd: stale ? null : this.#finalCmd, + stalled: cmd !== null && Math.abs(cmd.vx) < STALL_VX && Math.abs(cmd.wz) < STALL_WZ, + wzFlips: this.#oscillation.flipsInWindow, + goal: this.#commandedGoal + ? { ...this.#commandedGoal, commanded: true } + : this.#goal + ? { ...this.#goal, commanded: false } + : null, + distanceRemaining: distance, + recoveries: stale ? null : (this.#navFeedback?.recoveries ?? null), + navTimeS: stale ? null : (this.#navFeedback?.navTimeS ?? null), + costUnderRobot: cm && pose ? costLabel(costAt(cm, pose.x, pose.y)) : "—", + maxCostAhead: cm && path ? costLabel(maxCostAhead(cm, path, 0.5)) : "—", + pathOccupancy: occupancy, + headingErrDeg: headingErr === null ? null : (headingErr * 180) / Math.PI, + critics: criticStates(distance, headingErr, occupancy), + }; + } + /** Active, non-primary views whose PiP tiles need frames. */ liveThumbnails(): { index: number; name: string }[] { return this.#roster diff --git a/sim/viewer/src/simStage.ts b/sim/viewer/src/simStage.ts index 0531a8530..025782307 100644 --- a/sim/viewer/src/simStage.ts +++ b/sim/viewer/src/simStage.ts @@ -49,19 +49,58 @@ export function createSimStage(parent: HTMLElement, session: SimSession): { audi `padding:4px 10px;border-radius:999px;border:1px solid rgba(255,255,255,.25);background:${OFF_BG};` + "color:rgba(255,255,255,.75);font:500 11px system-ui;cursor:pointer;"; let on = false; - b.onclick = () => { - on = !on; + const apply = () => { b.style.background = on ? ON_BG : OFF_BG; b.style.color = on ? "#7dffc4" : "rgba(255,255,255,.75)"; + }; + b.onclick = () => { + on = !on; + apply(); onToggle(on); }; chips.appendChild(b); + // Programmatic toggle (e.g. set-goal auto-exits after placing one goal). + return { + set(v: boolean) { + if (on === v) return; + on = v; + apply(); + onToggle(on); + }, + }; }; addChip("lidar", (on) => session.setLidarVisible(on)); addChip("collisions", (on) => session.setCollisionHullsVisible(on)); - addChip("follower", (on) => session.setFollowerVisible(on)); + // Set-goal placement mode ("set goal" chip): click the floor to pick the + // position, drag to choose the heading, release to send. Auto-exits after + // one goal; while active the orbit drag is suspended (see the render loop). + let goalMode = false; + let goalChip: { set(v: boolean): void } | null = null; + if (session.navDebug) { + addChip("follower", (on) => session.setFollowerVisible(on)); + // Freeze pauses the simulated world in place (physics stops, the robot + // stack keeps running); unfreezing continues the navigation mid-flight. + addChip("freeze", (on) => session.setFrozen(on)); + goalChip = addChip("set goal", (on) => { + goalMode = on; + canvas.style.cursor = on ? "crosshair" : ""; + }); + } debugStack.appendChild(chips); + // ?navdebug: why is the follower doing that? Reports the state MPPI's + // critics are gated on, since it never publishes the costs themselves. + let navEl: HTMLElement | null = null; + let navNextAt = 0; + if (session.navDebug) { + navEl = document.createElement("div"); + navEl.style.cssText = + "align-self:flex-start;max-width:340px;padding:6px 9px;border-radius:6px;" + + "background:rgba(0,0,0,.72);color:#cfe;font:11px/1.5 ui-monospace,monospace;" + + "pointer-events:none;white-space:pre;"; + debugStack.prepend(navEl); + } + // Loading overlay: spinner + staged label instead of a black canvas until // assets and the first world state arrive. const loading = document.createElement("div"); @@ -128,9 +167,101 @@ export function createSimStage(parent: HTMLElement, session: SimSession): { audi debugStack.prepend(perfEl); } + /** Render one frame of the nav-debug HUD. Kept text-only + monospace so the + * numbers line up and it never costs a layout reflow worth measuring. */ + const renderNavHud = (el: HTMLElement) => { + const s = session.navDebugSnapshot; + if (!s) return; + const n = (v: number | null, digits = 2, unit = "") => (v === null ? "—" : `${v.toFixed(digits)}${unit}`); + const lines: string[] = []; + + // Make a paused world unmistakable -- the numbers below are live but static. + if (session.frozen) lines.push("❚❚ WORLD FROZEN — physics paused, unfreeze to continue"); + + if (s.cmd === null) { + lines.push("COMMAND idle (controller not running)"); + } else { + const badge = s.stalled ? " ⚠ STALLED" : s.wzFlips >= 4 ? ` ⚠ OSCILLATING (${s.wzFlips} flips/4s)` : ""; + lines.push(`COMMAND vx ${n(s.cmd.vx)} m/s wz ${n(s.cmd.wz)} rad/s${badge}`); + if (s.finalCmd) lines.push(` ->wheels vx ${n(s.finalCmd.vx)} wz ${n(s.finalCmd.wz)}`); + } + + const goalAt = s.goal + ? `(${s.goal.x.toFixed(2)}, ${s.goal.y.toFixed(2)}, ${((s.goal.yaw * 180) / Math.PI).toFixed(0)}°) ` + + (s.goal.commanded ? `[${s.goal.frame ?? "?"} frame, commanded]` : "[plan end]") + : "none"; + lines.push(`GOAL ${goalAt}`); + lines.push( + ` ${n(s.distanceRemaining, 2, " m")} away recoveries ${s.recoveries ?? "—"} t ${n(s.navTimeS, 0, "s")}`, + ); + lines.push( + `BLOCKING under robot: ${s.costUnderRobot} 0.5m ahead: ${s.maxCostAhead}` + + (s.pathOccupancy === null ? "" : `\n path blocked ${(s.pathOccupancy * 100).toFixed(0)}%`) + + (s.headingErrDeg === null ? "" : ` heading err ${s.headingErrDeg.toFixed(0)}°`), + ); + lines.push("CRITICS"); + for (const c of s.critics) { + lines.push(` ${c.active ? "●" : "○"} ${c.name.padEnd(11)}${c.note}`); + } + el.textContent = lines.join("\n"); + }; + const scene = new SimScene(canvas, { fixedSize: { width: parent.clientWidth || 1280, height: parent.clientHeight || 720 } }); scene.followCamera = true; + // Set-goal placement: hover shows a ghost ring on the floor; press picks + // the position; dragging picks the heading; release publishes /goal_pose. + // A sub-15cm drag means "no heading preference" -> face the goal from the + // robot's side, the direction it will arrive from. + let goalDrag: { x: number; y: number } | null = null; + const finishGoal = (e: PointerEvent) => { + if (!goalDrag) return; + const start = goalDrag; + goalDrag = null; + const cur = scene.groundPoint(e.clientX, e.clientY); + const dx = cur ? cur.x - start.x : 0; + const dy = cur ? cur.y - start.y : 0; + let yaw: number; + if (Math.hypot(dx, dy) > 0.15) { + yaw = Math.atan2(dy, dx); + } else { + const robot = session.robotPose; + yaw = robot ? Math.atan2(start.y - robot.y, start.x - robot.x) : 0; + } + session.publishGoalPose(start.x, start.y, yaw); + scene.clearGoalPreview(); + goalChip?.set(false); // one goal per activation, like the map widget + }; + canvas.addEventListener("pointerdown", (e) => { + if (!goalMode) return; + e.preventDefault(); + const p = scene.groundPoint(e.clientX, e.clientY); + if (!p) return; + goalDrag = p; + canvas.setPointerCapture(e.pointerId); + scene.setGoalPreview(p.x, p.y, null); + }); + canvas.addEventListener("pointermove", (e) => { + if (!goalMode) { + scene.clearGoalPreview(); + return; + } + const cur = scene.groundPoint(e.clientX, e.clientY); + if (!cur) return; + if (goalDrag) { + const dx = cur.x - goalDrag.x; + const dy = cur.y - goalDrag.y; + scene.setGoalPreview(goalDrag.x, goalDrag.y, Math.hypot(dx, dy) > 0.15 ? Math.atan2(dy, dx) : null); + } else { + scene.setGoalPreview(cur.x, cur.y, null); // hover ghost while aiming + } + }); + canvas.addEventListener("pointerup", finishGoal); + canvas.addEventListener("pointercancel", () => { + goalDrag = null; + scene.clearGoalPreview(); + }); + const resize = () => { const w = wrap.clientWidth; const h = wrap.clientHeight; @@ -171,9 +302,19 @@ export function createSimStage(parent: HTMLElement, session: SimSession): { audi } // ...then the primary view full-frame on top. scene.setView(VIEW_FOR[session.primaryCamera] ?? "orbit"); + // setView re-enables orbit every frame; goal placement must keep the drag + // for itself, so re-suspend it here rather than fighting setView's state. + if (goalMode) scene.controls.enabled = false; scene.render(); frame++; + // 5Hz: the snapshot walks the costmap along the path, and nothing here + // changes faster than the eye can read. + if (navEl && now >= navNextAt) { + navNextAt = now + 200; + renderNavHud(navEl); + } + if (perfEl) { frameTimes.push(performance.now() - now); if (now >= perfNextAt) { diff --git a/webapp/js/constants.js b/webapp/js/constants.js index 6b9e5f1d1..1d56cde94 100644 --- a/webapp/js/constants.js +++ b/webapp/js/constants.js @@ -62,6 +62,10 @@ export const PLAN_TOPICS = ["/navigation/plan", "/mapfree/plan"]; // smoothing and the teleop/skills/nav mux. Shown as a velocity arrow so a // stalled/oscillating follower is visible against the plan. export const CMD_VEL_RAW_TOPIC = "/cmd_vel_raw"; +// The exact goal navigate_to_position commanded (geometry_msgs/PoseStamped, +// latched; frame_id is "map", or "odom" for local goals) — the true target, +// unlike the plan endpoint which wiggles with every replan. +export const COMMANDED_GOAL_TOPIC = "/nav/commanded_goal"; // Click-to-navigate goal. Publishing a geometry_msgs/PoseStamped here kicks off // planning; the resulting route streams back on PLAN_TOPICS. Same topic the sim // console's map view publishes to. diff --git a/webapp/js/map/mapWidget.js b/webapp/js/map/mapWidget.js index 1b6748725..9ca8b3f4f 100644 --- a/webapp/js/map/mapWidget.js +++ b/webapp/js/map/mapWidget.js @@ -7,7 +7,15 @@ // is all a 2D map needs. import { ros } from "../rosClient.js"; -import { MAP_TOPIC, ODOM_TOPIC, PLAN_TOPICS, CMD_VEL_RAW_TOPIC, GOAL_POSE_TOPIC, CANCEL_NAVIGATION_SERVICE } from "../constants.js"; +import { + MAP_TOPIC, + ODOM_TOPIC, + PLAN_TOPICS, + CMD_VEL_RAW_TOPIC, + COMMANDED_GOAL_TOPIC, + GOAL_POSE_TOPIC, + CANCEL_NAVIGATION_SERVICE, +} from "../constants.js"; // Wheel-zoom bounds (metres of real-world width shown). const MIN_ZOOM_M = 1; @@ -50,6 +58,9 @@ export function createMap(root, opts = {}) { let pose = null; /** @type {Array<{ x: number, y: number }> | null} world-frame plan points */ let plan = null; + // The follower's command arrow is a debugging overlay: opt in with ?navdebug, + // the same flag the sim viewer's nav overlays use. + const navDebug = new URLSearchParams(location.search).has("navdebug"); /** @type {{ vx: number, wz: number } | null} latest follower command (body frame) */ let cmd = null; /** @type {ReturnType | undefined} clears the arrow when the controller goes quiet */ @@ -65,6 +76,9 @@ export function createMap(root, opts = {}) { let goalDrag = null; /** @type {{ x: number, y: number, yaw: number } | null} the active goal */ let goalMarker = null; + // True once /nav/commanded_goal set the marker for this navigation: the + // skill's exact target then wins over the per-replan plan endpoint. + let goalIsCommanded = false; /** @type {ReturnType | undefined} */ let navStaleTimer; @@ -167,11 +181,23 @@ export function createMap(root, opts = {}) { clearTimeout(navStaleTimer); navStaleTimer = setTimeout(() => { goalMarker = null; + goalIsCommanded = false; plan = null; draw(); }, NAV_STALE_MS); } + /** @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(); + } + /** @param {any} msg nav_msgs/Path */ function onPlan(msg) { const poses = msg?.poses; @@ -183,10 +209,24 @@ 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 }; + } + } armNavStale(); // route still streaming → keep the goal visible } else { plan = null; // empty path = navigation finished/aborted goalMarker = null; + goalIsCommanded = false; clearTimeout(navStaleTimer); } draw(); @@ -333,13 +373,15 @@ export function createMap(root, opts = {}) { function publishGoal(x, y, yaw) { const qz = Math.sin(yaw / 2); const qw = Math.cos(yaw / 2); - const now = Date.now(); + // Zero stamp = "latest" to TF. Never wall time: the sim runs on ROS sim + // time, where a Date.now() stamp is decades in the future. ros.publish(GOAL_POSE_TOPIC, { - header: { stamp: { sec: Math.floor(now / 1000), nanosec: (now % 1000) * 1_000_000 }, frame_id: "map" }, + header: { stamp: { sec: 0, nanosec: 0 }, frame_id: "map" }, pose: { position: { x, y, z: 0 }, orientation: { x: 0, y: 0, z: qz, w: qw } }, }); plan = null; // drop the stale route; the new one streams in on /plan goalMarker = { x, y, yaw }; + goalIsCommanded = false; // a fresh click supersedes any previous skill goal armNavStale(); // hold the goal until the route starts, then while it runs } @@ -397,6 +439,7 @@ export function createMap(root, opts = {}) { try { await ros.callService(CANCEL_NAVIGATION_SERVICE, {}); goalMarker = null; + goalIsCommanded = false; plan = null; setGoalMode(false); draw(); @@ -426,7 +469,8 @@ export function createMap(root, opts = {}) { const unsubOdom = ros.subscribe(ODOM_TOPIC, onOdom, 100); // Only the active planner publishes, so both feeds can share one handler. const unsubPlans = PLAN_TOPICS.map((topic) => ros.subscribe(topic, onPlan, 250, "nav_msgs/msg/Path")); - const unsubCmd = ros.subscribe(CMD_VEL_RAW_TOPIC, onCmd, 100, "geometry_msgs/msg/Twist"); + const unsubCmd = navDebug ? ros.subscribe(CMD_VEL_RAW_TOPIC, onCmd, 100, "geometry_msgs/msg/Twist") : null; + const unsubGoal = ros.subscribe(COMMANDED_GOAL_TOPIC, onCommandedGoal, 0, "geometry_msgs/msg/PoseStamped"); return { /** Swap to a saved zoom (e.g. when this widget reparents between thumbnail and full stage). */ @@ -443,7 +487,8 @@ export function createMap(root, opts = {}) { unsubMap(); unsubOdom(); for (const unsub of unsubPlans) unsub(); - unsubCmd(); + unsubCmd?.(); + unsubGoal(); canvas.remove(); controls.remove(); }, diff --git a/workspace/innate_skills/navigate_to_position.py b/workspace/innate_skills/navigate_to_position.py index a44bf239b..264e606a3 100644 --- a/workspace/innate_skills/navigate_to_position.py +++ b/workspace/innate_skills/navigate_to_position.py @@ -8,6 +8,7 @@ from geometry_msgs.msg import PoseStamped, Twist from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from rclpy.duration import Duration +from rclpy.qos import DurabilityPolicy, QoSProfile from rclpy.time import Time from tf2_ros import TransformException from tf2_ros.buffer import Buffer @@ -52,6 +53,13 @@ def __init__(self, logger, primitive): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self.navigator) + # The exact goal this skill commands, resolved to its fixed frame + # (map, or odom for local goals) -- lets UIs render the true target + # rather than inferring it from the replanned path's endpoint. + # Latched so a viewer that connects mid-navigation still sees it. + latched = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self._commanded_goal_pub = self.navigator.create_publisher(PoseStamped, "/nav/commanded_goal", latched) + self.logger.info("Nav2 position primitive node created") def _lookup_fresh_base_pose(self, timeout_sec: float = 2.0, max_age_sec: float = 1.0): @@ -133,6 +141,8 @@ def go_to_position(self, x: float, y: float, theta: float, local_frame: bool): goal_pose.pose.orientation.z = math.sin(goal_yaw / 2.0) goal_pose.pose.orientation.w = math.cos(goal_yaw / 2.0) + self._commanded_goal_pub.publish(goal_pose) + self.logger.debug(f"Sending goal pose ... behavior_tree: {behavior_tree}") path_navigator = self.navigator_mapfree if local_frame else self.navigator_navigation path = path_navigator.getPath(goal_pose, goal_pose, use_start=False)