diff --git a/src/components/BreadCrumbTrail.tsx b/src/components/BreadCrumbTrail.tsx index 6345b2f..895b86c 100644 --- a/src/components/BreadCrumbTrail.tsx +++ b/src/components/BreadCrumbTrail.tsx @@ -1,7 +1,7 @@ 'use client'; -import React, { useState, useEffect } from 'react'; -import { Polyline, Marker, Popup } from 'react-leaflet'; +import React, { useState, useEffect, useRef } from 'react'; +import { Circle, Polyline, useMap } from 'react-leaflet'; import { useROS } from '@/ros/ROSContext'; import { useWaypoints } from '@/contexts/WaypointContext'; import ROSLIB from 'roslib'; @@ -10,14 +10,18 @@ import L from 'leaflet' interface Breadcrumb { coordinate: [number, number]; timestamp: number; + covarianceRadius: number; + altitude?: number; } const BreadcrumbTrail: React.FC = () => { + const map = useMap(); const { ros, connectionStatus } = useROS(); const { addWaypoint } = useWaypoints(); const [breadcrumbs, setBreadcrumbs] = useState([]); const [paused, setPaused] = useState(false); const [lastFix, setLastFix] = useState(null); + const hasRecenteredRef = useRef(false); useEffect(() => { if (!ros) return; @@ -30,21 +34,39 @@ const BreadcrumbTrail: React.FC = () => { const handleFix = (message: any) => { if (paused) return; - // Assuming the /fix message contains 'latitude' and 'longitude' - const { latitude, longitude } = message; + + const { latitude, longitude, position_covariance } = message; + + if (!Number.isFinite(latitude) || !Number.isFinite(longitude)) return; + + const eastVariance = position_covariance?.[0] ?? 0; + const northVariance = position_covariance?.[4] ?? 0; + + const covarianceRadius = 2 * Math.sqrt( + Math.max(eastVariance, northVariance, 0) + ); + const newFix: Breadcrumb = { coordinate: [latitude, longitude], timestamp: Date.now(), + covarianceRadius, + altitude: message.altitude, }; + setBreadcrumbs((prev) => [...prev, newFix]); setLastFix(newFix); + + if (!hasRecenteredRef.current) { + map.flyTo(newFix.coordinate, map.getZoom()); + hasRecenteredRef.current = true; + } }; fixTopic.subscribe(handleFix); return () => { fixTopic.unsubscribe(handleFix); }; - }, [ros, paused]); + }, [ros, paused, map]); const clearBreadcrumbs = () => { setBreadcrumbs([]); @@ -87,6 +109,12 @@ const BreadcrumbTrail: React.FC = () => { } }; + const handleRecenter = () => { + if (lastFix) { + map.flyTo(lastFix.coordinate, map.getZoom()); + } + }; + return ( <> {/* render the crumbs bomboclart*/} @@ -96,6 +124,17 @@ const BreadcrumbTrail: React.FC = () => { color="yellow" /> )} + {lastFix && lastFix.covarianceRadius > 0 && ( + + )}
{
Lon: {lastFix.coordinate[1].toFixed(6)}
+ Altitude: {lastFix.altitude?.toFixed(2) || 'N/A'} m +
+ Accuracy: {lastFix.covarianceRadius.toFixed(2)} m (2σ) +
Time: {new Date(lastFix.timestamp).toLocaleTimeString()}
) : ( @@ -154,6 +197,7 @@ const BreadcrumbTrail: React.FC = () => { + {lastFix && (