Antenna panel changes

src/components/BreadCrumbTrail.tsx

@@ -17,6 +17,22 @@ type BreadcrumbTrailProps = {
   downloadPNG: Function,
 }
 
+// cool thing for calculating distance on a sphere
+export const haversineDistance = (
+  [lat1, lon1]: [number, number],
+  [lat2, lon2]: [number, number]
+): number => {
+  const toRad = (x: number) => (x * Math.PI) / 180;
+  const R = 6371;
+  const dLat = toRad(lat2 - lat1);
+  const dLon = toRad(lon2 - lon1);
+  const a =
+    Math.sin(dLat / 2) ** 2 +
+    Math.cos(toRad(lat1)) * Math.cos(toRad(lat2)) * Math.sin(dLon / 2) ** 2;
+  const c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
+  return R * c;
+};
+
 const BreadcrumbTrail: React.FC<BreadcrumbTrailProps> = ({ downloadPNG }) => {
   const map = useMap();
   const { ros, connectionStatus } = useROS();
@@ -76,22 +92,6 @@ const BreadcrumbTrail: React.FC<BreadcrumbTrailProps> = ({ downloadPNG }) => {
     setLastFix(null);
   };
 
-  // cool thing for calculating distance on a sphere
-  const haversineDistance = (
-    [lat1, lon1]: [number, number],
-    [lat2, lon2]: [number, number]
-  ): number => {
-    const toRad = (x: number) => (x * Math.PI) / 180;
-    const R = 6371; // radius of my nutz in kilometers
-    const dLat = toRad(lat2 - lat1);
-    const dLon = toRad(lon2 - lon1);
-    const a =
-      Math.sin(dLat / 2) ** 2 +
-      Math.cos(toRad(lat1)) * Math.cos(toRad(lat2)) * Math.sin(dLon / 2) ** 2;
-    const c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
-    return R * c;
-  };
-
   const computeDistance = (): number => {
     if (breadcrumbs.length < 2) return 0;
     let total = 0;
@@ -149,17 +149,17 @@ const BreadcrumbTrail: React.FC<BreadcrumbTrailProps> = ({ downloadPNG }) => {
           maxWidth: '300px',
         }}
       >
-        <div style={{ marginBottom: '0.5rem' }}>
+        <div style={{ marginBottom: '0.125rem' }}>
           <strong>GPS Fix Status</strong>
         </div>
-        <div style={{ marginBottom: '0.5rem' }}>
+        <div style={{ marginBottom: '0.125rem' }}>
           <strong>ROS Connection:</strong>{' '}
           <span style={{ color: connectionStatus === 'connected' ? 'green' : 'red' }}>
             {connectionStatus}
           </span>
         </div>
         {lastFix ? (
-          <div style={{ marginBottom: '0.5rem' }}>
+          <div style={{ marginBottom: '0.125rem' }}>
             <strong>Last Fix:</strong>
             <br />
             {/* TODO: Is this enough percision? */}
@@ -174,15 +174,15 @@ const BreadcrumbTrail: React.FC<BreadcrumbTrailProps> = ({ downloadPNG }) => {
             Time: {new Date(lastFix.timestamp).toLocaleTimeString()}
           </div>
         ) : (
-          <div style={{ marginBottom: '0.5rem' }}>No fix data received yet.</div>
+          <div style={{ marginBottom: '0.125rem' }}>No fix data received yet.</div>
         )}
-        <div style={{ marginBottom: '0.5rem' }}>
+        <div style={{ marginBottom: '0.125rem' }}>
           <strong>Total Fixes:</strong> {breadcrumbs.length}
         </div>
-        <div style={{ marginBottom: '0.5rem' }}>
+        <div style={{ marginBottom: '0.125rem' }}>
           <strong>Total Distance:</strong> {totalDistance.toFixed(2)} km
         </div>
-        <div style={{ marginBottom: '0.5rem' }}>
+        <div style={{ marginBottom: '0.125rem' }}>
           <button
             onClick={() => setPaused(!paused)}
             style={{

src/components/panels/AntennaControlPanel.tsx

@@ -2,13 +2,19 @@
 import React, { useEffect, useRef, useState } from 'react';
 import ROSLIB from 'roslib';
 import { useROS } from '@/ros/ROSContext';
+import { LatLngTuple } from 'leaflet';
+import { haversineDistance } from '../BreadCrumbTrail';
 
 const AntennaControlPanel: React.FC = () => {
   const { ros } = useROS();
 
   const [enabled, setEnabled] = useState(false);
   const [leftHeld, setLeftHeld] = useState(false);
   const [rightHeld, setRightHeld] = useState(false);
+  const [roverLoc, setRoverLoc] = useState<[number, number]>([0, 0]);
+  const [antennaLoc, setAntennaLoc] = useState<[number, number]>([0, 0]);
+  const [bearing, setBearing] = useState<number>(0);
+  const [targetBearing, setTargetBearing] = useState<number>(0);
 
   const antStatusTopicRef = useRef<ROSLIB.Topic | null>(null);
   const antValTopicRef = useRef<ROSLIB.Topic | null>(null);
@@ -33,6 +39,56 @@ const AntennaControlPanel: React.FC = () => {
       messageType: 'std_msgs/Bool',
     });
 
+    const fixTopic = new ROSLIB.Topic({
+      ros,
+      name: '/gps/fix',
+      messageType: 'sensor_msgs/NavSatFix',
+    });
+
+    const antennaFixTopic = new ROSLIB.Topic({
+      ros,
+      name: '/base_station/fix',
+      messageType: 'sensor_msgs/NavSatFix',
+    });
+
+    const bearingTopic = new ROSLIB.Topic({
+      ros,
+      name: '/antenna/bearing',
+      messageType: 'std_msgs/Float32',
+    });
+
+    const targetBearingTopic = new ROSLIB.Topic({
+      ros,
+      name: '/antenna/target_bearing',
+      messageType: 'std_msgs/Float32',
+    });
+
+    const handleFix = (message: any) => {
+      // Assuming the /fix message contains 'latitude' and 'longitude'
+      const { latitude, longitude } = message;
+      setRoverLoc([latitude, longitude]);
+    }
+
+    const handleAntennaFix = (message: any) => {
+      // Assuming the /fix message contains 'latitude' and 'longitude'
+      const { latitude, longitude } = message;
+      setAntennaLoc([latitude, longitude]);
+    };
+
+    const handleBearing = (message: any) => {
+      const angle = message.data * 180 / Math.PI;
+      setBearing(angle);
+    };
+
+    const handleTargetBearing = (message: any) => {
+      const angle = message.data * 180 / Math.PI;
+      setTargetBearing(angle);
+    };
+
+    fixTopic.subscribe(handleFix);
+    antennaFixTopic.subscribe(handleAntennaFix);
+    bearingTopic.subscribe(handleBearing);
+    targetBearingTopic.subscribe(handleTargetBearing);
     return () => {
       try {
         antStatusTopicRef.current?.unadvertise();
@@ -42,14 +98,17 @@ const AntennaControlPanel: React.FC = () => {
       }
       antStatusTopicRef.current = null;
       antValTopicRef.current = null;
+      antennaFixTopic.unsubscribe(handleAntennaFix);
+      bearingTopic.unsubscribe(handleBearing);
+      targetBearingTopic.unsubscribe(handleTargetBearing);
     };
   }, [ros]);
 
   // Determine what value should be published right now
   const computeValue = () => {
     if (enabled) return 0.0;
-    if (leftHeld && !rightHeld) return -0.125;
-    if (rightHeld && !leftHeld) return 0.125;
+    if (leftHeld && !rightHeld) return -Math.PI / 180;
+    if (rightHeld && !leftHeld) return Math.PI / 180;
     return 0.0; // neither held OR both held
   };
 
@@ -99,6 +158,8 @@ const AntennaControlPanel: React.FC = () => {
 
   const btnDisabled = enabled || !ros;
 
+  const bearingDelta = (targetBearing - bearing + 180) % 360 - 180
+
   return (
     <div className="antenna-panel">
       <div className="controls">
@@ -128,16 +189,34 @@ const AntennaControlPanel: React.FC = () => {
           Right
         </button>
       </div>
-
-      <label className="checkbox">
-        <input
-          type="checkbox"
-          checked={enabled}
-          onChange={(e) => setEnabled(e.target.checked)}
-        />
-        Auto tracking
-      </label>
-
+      <div style={{ marginBottom: '0.125rem' }}>
+        <label className="checkbox">
+          <input
+            type="checkbox"
+            checked={enabled}
+            onChange={(e) => setEnabled(e.target.checked)}
+          />
+          Auto tracking
+        </label>
+      </div>
+      <div style={{ marginBottom: '0.125rem' }}>
+        <strong>Pointing:</strong>
+        <br />
+        Delta: <span className={Math.abs(bearingDelta) < 15 ? 'ok' : 'bad'}>{bearingDelta.toFixed(1)}</span>°
+        <br />
+        Bearing: {bearing.toFixed(1)}°
+        <br />
+        Target Bearing: {targetBearing.toFixed(1)}°
+      </div>
+      <div style={{ marginBottom: '0.125rem' }}>
+        <strong>Location:</strong>
+        <br />
+          Distance to Rover: {(haversineDistance(antennaLoc, roverLoc) * 1000).toFixed(2)} m
+          <br />
+          Lat: {antennaLoc[0].toFixed(6)}
+          <br />
+          Lon: {antennaLoc[1].toFixed(6)}
+      </div>
       <style jsx>{`
         .antenna-panel {
           background: #1e1e1e;