diff --git a/src/components/BreadCrumbTrail.tsx b/src/components/BreadCrumbTrail.tsx index 508df84..d4e6674 100644 --- a/src/components/BreadCrumbTrail.tsx +++ b/src/components/BreadCrumbTrail.tsx @@ -4,6 +4,7 @@ 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 { useGpsHeading } from '@/hooks/useGpsHeading'; import ROSLIB from 'roslib'; interface Breadcrumb { @@ -36,6 +37,7 @@ export const haversineDistance = ( const BreadcrumbTrail: React.FC = ({ downloadPNG }) => { const map = useMap(); const { ros, connectionStatus } = useROS(); + const { headingAccuracy } = useGpsHeading(); const { addWaypoint } = useWaypoints(); const [breadcrumbs, setBreadcrumbs] = useState([]); const [paused, setPaused] = useState(false); @@ -176,6 +178,10 @@ const BreadcrumbTrail: React.FC = ({ downloadPNG }) => { ) : (
No fix data received yet.
)} +
+ Heading Accuracy:{' '} + {headingAccuracy !== null ? `±${headingAccuracy.toFixed(2)}°` : 'N/A'} (2σ) +
Total Fixes: {breadcrumbs.length}
diff --git a/src/components/MapCompass.tsx b/src/components/MapCompass.tsx index e60ccf0..20cce14 100644 --- a/src/components/MapCompass.tsx +++ b/src/components/MapCompass.tsx @@ -1,59 +1,35 @@ -import React, {useEffect, useState} from "react"; -import ROSLIB from 'roslib'; -import { useROS } from '@/ros/ROSContext'; - -type Quaternion = { - x: number; - y: number; - z: number; - w: number; -}; - -//extract yaw, convert to degrees, normalize -const headingFromQuaternion = (q: Quaternion): number => { - const { x, y, z, w } = q; - const yaw = Math.atan2( - 2.0 * (w * z + x * y), - 1.0 - 2.0 * (y * y + z * z) - ); - const heading = (yaw * 180) / Math.PI; - return (heading + 360) % 360; -}; +import React from "react"; +import { useGpsHeading } from '@/hooks/useGpsHeading'; const MapCompass: React.FC = () => { - const { ros, connectionStatus: rosStatus } = useROS(); - const [heading, setHeading] = useState(null); - const [valid, setValid] = useState(false); - - useEffect(() => { - if (!ros) return; + const { heading, valid, headingAccuracy } = useGpsHeading(); + const RIM_RADIUS = 95; - const headingTopic = new ROSLIB.Topic({ - ros, - name: '/gps/heading', - messageType: 'sensor_msgs/Imu', - throttle_rate: 100, - }); + // Arc along the rim indicating the accuracy of the heading + const arcPath = (halfAngleDeg: number, r: number): string => { + const a = (halfAngleDeg * Math.PI) / 180; + const x1 = 100 - r * Math.sin(a); + const x2 = 100 + r * Math.sin(a); + const y = 100 - r * Math.cos(a); + const largeArc = halfAngleDeg > 90 ? 1 : 0; + return `M ${x1} ${y} A ${r} ${r} 0 ${largeArc} 1 ${x2} ${y}`; + }; - const handleHeading = (msg: any) => { - if (msg.orientation_covariance[8] > 50) { - setValid(false); - return; - } - const heading = headingFromQuaternion(msg.orientation); - setValid(true); - setHeading(heading); - }; - - headingTopic.subscribe(handleHeading); - return () => headingTopic.unsubscribe(handleHeading); - }, [ros]); - - return( -
+ return ( +
{/* */} - + + {heading !== null && headingAccuracy !== null && headingAccuracy <= 180 && ( + + )} {/* */} { const x2 = 100 + Math.cos(angle) * 85; const y2 = 100 + Math.sin(angle) * 85; return ( - + ); })} - +
); } diff --git a/src/hooks/useGpsHeading.ts b/src/hooks/useGpsHeading.ts new file mode 100644 index 0000000..a2302d5 --- /dev/null +++ b/src/hooks/useGpsHeading.ts @@ -0,0 +1,59 @@ +import { useEffect, useState } from 'react'; +import ROSLIB from 'roslib'; +import { useROS } from '@/ros/ROSContext'; + +type Quaternion = { + x: number; + y: number; + z: number; + w: number; +}; + +//extract yaw, convert to degrees, normalize +const headingFromQuaternion = (q: Quaternion): number => { + const { x, y, z, w } = q; + const yaw = Math.atan2( + 2.0 * (w * z + x * y), + 1.0 - 2.0 * (y * y + z * z) + ); + const heading = (yaw * 180) / Math.PI; + return (heading + 360) % 360; +}; + +export function useGpsHeading(){ + const { ros } = useROS(); + const [heading, setHeading] = useState(null); + const [valid, setValid] = useState(false); + const [headingAccuracy, setHeadingAccuracy] = useState(null); + + useEffect(() => { + if (!ros) return; + + const headingTopic = new ROSLIB.Topic({ + ros, + name: '/gps/heading', + messageType: 'sensor_msgs/Imu', + throttle_rate: 100, + }); + + const handleHeading = (msg: any) => { + const yawVariance = msg.orientation_covariance?.[8]; + + setHeadingAccuracy((2 * Math.sqrt(yawVariance) * 180) / Math.PI); + + if (yawVariance > 0.5) { + setValid(false); + return; + } + + const heading = headingFromQuaternion(msg.orientation); + setValid(true); + setHeading(heading); + }; + + headingTopic.subscribe(handleHeading); + return () => headingTopic.unsubscribe(handleHeading); + }, [ros]); + + return { heading, valid, headingAccuracy }; +}