diff --git a/umdloop_gui_web/__pycache__/ros_bridge.cpython-310.pyc b/umdloop_gui_web/__pycache__/ros_bridge.cpython-310.pyc index 38fc514..ac467a6 100644 Binary files a/umdloop_gui_web/__pycache__/ros_bridge.cpython-310.pyc and b/umdloop_gui_web/__pycache__/ros_bridge.cpython-310.pyc differ diff --git a/umdloop_gui_web/app/GUI functions/Navigation.js b/umdloop_gui_web/app/GUI functions/Navigation.js index 7aac68e..1cacbfe 100644 --- a/umdloop_gui_web/app/GUI functions/Navigation.js +++ b/umdloop_gui_web/app/GUI functions/Navigation.js @@ -1,6 +1,7 @@ "use client"; import React, { useEffect, useState } from "react"; +import RoverStateDashboard from "./RoverStateDashboard"; export default function Navigation({ selectedNavItem }) { const [running, setRunning] = useState(false); @@ -79,7 +80,6 @@ export default function Navigation({ selectedNavItem }) { useEffect(() => { if (selectedNavItem !== "Object Detection") return undefined; - fetchStatus(); const id = setInterval(fetchStatus, 1000); return () => clearInterval(id); @@ -92,51 +92,27 @@ export default function Navigation({ selectedNavItem }) { return (
-

{selectedNavItem} - Navigation Mode

- + {/* ── Object Detection ── */} {selectedNavItem === "Object Detection" && (
-
+
{running ? "RUNNING ✅" : "STOPPED ❌"}
- -
- PID: {pid ?? "—"} -
- +
PID: {pid ?? "—"}
{error &&
{error}
}
- - - + + +
@@ -146,51 +122,34 @@ export default function Navigation({ selectedNavItem }) {
)} + {/* ── Control Panel ── */} {selectedNavItem === "Control Panel" && (
-
+

Control Panel

- setLatitude(e.target.value)} placeholder="e.g. 38.4239116" style={{ width: "100%", padding: "10px 12px", borderRadius: "10px", border: "2px solid #1f1e1eff", background: "#3d3d3d", color: "white", outline: "none" }} /> + setLatitude(e.target.value)} placeholder="e.g. 38.4239116" + style={{ width: "100%", padding: "10px 12px", borderRadius: "10px", border: "2px solid #1f1e1eff", background: "#3d3d3d", color: "white", outline: "none" }} />
-
- setLongitude(e.target.value)} placeholder="e.g. -110.7849055" style={{ width: "100%", padding: "10px 12px", borderRadius: "10px", border: "2px solid #1f1e1eff", background: "#3d3d3d", color: "white", outline: "none" }} /> + setLongitude(e.target.value)} placeholder="e.g. -110.7849055" + style={{ width: "100%", padding: "10px 12px", borderRadius: "10px", border: "2px solid #1f1e1eff", background: "#3d3d3d", color: "white", outline: "none" }} />
Mode
- {["GNSS", "Object Detection", "Aruco Tag"].map((opt) => ( -
- - {pathPlanStatus ?
{pathPlanStatus}
: null} - {error ?
{error}
: null} + {pathPlanStatus &&
{pathPlanStatus}
} + {error &&
{error}
}
)} + + {/* ── Rover State */} + {selectedNavItem === "Rover State" && }
); } diff --git a/umdloop_gui_web/app/GUI functions/RoverStateDashboard.js b/umdloop_gui_web/app/GUI functions/RoverStateDashboard.js new file mode 100644 index 0000000..57646fd --- /dev/null +++ b/umdloop_gui_web/app/GUI functions/RoverStateDashboard.js @@ -0,0 +1,237 @@ +"use client"; + +import React, { useEffect, useState, useCallback } from "react"; +import { getApiBaseUrl } from "../config"; + +const fmt = (v, d = 3) => (v == null || isNaN(v) ? "—" : Number(v).toFixed(d)); +const fmtAge = (ts) => { + if (!ts) return "—"; + const s = (Date.now() - ts) / 1000; + if (s < 1.5) return "live"; + return `${s.toFixed(1)}s ago`; +}; +const isLive = (ts, maxMs = 2500) => ts && Date.now() - ts < maxMs; + +const gpsFixLabel = (status) => { + if (status == null) return "—"; + if (status < 0) return "No Fix"; + if (status === 0) return "GPS Fix"; + return "DGPS Fix"; +}; + +const diagColor = (level) => level === 0 ? "#4ade80" : level === 1 ? "#facc15" : "#f87171"; +const diagLabel = (level) => level === 0 ? "OK" : level === 1 ? "WARN" : level === 2 ? "ERR" : "STALE"; + +function StatusDot({ live }) { + return ( + + ); +} + +function Card({ title, live, children }) { + return ( +
+
+ + + {title} + + +
+ {children} +
+ ); +} + +function Row({ label, value, unit = "" }) { + return ( +
+ {label} + + {value ?? "—"}{unit && {unit}} + +
+ ); +} + + +function VelBar({ label, value, max = 2 }) { + const pct = Math.min(100, (Math.abs(value ?? 0) / max) * 100); + const neg = (value ?? 0) < 0; + return ( +
+
+ {label} + + {fmt(value, 3)} m/s + +
+
+
+
+
+ ); +} + + +export default function RoverStateDashboard() { + const [state, setState] = useState(null); + const [apiStatus, setApiStatus] = useState("connecting"); + const [lastPoll, setLastPoll] = useState(null); + const [errorMsg, setErrorMsg] = useState(""); + + const poll = useCallback(async () => { + try { + const res = await fetch(`${getApiBaseUrl()}/rover/state`); + const data = await res.json(); + if (data.ok) { + setState(data); + setApiStatus("ok"); + setLastPoll(Date.now()); + setErrorMsg(""); + } else { + setApiStatus("error"); + setErrorMsg(data.error || "Unknown error"); + } + } catch (e) { + setApiStatus("error"); + setErrorMsg("Flask backend unreachable"); + } + }, []); + + useEffect(() => { + poll(); + const id = setInterval(poll, 1000); + return () => clearInterval(id); + }, [poll]); + + const ok = apiStatus === "ok"; + const gps = state?.gps; + const imu = state?.imu; + const imuf = state?.imu_filtered; + const odom = state?.odom; + const joints = state?.joints ?? []; + const diags = state?.diagnostics ?? []; + const headingDeg = state?.heading ?? odom?.yaw_deg ?? null; + + return ( +
+
+
+

ROVER STATE

+
+ + + {ok ? "FLASK API CONNECTED" : errorMsg || "CONNECTING…"} + +
+
+ polled {fmtAge(lastPoll)} +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0}> + {joints.length > 0 ? ( + joints.slice(0, 6).map((j) => ( + + )) + ) : ( + Waiting for /joint_states… + )} + + + 0}> + {diags.length === 0 ? ( + Waiting for /diagnostics… + ) : ( + <> + {diags.slice(0, 10).map((d, i) => ( +
+
+ {d.name} + {diagLabel(d.level)} +
+ {d.message && {d.message}} +
+ ))} + {diags.length > 10 && +{diags.length - 10} more} + + )} + + +
+
+ ); +} diff --git a/umdloop_gui_web/app/GUI functions/pageConstants.js b/umdloop_gui_web/app/GUI functions/pageConstants.js index 2f8a9ef..5709387 100644 --- a/umdloop_gui_web/app/GUI functions/pageConstants.js +++ b/umdloop_gui_web/app/GUI functions/pageConstants.js @@ -3,4 +3,4 @@ export const MODES = ["Operator", "Technician", "Drone", "Navigation", "Map"]; export const MODE_ICONS = ["camera.png", "sensor.png", "camera.png", "navigation.png", "map.png"]; export const SUBSYSTEMS = ["Drive", "Arm", "Science"]; -export const NAVIGATION_BUTTONS = ["Object Detection", "Control Panel", "Placeholder2"]; +export const NAVIGATION_BUTTONS = ["Object Detection", "Control Panel", "Rover State"]; diff --git a/umdloop_gui_web/app/RoverStateDashboard.js b/umdloop_gui_web/app/RoverStateDashboard.js new file mode 100644 index 0000000..e446b62 --- /dev/null +++ b/umdloop_gui_web/app/RoverStateDashboard.js @@ -0,0 +1,234 @@ +"use client"; + +import React, { useEffect, useState, useCallback } from "react"; +import { getApiBaseUrl } from "../config"; + +const fmt = (v, d = 3) => (v == null || isNaN(v) ? "—" : Number(v).toFixed(d)); +const fmtAge = (ts) => { + if (!ts) return "—"; + const s = (Date.now() - ts) / 1000; + if (s < 1.5) return "live"; + return `${s.toFixed(1)}s ago`; +}; +const isLive = (ts, maxMs = 2500) => ts && Date.now() - ts < maxMs; + +const gpsFixLabel = (status) => { + if (status == null) return "—"; + if (status < 0) return "No Fix"; + if (status === 0) return "GPS Fix"; + return "DGPS Fix"; +}; + +const diagColor = (level) => level === 0 ? "#4ade80" : level === 1 ? "#facc15" : "#f87171"; +const diagLabel = (level) => level === 0 ? "OK" : level === 1 ? "WARN" : level === 2 ? "ERR" : "STALE"; + +function StatusDot({ live }) { + return ( + + ); +} + +function Card({ title, live, children }) { + return ( +
+
+ + + {title} + + +
+ {children} +
+ ); +} + +function Row({ label, value, unit = "" }) { + return ( +
+ {label} + + {value ?? "—"}{unit && {unit}} + +
+ ); +} + + +function VelBar({ label, value, max = 2 }) { + const pct = Math.min(100, (Math.abs(value ?? 0) / max) * 100); + const neg = (value ?? 0) < 0; + return ( +
+
+ {label} + + {fmt(value, 3)} m/s + +
+
+
+
+
+ ); +} + + +export default function RoverStateDashboard() { + const [state, setState] = useState(null); + const [apiStatus, setApiStatus] = useState("connecting"); + const [lastPoll, setLastPoll] = useState(null); + const [errorMsg, setErrorMsg] = useState(""); + + const poll = useCallback(async () => { + try { + const res = await fetch(`${getApiBaseUrl()}/rover/state`); + const data = await res.json(); + if (data.ok) { + setState(data); + setApiStatus("ok"); + setLastPoll(Date.now()); + setErrorMsg(""); + } else { + setApiStatus("error"); + setErrorMsg(data.error || "Unknown error"); + } + } catch (e) { + setApiStatus("error"); + setErrorMsg("Flask backend unreachable"); + } + }, []); + + useEffect(() => { + poll(); + const id = setInterval(poll, 1000); + return () => clearInterval(id); + }, [poll]); + + const ok = apiStatus === "ok"; + const gps = state?.gps; + const imu = state?.imu; + const imuf = state?.imu_filtered; + const odom = state?.odom; + const joints = state?.joints ?? []; + const diags = state?.diagnostics ?? []; + const headingDeg = state?.heading ?? odom?.yaw_deg ?? null; + + return ( +
+
+
+

ROVER STATE

+
+ + + {ok ? "FLASK API CONNECTED" : errorMsg || "CONNECTING…"} + +
+
+ polled {fmtAge(lastPoll)} +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0}> + {joints.length > 0 ? ( + joints.slice(0, 6).map((j) => ( + + )) + ) : ( + Waiting for /joint_states… + )} + + + 0}> + {diags.length === 0 ? ( + Waiting for /diagnostics… + ) : ( + <> + {diags.slice(0, 10).map((d, i) => ( +
+ {d.name} + {diagLabel(d.level)} +
+ ))} + {diags.length > 10 && +{diags.length - 10} more} + + )} + + +
+
+ ); +} diff --git a/umdloop_gui_web/ros_bridge.py b/umdloop_gui_web/ros_bridge.py index a938af8..777a1bf 100644 --- a/umdloop_gui_web/ros_bridge.py +++ b/umdloop_gui_web/ros_bridge.py @@ -5,58 +5,202 @@ from rclpy.node import Node from rclpy.action import ActionClient -from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy, LivelinessPolicy -from std_msgs.msg import String +from std_msgs.msg import String, Float64 from msgs.action import NavigateToGPS -from sensor_msgs.msg import NavSatFix +from sensor_msgs.msg import NavSatFix, Imu, JointState +from nav_msgs.msg import Odometry +from diagnostic_msgs.msg import DiagnosticArray + +import math + + +def quat_to_rpy(q): + x, y, z, w = q.x, q.y, q.z, q.w + roll = math.degrees(math.atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))) + pitch = math.degrees(math.asin(max(-1.0, min(1.0, 2*(w*y - z*x))))) + yaw = math.degrees(math.atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))) + return roll, pitch, yaw + + +def safe_str(v): + if v is None: + return None + if isinstance(v, bytes): + return v.decode("utf-8", errors="replace") + return str(v) + +def safe_float(v): + try: + f = float(v) + return None if (f != f or f == float("inf") or f == float("-inf")) else f + except Exception: + return None class RosGpsClient(Node): def __init__(self): super().__init__("umdloop_gui_ros_bridge") - # Latest rover GPS fix (None until first message arrives) - self.rover_position = None - - # Action client (GNSS navigation) - self._client = ActionClient( - self, - NavigateToGPS, - "/navigate_to_gps" - ) + # Action client (GNSS navigation) + self._client = ActionClient(self, NavigateToGPS, "/navigate_to_gps") # QoS MUST match bt_navigator subscriber - qos = QoSProfile(depth=1) - qos.durability = DurabilityPolicy.TRANSIENT_LOCAL - qos.reliability = ReliabilityPolicy.RELIABLE + qos_latched = QoSProfile(depth=1) + qos_latched.durability = DurabilityPolicy.TRANSIENT_LOCAL + qos_latched.reliability = ReliabilityPolicy.RELIABLE + # nav mode publisher - self._nav_mode_pub = self.create_publisher( - String, - "/nav_mode", - qos + self._nav_mode_pub = self.create_publisher(String, "/nav_mode", qos_latched) + + self._state_lock = threading.Lock() + self._state = { + "gps": None, + "imu": None, + "imu_filtered": None, + "odom": None, + "joints": None, + "nav_mode": None, + "heading": None, + "diagnostics": [], + } + + sensor_qos = QoSProfile( + depth=5, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, ) - - # GPS fix subscriber — best-effort sensor QoS - gps_qos = QoSProfile(depth=1) - gps_qos.reliability = ReliabilityPolicy.BEST_EFFORT - self.create_subscription( - NavSatFix, - "/gps/fix", - self._gps_callback, - gps_qos, + + reliable_qos = QoSProfile( + depth=5, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, ) - self.get_logger().info("RosGpsClient initialized") + self._state["nav_mode"] = "GNSS" + threading.Timer(0.5, lambda: self.publish_nav_mode("GNSS")).start() + + self.create_subscription(NavSatFix, "/gps/fix", self.gps, sensor_qos) + self.create_subscription(Imu, "/imu", self.imu, sensor_qos) + self.create_subscription(Imu, "/imu/filtered", self.imu_filt, sensor_qos) + self.create_subscription(Odometry, "/localization/odom", self.odom, reliable_qos) + self.create_subscription(JointState, "/joint_states", self.joints, reliable_qos) + self.create_subscription(String, "/nav_mode", self.nav_mode, qos_latched) + self.create_subscription(Float64, "/heading", self.heading, reliable_qos) + self.create_subscription(DiagnosticArray, "/diagnostics", self.diag, reliable_qos) + + + self.get_logger().info("RosGpsClient initialized with rover state subscribers") + - def _gps_callback(self, msg: NavSatFix): - if msg.latitude is not None and msg.longitude is not None: - self.rover_position = { - "latitude": msg.latitude, + def gps(self, msg): + with self._state_lock: + self._state["gps"] = { + "latitude": msg.latitude, "longitude": msg.longitude, + "altitude": msg.altitude, + "status": msg.status.status, + } + + def imu(self, msg): + with self._state_lock: + roll, pitch, yaw = quat_to_rpy(msg.orientation) + self._state["imu"] = { + "roll_deg": roll, + "pitch_deg": pitch, + "yaw_deg": yaw, + "accel_x": msg.linear_acceleration.x, + "accel_y": msg.linear_acceleration.y, + "accel_z": msg.linear_acceleration.z, + "gyro_x": msg.angular_velocity.x, + "gyro_y": msg.angular_velocity.y, + "gyro_z": msg.angular_velocity.z, + } + + def imu_filt(self, msg): + with self._state_lock: + roll, pitch, yaw = quat_to_rpy(msg.orientation) + self._state["imu_filtered"] = { + "roll_deg": roll, + "pitch_deg": pitch, + "yaw_deg": yaw, + "accel_x": msg.linear_acceleration.x, + "accel_y": msg.linear_acceleration.y, + "accel_z": msg.linear_acceleration.z, } + def odom(self, msg): + with self._state_lock: + p = msg.pose.pose.position + roll, pitch, yaw = quat_to_rpy(msg.pose.pose.orientation) + t = msg.twist.twist + lin_speed = math.hypot(t.linear.x, t.linear.y) + self._state["odom"] = { + "x": safe_float(p.x), "y": safe_float(p.y), "z": safe_float(p.z), + "roll_deg": safe_float(roll), + "pitch_deg": safe_float(pitch), + "yaw_deg": safe_float(yaw), + "linear_x": safe_float(t.linear.x), + "linear_y": safe_float(t.linear.y), + "linear_speed": safe_float(lin_speed), + "angular_z": safe_float(t.angular.z), + } + + def joints(self, msg): + def safe(arr, i): + if i >= len(arr): + return None + v = arr[i] + return None if (v != v or v == float("inf") or v == float("-inf")) else v + + with self._state_lock: + self._state["joints"] = [ + { + "name": safe_str(name), + "position": safe(msg.position, i), + "velocity": safe(msg.velocity, i), + "effort": safe(msg.effort, i), + } + for i, name in enumerate(msg.name) + ] + + def nav_mode(self, msg): + with self._state_lock: + self._state["nav_mode"] = safe_str(msg.data) + + def heading(self, msg): + with self._state_lock: + self._state["heading"] = safe_float(msg.data) + + def diag(self, msg): + def parse_level(v): + if isinstance(v, bytes): + return v[0] + if isinstance(v, str): + return ord(v) + return int(v) + + + with self._state_lock: + self._state["diagnostics"] = [ + { + "name": safe_str(s.name), + "level": parse_level(s.level), + "message": safe_str(s.message), + } + for s in msg.status + ] + + # -------------------------------------------------- + # Publish API + # -------------------------------------------------- + def get_rover_state(self): + with self._state_lock: + import copy + return copy.deepcopy(self._state) + # -------------------------------------------------- # Publish navigation mode # -------------------------------------------------- @@ -64,16 +208,13 @@ def publish_nav_mode(self, mode: str): msg = String() msg.data = mode self._nav_mode_pub.publish(msg) - self.get_logger().info(f"Published /nav_mode = '{mode}'") # -------------------------------------------------- # Send GNSS goal (blocking) # -------------------------------------------------- def send_gps_goal_blocking(self, lat, lon, tol=0.0, timeout_sec=60.0): - self.get_logger().info( - f"Sending GPS goal: lat={lat}, lon={lon}, tol={tol}" - ) + self.get_logger().info(f"Sending GPS goal: lat={lat}, lon={lon}, tol={tol}") if not self._client.wait_for_server(timeout_sec=2.0): return False, False, "navigate_to_gps action server not available" @@ -85,33 +226,23 @@ def send_gps_goal_blocking(self, lat, lon, tol=0.0, timeout_sec=60.0): # send goal async send_future = self._client.send_goal_async(goal) - rclpy.spin_until_future_complete( - self, - send_future, - timeout_sec=5.0 - ) + rclpy.spin_until_future_complete(self, send_future, timeout_sec=5.0) if not send_future.done(): return False, False, "Failed to send goal" goal_handle = send_future.result() - if not goal_handle.accepted: return False, False, "Goal rejected" # wait for result result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete( - self, - result_future, - timeout_sec=timeout_sec - ) + rclpy.spin_until_future_complete(self, result_future, timeout_sec=timeout_sec) if not result_future.done(): return True, False, "Timed out waiting for result" result = result_future.result().result - return True, bool(result.success), result.message @@ -127,15 +258,10 @@ def __init__(self): self._lock = threading.Lock() def start(self): - """ - Safe to call multiple times. - Prevents double rclpy.init() crashes. - """ with self._lock: - + if self.started and self.node is not None: return - # init ROS only if needed try: if not rclpy.ok(): @@ -147,13 +273,13 @@ def start(self): # create node once if self.node is None: self.node = RosGpsClient() - + # start spin thread once if self.thread is None or not self.thread.is_alive(): self.thread = threading.Thread( target=rclpy.spin, args=(self.node,), - daemon=True + daemon=True, ) self.thread.start() @@ -162,16 +288,15 @@ def start(self): def shutdown(self): with self._lock: - if self.node is not None: self.node.destroy_node() self.node = None - if rclpy.ok(): + if rclpy is not None and rclpy.ok(): rclpy.shutdown() self.started = False # global singleton -ros_context = RosContext() \ No newline at end of file +ros_context = RosContext() diff --git a/umdloop_gui_web/server.py b/umdloop_gui_web/server.py index e2fb016..0c7b057 100644 --- a/umdloop_gui_web/server.py +++ b/umdloop_gui_web/server.py @@ -304,6 +304,11 @@ def radio_status(): status = get_mikrotik_radio_status() return jsonify({"ok": True, **status}), 200 +@app.get("/rover/state") +def rover_state(): + ros_context.start() + state = ros_context.node.get_rover_state() + return jsonify({"ok": True, **state}), 200 @app.get("/object-detection/status") def status():