Raghumanimehta/582 advanced gps visualizer (#591)

* improved gps reference

* added moving boat marker

* added moving boat marker

* Flake8 Linting

* Fixed documentation for true_bearing_to_plotly_cartesian

---------

Co-authored-by: Akshanjay Kompelli <63443017+FireBoyAJ24@users.noreply.github.com>

Author: raghumanimehta

src/local_pathfinding/local_pathfinding/coord_systems.py

@@ -34,6 +34,24 @@ def cartesian_to_true_bearing(cartesian: float) -> float:
     return (90 - cartesian + 360) % 360
 
 
+def true_bearing_to_plotly_cartesian(true_bearing: float) -> float:
+    """Convert a true bearing angle to the equivalent cartesian angle .
+
+    Args:
+        true_bearing (float): Angle where 0 is true north. Range: -180 < heading <= 180.
+        Increases in the clockwise direction till 180 degrees.
+        Decreases in the counter-clockwise direction till -180 (exclusive)
+    Returns:
+        float:  Angle where 0 is north and values increases clockwise.
+    """
+    assert -180 < true_bearing <= 180
+
+    plotly_cartesian = true_bearing
+    if -180 < true_bearing < 0:
+        plotly_cartesian += 360.0
+    return plotly_cartesian
+
+
 def meters_to_km(meters: float) -> float:
     return meters / 1000
 

src/local_pathfinding/local_pathfinding/visualizer.py

@@ -15,6 +15,7 @@
 Call the `dash_app` function with a multiprocessing shared Manager.queue to start the Dash app.
 """
 
+import math
 from multiprocessing import Queue
 from typing import List, Optional, Tuple
 
@@ -190,32 +191,49 @@ def live_update_plot(state: VisualizerState) -> go.Figure:
         name="Intermediate",
     )
 
+    goal_x = [state.final_local_wp_x[-1]]
+    goal_y = [state.final_local_wp_y[-1]]
+    boat_x = [state.sailbot_pos_x[-1]]
+    boat_y = [state.sailbot_pos_y[-1]]
+    angle_from_boat = math.atan2(goal_x[0] - boat_x[0], goal_y[0] - boat_y[0])
+    angle_degrees = math.degrees(angle_from_boat)
+
     # goal local waypoint
     goal_trace = go.Scatter(
-        x=[state.final_local_wp_x[-1]],
-        y=[state.final_local_wp_y[-1]],
+        x=goal_x,
+        y=goal_y,
         mode="markers",
         marker=dict(color="red", size=10),
         name="Goal",
+        hovertemplate="X: %{x:.2f} <br>"
+        + "Y: %{y:.2f} <br>"
+        + "Angle from the boat: "
+        + f"{angle_degrees:.1f}°"
+        + "<extra></extra>",
     )
 
-    # boat marker (current position)
     boat_trace = go.Scatter(
         x=[state.sailbot_pos_x[-1]],
         y=[state.sailbot_pos_y[-1]],
         mode="markers",
-        marker_symbol="arrow",
-        marker_line_color="darkseagreen",
-        marker_color="lightgreen",
-        marker_line_width=2,
-        marker_size=15,
         name="Boat",
-        hovertemplate="<b>🚢 Sailbot Current Position</b><br>" +
-        "X: %{x:.2f} meters<br>" +
-        "Y: %{y:.2f} meters<br>" +
-        "Heading: " + f"{state.sailbot_gps[-1].heading.heading:.1f}°<br>" +
-        "Speed: " + f"{state.sailbot_gps[-1].speed.speed:.1f}<br>" +
-        "<extra></extra>"
+        hovertemplate=(
+            "<b>🚢 Sailbot Current Position</b><br>"
+            "X: %{x:.2f} <br>"
+            "Y: %{y:.2f} <br>"
+            "Heading: " + f"{state.sailbot_gps[-1].heading.heading:.1f}°<br>"
+            f"Speed: {state.sailbot_gps[-1].speed.speed:.1f}<br>"
+            "<extra></extra>"
+        ),
+        marker=dict(
+            symbol="arrow-wide",
+            line_color="darkseagreen",
+            color="lightgreen",
+            line_width=2,
+            size=15,
+            angleref="up",
+            angle=cs.true_bearing_to_plotly_cartesian(state.sailbot_gps[-1].heading.heading),
+        ),
     )
 
     # Add all traces to the figure
@@ -265,12 +283,14 @@ def animated_update_plot(state: VisualizerState) -> go.Figure:
         marker_size=15,
         text=["Boat"],
         name="Boat",
-        hovertemplate="<b>🚢 Sailbot Current Position</b><br>" +
-        "X: %{x:.2f} meters<br>" +
-        "Y: %{y:.2f} meters<br>" +
-        "Heading: " + f"{state.sailbot_gps[0].heading.heading:.1f}°<br>" +
-        "Speed: " + f"{state.sailbot_gps[0].speed.speed:.1f}<br>" +
-        "<extra></extra>"
+        hovertemplate="<b>🚢 Sailbot Current Position</b><br>"
+        + "X: %{x:.2f} meters<br>"
+        + "Y: %{y:.2f} meters<br>"
+        + "Heading: "
+        + f"{state.sailbot_gps[0].heading.heading:.1f}°<br>"
+        + "Speed: "
+        + f"{state.sailbot_gps[0].speed.speed:.1f}<br>"
+        + "<extra></extra>",
     )
     initial_state = [
         go.Scatter(
@@ -338,12 +358,14 @@ def animated_update_plot(state: VisualizerState) -> go.Figure:
                     marker_size=15,
                     text=["Boat"],
                     name="Boat",
-                    hovertemplate="<b>🚢 Sailbot Current Position</b><br>" +
-                    "X: %{x:.2f} meters<br>" +
-                    "Y: %{y:.2f} meters<br>" +
-                    "Heading: " + f"{state.sailbot_gps[i].heading.heading:.1f}°<br>" +
-                    "Speed: " + f"{state.sailbot_gps[i].speed.speed:.1f}<br>" +
-                    "<extra></extra>"
+                    hovertemplate="<b>🚢 Sailbot Current Position</b><br>"
+                    + "X: %{x:.2f} meters<br>"
+                    + "Y: %{y:.2f} meters<br>"
+                    + "Heading: "
+                    + f"{state.sailbot_gps[i].heading.heading:.1f}°<br>"
+                    + "Speed: "
+                    + f"{state.sailbot_gps[i].speed.speed:.1f}<br>"
+                    + "<extra></extra>",
                 )
             ],
             name=f"Boat {i}",

src/local_pathfinding/test/test_coord_systems.py

@@ -23,6 +23,29 @@ def test_cartesian_to_true_bearing(cartesian: float, true_bearing: float):
     ), "incorrect angle conversion"
 
 
+@pytest.mark.parametrize(
+    "true_bearing, plotly_cartesian",
+    [
+        (0.0, 0.0),
+        (-90.0, 270.0),
+        (180.0, 180.0),
+        (90.0, 90.0),
+        (45.0, 45.0),
+        (-45.0, 315.0),
+        (135.0, 135.0),
+        (-135.0, 225.0),
+        (1.0, 1.0),
+        (-1.0, 359.0),
+        (-179.0, 181.0),
+        (179.0, 179.0),
+    ],
+)
+def test_true_bearing_to_plotly_cartesian(true_bearing: float, plotly_cartesian: float):
+    assert cs.true_bearing_to_plotly_cartesian(true_bearing) == pytest.approx(
+        plotly_cartesian
+    ), "incorrect angle conversion"
+
+
 @pytest.mark.parametrize(
     "meters,km",
     [(0.0, 0.0), (30, 0.03), (500, 0.5), (-30.5, -0.0305), (-0.0, 0.0)],