Added rover path and test new object detection stream

umdloop_gui_web/app/GUI functions/MapView.js

@@ -1,10 +1,29 @@
 "use client";
 
-import React, { useState, useRef, useCallback, useEffect } from "react";
-import { Map, Marker } from "react-map-gl/maplibre";
+import React, { useState, useRef, useCallback, useEffect, useMemo } from "react";
+import { Map, Marker, Source, Layer } from "react-map-gl/maplibre";
 import "maplibre-gl/dist/maplibre-gl.css";
 import { useLocalTiles } from "../config";
 
+const PLAN_LINE_LAYER = {
+  id: "global-plan-line",
+  type: "line",
+  paint: {
+    "line-color": "#3b82f6",
+    "line-width": 3,
+    "line-dasharray": [4, 2],
+  },
+};
+
+const DRIVEN_PATH_LAYER = {
+  id: "driven-path-line",
+  type: "line",
+  paint: {
+    "line-color": "#f59e0b",
+    "line-width": 2,
+  },
+};
+
 export default function MapView({ selectedSubsystem, titleOverride }) {
   const [viewState, setViewState] = useState({
     longitude: -76.9378,
@@ -15,9 +34,18 @@ export default function MapView({ selectedSubsystem, titleOverride }) {
   const [roverPosition, setRoverPosition] = useState(null);
   const [rosStatus, setRosStatus] = useState("no fix");
   const [followRover, setFollowRover] = useState(false);
+
+  // Global plan: raw coords held in a ref; planVersion bumps only when plan actually changes
+  const globalPlanRef = useRef(null);
+  const [planVersion, setPlanVersion] = useState(0);
+
+  // Driven path history — [[lon, lat], ...]
+  const [drivenPath, setDrivenPath] = useState([]);
+  const lastDrivenPoint = useRef(null);
+
   const mapRef = useRef();
 
-  // Poll Flask backend for latest /gps/fix data (sourced from ROS via ros_bridge.py)
+  // Poll Flask backend for latest /gps/fix and accumulate driven path
   useEffect(() => {
     const poll = async () => {
       try {
@@ -27,7 +55,18 @@ export default function MapView({ selectedSubsystem, titleOverride }) {
           const pos = { latitude: data.latitude, longitude: data.longitude };
           setRoverPosition(pos);
           setRosStatus("fix");
-          // Keep map centered on rover when follow mode is active
+
+          // Accumulate driven path; only append if moved more than ~0.5 m
+          const lon = data.longitude;
+          const lat = data.latitude;
+          const last = lastDrivenPoint.current;
+          const MIN_DELTA = 0.000005; // ~0.5 m in degrees
+          if (!last || Math.abs(lon - last[0]) > MIN_DELTA || Math.abs(lat - last[1]) > MIN_DELTA) {
+            const point = [lon, lat];
+            lastDrivenPoint.current = point;
+            setDrivenPath((prev) => [...prev, point]);
+          }
+
           setFollowRover((prev) => {
             if (prev) {
               setViewState((vs) => ({ ...vs, latitude: pos.latitude, longitude: pos.longitude }));
@@ -47,6 +86,53 @@ export default function MapView({ selectedSubsystem, titleOverride }) {
     return () => clearInterval(id);
   }, []);
 
+  // Poll Flask backend for latest /plan (global plan from Nav2).
+  // Only bump planVersion (triggering re-render) when the plan actually changes.
+  useEffect(() => {
+    const fetchPlan = async () => {
+      try {
+        const res = await fetch("http://127.0.0.1:5000/navigation/plan");
+        const data = await res.json();
+        if (data.available && data.coordinates?.length >= 2) {
+          const coords = data.coordinates;
+          const prev = globalPlanRef.current;
+          const changed =
+            !prev ||
+            prev.length !== coords.length ||
+            prev[0][0] !== coords[0][0] ||
+            prev[0][1] !== coords[0][1];
+          if (changed) {
+            globalPlanRef.current = coords;
+            setPlanVersion((v) => v + 1);
+          }
+        }
+      } catch {
+        // silently ignore; plan display is optional
+      }
+    };
+
+    fetchPlan();
+    const id = setInterval(fetchPlan, 2000);
+    return () => clearInterval(id);
+  }, []);
+
+  // Stable GeoJSON objects — only recreated when underlying data actually changes
+  const planGeoJSON = useMemo(
+    () =>
+      globalPlanRef.current?.length >= 2
+        ? { type: "Feature", geometry: { type: "LineString", coordinates: globalPlanRef.current } }
+        : null,
+    [planVersion]
+  );
+
+  const drivenGeoJSON = useMemo(
+    () =>
+      drivenPath.length >= 2
+        ? { type: "Feature", geometry: { type: "LineString", coordinates: drivenPath } }
+        : null,
+    [drivenPath]
+  );
+
   // Snap to rover and enable follow mode
   const centerOnRover = () => {
     if (!roverPosition) return;
@@ -80,6 +166,11 @@ export default function MapView({ selectedSubsystem, titleOverride }) {
 
   const deleteAllWaypoints = () => setWaypoints([]);
 
+  const clearDrivenPath = () => {
+    setDrivenPath([]);
+    lastDrivenPoint.current = null;
+  };
+
   const MAPTILER_KEY = "DDQqKsPBfdOZOVxgcoy5";
   const tileUrl = useLocalTiles()
     ? "/tiles/{z}/{x}/{y}.jpg"
@@ -143,8 +234,41 @@ export default function MapView({ selectedSubsystem, titleOverride }) {
           {followRover ? "Following Rover" : "Center on Rover"}
         </button>
 
-        {/* Waypoint controls */}
+        {/* Legend */}
+        <div style={{ display: "flex", alignItems: "center", gap: "14px", fontSize: "12px", opacity: 0.8 }}>
+          {planGeoJSON && (
+            <span style={{ display: "flex", alignItems: "center", gap: "5px" }}>
+              <span style={{ display: "inline-block", width: 20, height: 3, background: "#3b82f6", borderTop: "2px dashed #3b82f6" }} />
+              Global Plan
+            </span>
+          )}
+          {drivenPath.length >= 2 && (
+            <span style={{ display: "flex", alignItems: "center", gap: "5px" }}>
+              <span style={{ display: "inline-block", width: 20, height: 3, background: "#f59e0b" }} />
+              Driven Path
+            </span>
+          )}
+        </div>
+
+        {/* Controls */}
         <div style={{ display: "flex", alignItems: "center", gap: "12px", marginLeft: "auto" }}>
+          {drivenPath.length >= 2 && (
+            <button
+              onClick={clearDrivenPath}
+              style={{
+                padding: "6px 14px",
+                cursor: "pointer",
+                background: "#4b5563",
+                color: "white",
+                border: "none",
+                borderRadius: "6px",
+                fontWeight: 700,
+                fontSize: "13px",
+              }}
+            >
+              Clear Path
+            </button>
+          )}
           <span style={{ fontSize: "13px" }}>Waypoints: {waypoints.length}</span>
           {waypoints.length > 0 && (
             <button
@@ -200,6 +324,20 @@ export default function MapView({ selectedSubsystem, titleOverride }) {
           mapStyle={mapStyle}
           attributionControl={true}
         >
+          {/* Global plan from Nav2 /plan topic */}
+          {planGeoJSON && (
+            <Source id="global-plan" type="geojson" data={planGeoJSON}>
+              <Layer {...PLAN_LINE_LAYER} />
+            </Source>
+          )}
+
+          {/* Driven path accumulated from /gps/fix */}
+          {drivenGeoJSON && (
+            <Source id="driven-path" type="geojson" data={drivenGeoJSON}>
+              <Layer {...DRIVEN_PATH_LAYER} />
+            </Source>
+          )}
+
           {roverPosition && (
             <Marker
               longitude={roverPosition.longitude}

umdloop_gui_web/ros_bridge.py

@@ -1,5 +1,6 @@
 # ros_bridge.py
 
+import math
 import threading
 import rclpy
 
@@ -9,7 +10,9 @@
 
 from std_msgs.msg import String
 from msgs.action import NavigateToGPS
-from sensor_msgs.msg import NavSatFix
+from nav_msgs.msg import Path
+from sensor_msgs.msg import NavSatFix, CompressedImage
+from vision_msgs.msg import Detection2DArray
 
 
 class RosGpsClient(Node):
@@ -19,6 +22,9 @@ def __init__(self):
         # Latest rover GPS fix (None until first message arrives)
         self.rover_position = None
 
+        # Global plan as [[lon, lat], ...] in GPS coords (None until first /plan arrives)
+        self.plan = None
+
         # Action client (GNSS navigation)
         self._client = ActionClient(
             self,
@@ -48,6 +54,43 @@ def __init__(self):
             gps_qos,
         )
 
+        # Global plan subscriber — Nav2 planner publishes with VOLATILE durability
+        plan_qos = QoSProfile(depth=1)
+        plan_qos.durability = DurabilityPolicy.VOLATILE
+        plan_qos.reliability = ReliabilityPolicy.RELIABLE
+        self.create_subscription(
+            Path,
+            "/plan",
+            self._plan_callback,
+            plan_qos,
+        )
+
+        # Object detection: latest compressed JPEG + bounding boxes.
+        # The streaming endpoint reads these without decoding here, so the
+        # callback path stays cheap (one bytes() copy per frame).
+        self._frame_cv = threading.Condition()
+        self._latest_jpeg = None
+        self._latest_detections = []
+        self._frame_seq = 0
+
+        cam_qos = QoSProfile(depth=1)
+        cam_qos.reliability = ReliabilityPolicy.BEST_EFFORT
+        self.create_subscription(
+            CompressedImage,
+            "/camera/image_raw/compressed",
+            self._image_callback,
+            cam_qos,
+        )
+
+        det_qos = QoSProfile(depth=1)
+        det_qos.reliability = ReliabilityPolicy.BEST_EFFORT
+        self.create_subscription(
+            Detection2DArray,
+            "/yolo/detections",
+            self._detections_callback,
+            det_qos,
+        )
+
         self.get_logger().info("RosGpsClient initialized")
 
     def _gps_callback(self, msg: NavSatFix):
@@ -57,6 +100,78 @@ def _gps_callback(self, msg: NavSatFix):
                 "longitude": msg.longitude,
             }
 
+    def _plan_callback(self, msg: Path):
+        if not msg.poses or self.rover_position is None:
+            return
+
+        # Use the first pose (rover's current map position) as the local origin.
+        # Distances from that origin are converted to GPS offsets using a flat-earth
+        # approximation — accurate to <1 m over competition-scale distances (<500 m).
+        anchor_x = msg.poses[0].pose.position.x
+        anchor_y = msg.poses[0].pose.position.y
+        anchor_lat = self.rover_position["latitude"]
+        anchor_lon = self.rover_position["longitude"]
+        cos_lat = math.cos(math.radians(anchor_lat))
+
+        gps_coords = []
+        for pose in msg.poses:
+            dx = pose.pose.position.x - anchor_x
+            dy = pose.pose.position.y - anchor_y
+            lat = anchor_lat + dy / 111111.0
+            lon = anchor_lon + dx / (111111.0 * cos_lat)
+            gps_coords.append([lon, lat])
+
+        self.plan = gps_coords
+        self.get_logger().info(f"Received /plan with {len(gps_coords)} poses")
+
+    # --------------------------------------------------
+    # Object detection stream callbacks
+    # --------------------------------------------------
+    def _image_callback(self, msg: CompressedImage):
+        data = bytes(msg.data)
+        with self._frame_cv:
+            self._latest_jpeg = data
+            self._frame_seq += 1
+            self._frame_cv.notify_all()
+
+    def _detections_callback(self, msg: Detection2DArray):
+        boxes = []
+        for det in msg.detections:
+            bbox = det.bbox
+            center = bbox.center
+            # vision_msgs >= Iron uses center.position.x/.y; older uses center.x/.y
+            if hasattr(center, "position"):
+                cx, cy = center.position.x, center.position.y
+            else:
+                cx, cy = center.x, center.y
+            sx = bbox.size_x
+            sy = bbox.size_y
+            x1 = int(cx - sx / 2.0)
+            y1 = int(cy - sy / 2.0)
+            x2 = int(cx + sx / 2.0)
+            y2 = int(cy + sy / 2.0)
+
+            label = ""
+            score = 0.0
+            if det.results:
+                first = det.results[0]
+                hyp = getattr(first, "hypothesis", first)
+                label = getattr(hyp, "class_id", getattr(hyp, "id", "")) or ""
+                score = float(getattr(hyp, "score", 0.0))
+
+            boxes.append((x1, y1, x2, y2, label, score))
+
+        with self._frame_cv:
+            self._latest_detections = boxes
+
+    def wait_for_frame(self, last_seq, timeout=1.0):
+        with self._frame_cv:
+            self._frame_cv.wait_for(
+                lambda: self._frame_seq != last_seq,
+                timeout=timeout,
+            )
+            return self._frame_seq, self._latest_jpeg, list(self._latest_detections)
+
     # --------------------------------------------------
     # Publish navigation mode
     # --------------------------------------------------