patch..

resources/nav2-ros-bridge/bridge.py

@@ -209,6 +209,9 @@ class SubscriberNode(Node):
         self._trajectory_event = threading.Event()
         self._behavior_path = None
         self._behavior_path_event = threading.Event()
+        self._nav2_plan = None
+        self._nav2_plan_revision = 0
+        self._nav2_plan_lock = threading.Lock()
         if _ROBOT_STACK == "autoware" and HAS_PLANNING:
             self.create_subscription(
                 Trajectory,
@@ -350,11 +353,20 @@ class SubscriberNode(Node):
 
     def _on_nav2_plan(self, msg: NavPath):
         if len(msg.poses) > 0:
+            with self._nav2_plan_lock:
+                self._nav2_plan = msg
+                self._nav2_plan_revision += 1
             self._behavior_path = msg
             self._trajectory = msg
             self._behavior_path_event.set()
             self._trajectory_event.set()
 
+    def get_nav2_plan_snapshot(self):
+        with self._nav2_plan_lock:
+            if self._nav2_plan is None:
+                return self._nav2_plan_revision, []
+            return self._nav2_plan_revision, self._nav_path_to_points(self._nav2_plan)
+
     def _on_kinematic_state(self, msg: Odometry):
         with self._kinematic_lock:
             self._kinematic_state = msg
@@ -1104,6 +1116,36 @@ async def ws_map(websocket: WebSocket, stack: Optional[str] = None):
         pass
 
 
+@app.websocket("/ws/nav2")
+async def ws_nav2(websocket: WebSocket):
+    await websocket.accept()
+    if _ROBOT_STACK != "nav2":
+        await websocket.send_json({
+            "type": "stack_mismatch",
+            "requested": "nav2",
+            "active": _ROBOT_STACK,
+            "message": "Restart bridge with ROBOT_STACK=nav2 to stream Nav2 paths",
+        })
+        await websocket.close()
+        return
+
+    last_revision = -1
+    try:
+        while True:
+            if sub_node:
+                revision, poses = sub_node.get_nav2_plan_snapshot()
+                if poses and revision != last_revision:
+                    last_revision = revision
+                    await websocket.send_json({
+                        "type": "nav2_path",
+                        "revision": revision,
+                        "poses": poses,
+                    })
+            await asyncio.sleep(0.25)
+    except (WebSocketDisconnect, Exception):
+        pass
+
+
 @app.post("/set_initial_pose")
 def set_initial_pose(payload: PosePayload):
     try: