Added the target heading and distance to the compass when it is set

src/components/navigation/Compass.css

@@ -58,6 +58,20 @@
   background-color: #f44336;
 }
 
+.target-dot {
+  background-color: rgba(0, 0, 0, 0);
+  border: 5px solid rgba(255, 215, 0, 0.75);
+  width: 15px;
+  height: 15px;
+  position: absolute;
+  transform-origin: 75px 75px;
+  top: 25px;
+  left: 25px;
+  border-radius: 100%;
+  box-shadow: 0 0 5px rgba(0, 0, 0, 0.5);
+  z-index: 100;
+}
+
 .compass__label {
   position: absolute;
   font-size: 12px;
@@ -94,6 +108,15 @@
   transform: translateY(-50%);
 }
 
+.compass__label--distance {
+  font-size: 15px;
+  bottom: 21%;
+  left: 50%;
+  transform: translateX(-50%);
+  font-weight: normal;
+  white-space: nowrap;
+}
+
 .compass__outer-ring {
   position: absolute;
   width: calc(100% + 10px);
@@ -115,9 +138,21 @@
   border-color: red;
 }
 
+.target-far {
+  color: red;
+}
+
+.target-close {
+  color: green;
+}
+
 .info {
   color: #f5f5f5;
-  font-size:calc(5px + 1vmax);
+  font-size: calc(5px + 1vmax);
   font-family: monospace;
   text-align: left;
+}
+
+.distance-to-target {
+  color: black;
 }

src/components/navigation/Compass.jsx

@@ -1,10 +1,14 @@
-import React from "react";
+import React, { useState, useEffect, useCallback } from "react";
 import { useSelector } from "react-redux";
 import { selectRoverPosition } from "../../store/telemetrySlice";
+import { selectLongitude, selectLatitude } from "../../store/waypointNavSlice";
 import "./Compass.css";
-import { Quaternion, Euler, Vector3 } from '@math.gl/core';
+import { Quaternion, Euler, Vector3, clamp } from '@math.gl/core';
 import * as Quat from 'gl-matrix/quat';
 
+const CLOSE_DISTANCE = 0.5;  // Distance to be considered at the target
+const FAR_DISTANCE = 20.0;  // Distance to be considered far from the target
+
 function getAttitude(roll, pitch) {
   let attitudeRPY = new Euler().fromRollPitchYaw(roll, pitch, 0.0);
   // taken from Euler.getQuaternion(), which we can't call because it's broken
@@ -32,6 +36,53 @@ function sanitize(num, decimals) {
   return num >= 0 ? " " + ret : ret;
 }
 
+/**
+ * Convert latitude and longitudes to heading.
+ * @see https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/
+ * @param lati latitude of starting point in degrees.
+ * @param loni longitude of starting point in degrees.
+ * @param latf latitude of ending point in degrees.
+ * @param lonf longitude of ending point in degrees.
+ * @return The heading of the ending point relative to North (CW is +) in degrees.
+ */
+function convertCoordsToHeading(lati, loni, latf, lonf) {
+  const DEGREES_TO_RADIANS = Math.PI / 180;
+  lati *= DEGREES_TO_RADIANS;
+  loni *= DEGREES_TO_RADIANS;
+  latf *= DEGREES_TO_RADIANS;
+  lonf *= DEGREES_TO_RADIANS;
+
+  const deltaL = lonf - loni;
+  const x = Math.cos(latf) * Math.sin(deltaL);
+  const y = Math.cos(lati) * Math.sin(latf) - Math.sin(lati) * Math.cos(latf) * Math.cos(deltaL);
+  const bearing = Math.atan2(x, y);
+  return bearing / DEGREES_TO_RADIANS;
+}
+
+/**
+ * Convert latitude and longitudes to distance.
+ * @see https://en.wikipedia.org/wiki/Haversine_formula
+ * @param lati latitude of starting point in degrees.
+ * @param loni longitude of starting point in degrees.
+ * @param latf latitude of ending point in degrees.
+ * @param lonf longitude of ending point in degrees.
+ * @param radius radius of the planet in km (default is Earth: 6,371km).
+ * @return The distance in km.
+ */
+function convertCoordsToDistance(lati, loni, latf, lonf, radius = 6371) {
+  const DEGREES_TO_RADIANS = Math.PI / 180;
+  lati *= DEGREES_TO_RADIANS;
+  loni *= DEGREES_TO_RADIANS;
+  latf *= DEGREES_TO_RADIANS;
+  lonf *= DEGREES_TO_RADIANS;
+
+  let h = (1 - Math.cos(latf - lati) + Math.cos(lati) * Math.cos(latf) * (1 - Math.cos(lonf - loni))) / 2;
+  h = clamp(h, 0, 1);  // ensure 0 <= h <= 1
+  const d = 2 * radius * Math.asin(Math.sqrt(h));
+  return d;
+}
+
+const TARGET_CIRCLE_OFFSET = 42.5;  // how many degrees the target "circle" needs to be offset
 const Compass = () => {
   const { orientW, orientX, orientY, orientZ, lon, lat } = useSelector(selectRoverPosition);
 
@@ -61,6 +112,36 @@ const Compass = () => {
   }
   const heading = yaw != null ? -yaw : undefined; // yaw is CCW, heading is CW
 
+  const [targetHeading, setTargetHeading] = useState(null);
+  const [targetDistance, setTargetDistance] = useState(null);
+  const targetLongitude = useSelector(selectLongitude);
+  const targetLatitude = useSelector(selectLatitude);
+
+  /**
+   * Method to create text for the target distance indiciator  
+   */
+  function distanceToText(dist) {
+    if (dist == null) return "Unknown"
+    if (dist > FAR_DISTANCE) {
+      return `>${FAR_DISTANCE.toFixed(1)} m`;
+    } else if (dist < CLOSE_DISTANCE) {
+      return `<${CLOSE_DISTANCE.toFixed(1)} m`;
+    } else {
+      return `${targetDistance.toFixed(1)} m`;
+    }
+  };
+
+  useEffect(() => {
+    if (targetLongitude == null || targetLatitude == null || lat == null || lon == null) {
+      setTargetHeading(null);
+      setTargetDistance(null);
+      return;
+    }
+    setTargetHeading(convertCoordsToHeading(lat, lon, targetLatitude, targetLongitude));
+    let dist = convertCoordsToDistance(lat, lon, targetLatitude, targetLongitude) * 1000;
+    setTargetDistance(dist);
+  }, [targetLatitude, targetLongitude, lat, lon]);
+
   return (
     <div className="compass-container">
       <div className="info">
@@ -91,6 +172,10 @@ const Compass = () => {
       </div>
       <div className="compass">
         <div className="compass-parts">
+          {targetHeading != null && targetDistance > CLOSE_DISTANCE && <div
+            className={`target-dot`}
+            style={{ transform: `rotate(${targetHeading + TARGET_CIRCLE_OFFSET}deg)` }}
+          ></div>}
           <div
             className={`compass__needle compass__needle--${needleColor}`}
             style={{ transform: `rotate(${heading ?? 0}deg)` }}
@@ -100,6 +185,12 @@ const Compass = () => {
           <div className="compass__label compass__label--south">S</div>
           <div className="compass__label compass__label--west">W</div>
           <div className="compass__label compass__label--east">E</div>
+          {targetDistance != null &&
+            <div className="compass__label compass__label--distance">
+              <span className={targetDistance > CLOSE_DISTANCE ? "target-far" : "target-close"}>
+                Target: {distanceToText(targetDistance)}
+              </span>
+            </div>}
         </div>
       </div>
     </div>

src/components/navigation/WaypointNav.css

@@ -33,34 +33,39 @@
 }
 
 /* Enabled Buttons */
-.waypoint-select > button {
+.waypoint-select>button {
   background-color: #4fb185;
   border: none;
   color: white;
-  margin: 14px;
-  padding: 20px;
+  margin: 7px;
+  padding: 10px;
   display: block;
-  font-size: 35px;
+  font-size: 25px;
   font-weight: 600;
   border-radius: 5px;
   width: 50%;
+  cursor: pointer;
 }
 
 /* Disabled Buttons */
-.waypoint-select > button:disabled {
+.waypoint-select>button:disabled {
   background-color: #343836;
   border: none;
   color: white;
-  margin: 14px;
-  padding: 20px;
+  margin: 7px;
+  padding: 10px;
   display: block;
-  font-size: 35px;
+  font-size: 25px;
   font-weight: 600;
   border-radius: 5px;
   width: 50%;
+  cursor: not-allowed;
 }
 
-.waypoint-select > button:hover {
-  cursor: pointer;
+.waypoint-select>button:hover {
   background-color: linear-gradient(rgb(0 0 0/40%) 0 0);
 }
+
+.waypoint-select>.unset-waypoint-button {
+  background-color: #ed4245;
+}

src/components/navigation/WaypointNav.jsx

@@ -1,30 +1,47 @@
 import { useDispatch, useSelector } from "react-redux";
-import React, { useState, useEffect } from "react";
-import { requestWaypointNav } from "../../store/waypointNavSlice";
+import React, { useState, useEffect, useCallback } from "react";
+import { requestWaypointNav, setWaypointPosition, selectLatitude, selectLongitude } from "../../store/waypointNavSlice";
 import { selectOpMode } from "../../store/opModeSlice";
+import { selectRoverIsConnected } from "../../store/roverSocketSlice";
 import "./WaypointNav.css";
 
 function WaypointNav() {
   const dispatch = useDispatch();
   const [submitted, setSubmitted] = useState(false);
   const [lat, setLat] = useState(0);
   const [lon, setLon] = useState(0);
-  var opMode = useSelector(selectOpMode);
 
-  function handleSubmit (e) {
+  const [isWaypointSet, setIsWaypointSet] = useState(false);
+
+  const storedLat = useSelector(selectLatitude);
+  const storedLon = useSelector(selectLongitude);
+  const opMode = useSelector(selectOpMode);
+
+  const roverIsConnected = useSelector(selectRoverIsConnected);
+
+  const handleWaypoint = useCallback(() => {
+    dispatch(setWaypointPosition({
+      longitude: lon,
+      latitude: lat
+    }));
+  }, [lat, lon]);
+
+  function handleSubmit(e) {
     e.preventDefault();
-    const form = e.target;
-    const formData = new FormData(form);
-    const formJson = Object.fromEntries(formData.entries());
-    setSubmitted(true);
-    dispatch(requestWaypointNav(formJson));
+    if (roverIsConnected) {
+      const form = e.target;
+      const formData = new FormData(form);
+      const formJson = Object.fromEntries(formData.entries());
+      setSubmitted(true);
+      dispatch(requestWaypointNav(formJson));
+    }
   };
 
-  function grabFromClipboard () {
+  function grabFromClipboard() {
     navigator.clipboard.readText().then(text => {
       // Matches coordinates in the form of (-)*(.*), (-)*(.*)
       // where * are numbers and () are optional, e.g. -0.2, 0
-      if(text.match("-?\\d+\\.?\\d*, -?\\d+\\.?\\d*")) {
+      if (text.match("-?\\d+\\.?\\d*, -?\\d+\\.?\\d*")) {
         const [lat, lon] = text.split(", ", 2);
         setLat(lat);
         setLon(lon);
@@ -42,22 +59,31 @@ function WaypointNav() {
       setSubmitted(false);
     }
   }, [opMode]);
-
+
+  useEffect(() => {
+    setIsWaypointSet(storedLat != null && storedLon != null);
+  }, [storedLat, storedLon]);
+
   return (
-  <form method="post" onSubmit={handleSubmit} className="waypoint-select"> 
-    <div className="waypoint-select__params">
-      <label htmlFor="latitude">Latitude</label>
-      {submitted ? <input disabled value={lat} onChange={e => e}/> : <input type="number" step="any" name="latitude" value={lat} onChange={e => setLat(e.target.value)}/>}
-      <label htmlFor="longitude">Longitude</label>
-      {submitted ? <input disabled value={lon} onChange={e => e}/> : <input type="number" step="any" name="longitude" value={lon} onChange={e => setLon(e.target.value)}/>}
-      {submitted ? <button disabled>Copy from Clipboard</button> : <button type="button" onClick={grabFromClipboard}>Copy from Clipboard</button>}
-    </div>
-    <div className="waypoint-checkbox">
-      <label>{submitted ? <input disabled type="checkbox" name="isApproximate" /> : <input type="checkbox" name="isApproximate" />} Approximate</label>
-      <label>{submitted ? <input disabled type="checkbox" name="isGate" />: <input type="checkbox" name="isGate" />} Is Gate</label>
-    </div>
-    {opMode === "autonomous" && !submitted ? <button type="submit">Go</button> : <button disabled>Go</button>}
-  </form>
+    <form method="post" onSubmit={handleSubmit} className="waypoint-select">
+      <div className="waypoint-select__params">
+        <label htmlFor="latitude">Latitude</label>
+        <input disabled={isWaypointSet} type="number" step="any" name="latitude" value={lat} onChange={e => setLat(e.target.value)} />
+        <label htmlFor="longitude">Longitude</label>
+        <input disabled={isWaypointSet} type="number" step="any" name="longitude" value={lon} onChange={e => setLon(e.target.value)} />
+        <button disabled={isWaypointSet} type="button" onClick={grabFromClipboard}>Copy from Clipboard</button>
+      </div>
+      <div className="waypoint-checkbox">
+        <label><input disabled={isWaypointSet} type="checkbox" name="isApproximate" /> Approximate</label>
+        <label><input disabled={isWaypointSet} type="checkbox" name="isGate" /> Is Gate</label>
+      </div>
+      {
+        isWaypointSet ?
+          <button className='unset-waypoint-button' disabled={submitted} type="button" onClick={() => { dispatch(setWaypointPosition({ longitude: null, latitude: null })); }}>Unset Waypoint</button> :
+          <button type="button" onClick={handleWaypoint}>Set Waypoint</button>
+      }
+      <button disabled={!roverIsConnected || opMode !== "autonomous" || submitted || !isWaypointSet} type="submit">Go</button>
+    </form>
   );
 }
 

src/store/waypointNavSlice.js

@@ -1,8 +1,8 @@
 import { createSlice } from "@reduxjs/toolkit";
 
 const initialState = {
-  latitude: 0,
-  longitude: 0,
+  latitude: null,
+  longitude: null,
   isApproximate: false,
   isGate: false,
 };
@@ -12,20 +12,23 @@ const waypointNavSlice = createSlice({
   initialState,
   reducers: {
     requestWaypointNav(state, action) {
-      const { latitude, longitude, isApproximate, isGate } = action.payload;
-      state.latitude = typeof latitude == "string" ? Number.parseFloat(latitude) : latitude;
-      state.longitude = typeof longitude == "string" ? Number.parseFloat(longitude) : longitude;
+      const { isApproximate, isGate } = action.payload;
       state.isApproximate = !!isApproximate;
       state.isGate = !!isGate;
+    },
+    setWaypointPosition(state, action) {
+      const { latitude, longitude } = action.payload;
+      state.latitude = typeof latitude == "string" ? Number.parseFloat(latitude) : latitude;
+      state.longitude = typeof longitude == "string" ? Number.parseFloat(longitude) : longitude;
     }
   }
 });
 
-export const { requestWaypointNav } = waypointNavSlice.actions;
+export const { requestWaypointNav, setWaypointPosition } = waypointNavSlice.actions;
 
-export const selectLatitude = state => state.latitude;
-export const selectLongitude = state => state.longitude;
-export const selectIsApproximate = state => state.isApproximate;
-export const selectIsGate = state => state.isGate;
+export const selectLatitude = state => state.waypointNav.latitude;
+export const selectLongitude = state => state.waypointNav.longitude;
+export const selectIsApproximate = state => state.waypointNav.isApproximate;
+export const selectIsGate = state => state.waypointNav.isGate;
 
 export default waypointNavSlice.reducer;