Pathfinding Mock GPS node fixed

src/local_pathfinding/local_pathfinding/node_mock_gps.py

@@ -5,18 +5,109 @@
 import math
 
 import custom_interfaces.msg as ci
+import numpy as np
 import rclpy
 from geopy.distance import great_circle
 from rclpy.node import Node
-from local_pathfinding.objectives import SpeedObjective
 
+from local_pathfinding.objectives import get_true_wind
 
 MEAN_SPEED = ci.HelperSpeed(speed=15.0)  # mean boat speed in kmph
 
 START_POINT = ci.HelperLatLon(latitude=48.46, longitude=-125.1)  # Starting location of the mock
 START_HEADING = ci.HelperHeading(heading=180.0)  # in degrees, heading of the boat
 
 
+BOATSPEEDS = np.array(
+    [
+        [0, 0, 0, 0, 0, 0, 0],
+        [0, 0, 0.4, 1.1, 3.2, 3.7, 2.8],
+        [0, 0.3, 1.9, 3.7, 9.3, 13.0, 9.2],
+        [0, 0.9, 3.7, 7.4, 14.8, 18.5, 13.0],
+        [0, 1.3, 5.6, 9.3, 18.5, 24.1, 18.5],
+    ]
+)
+
+WINDSPEEDS = [0, 9.3, 18.5, 27.8, 37.0]  # The row labels
+ANGLES = [0, 20, 30, 45, 90, 135, 180]  # The column labels
+
+
+def get_sailbot_speed(
+    heading: float, apparent_wind_direction: float, apparent_wind_speed: float, boat_speed: float
+) -> float:
+    """Get an estimated boat speed given the wind conditions and the speed of the boat from the
+    previous iteration.
+
+    Returns:
+        float: the interpolated boat speed
+    """
+
+    true_wind_direction, true_wind_speed = get_true_wind(
+        apparent_wind_direction=apparent_wind_direction,
+        apparent_wind_speed=apparent_wind_speed,
+        heading_degrees=heading,
+        boat_speed_over_ground=boat_speed,
+    )
+
+    # Get the sailing angle: [0, 180]
+    sailing_angle = abs(heading - true_wind_direction)
+    sailing_angle = min(sailing_angle, 360 - sailing_angle)
+
+    # Find the nearest windspeed values above and below the true windspeed
+    lower_windspeed_index = max([i for i, ws in enumerate(WINDSPEEDS) if ws <= true_wind_speed])
+    upper_windspeed_index = (
+        lower_windspeed_index + 1
+        if lower_windspeed_index < len(WINDSPEEDS) - 1
+        else lower_windspeed_index
+    )
+
+    # Find the nearest angle values above and below the sailing angle
+    lower_angle_index = max([i for i, ang in enumerate(ANGLES) if ang <= sailing_angle])
+    upper_angle_index = (
+        lower_angle_index + 1 if lower_angle_index < len(ANGLES) - 1 else lower_angle_index
+    )
+
+    # Find the maximum angle and maximum windspeed based on the actual data in the table
+    max_angle = max(ANGLES)
+    max_windspeed = max(WINDSPEEDS)
+
+    # Handle the case of maximum angle (use the dynamic max_angle)
+    if upper_angle_index == len(ANGLES) - 1:
+        lower_angle_index = ANGLES.index(max_angle) - 1
+        upper_angle_index = ANGLES.index(max_angle)
+
+    # Handle the case of the maximum windspeed (use the dynamic max_windspeed)
+    if upper_windspeed_index == len(WINDSPEEDS) - 1:
+        lower_windspeed_index = WINDSPEEDS.index(max_windspeed) - 1
+        upper_windspeed_index = WINDSPEEDS.index(max_windspeed)
+
+    # Perform linear interpolation
+    lower_windspeed = WINDSPEEDS[lower_windspeed_index]
+    upper_windspeed = WINDSPEEDS[upper_windspeed_index]
+    lower_angle = ANGLES[lower_angle_index]
+    upper_angle = ANGLES[upper_angle_index]
+
+    boat_speed_lower = BOATSPEEDS[lower_windspeed_index][lower_angle_index]
+    boat_speed_upper = BOATSPEEDS[upper_windspeed_index][lower_angle_index]
+
+    interpolated_1 = boat_speed_lower + (true_wind_speed - lower_windspeed) * (
+        boat_speed_upper - boat_speed_lower
+    ) / (upper_windspeed - lower_windspeed)
+
+    boat_speed_lower = BOATSPEEDS[lower_windspeed_index][upper_angle_index]
+    boat_speed_upper = BOATSPEEDS[upper_windspeed_index][upper_angle_index]
+
+    interpolated_2 = boat_speed_lower + (true_wind_speed - lower_windspeed) * (
+        boat_speed_upper - boat_speed_lower
+    ) / (upper_windspeed - lower_windspeed)
+
+    interpolated_value = interpolated_1 + (sailing_angle - lower_angle) * (
+        interpolated_2 - interpolated_1
+    ) / (upper_angle - lower_angle)
+
+    return interpolated_value
+
+
 class MockGPS(Node):
 
     def __init__(self):
@@ -129,14 +220,15 @@ def wind_sensor_callback(self, msg: ci.WindSensor):
         """
 
         self._logger.debug(f"Received data from {self.__mock_wind_sensor_sub.topic}: {msg}")
-        wind_speed: float = msg.speed.speed
-        wind_direction: float = msg.direction
+        apparent_wind_speed: float = msg.speed.speed
+        apparent_wind_direction: float = msg.direction
         self.__mean_speed = ci.HelperSpeed(
-            speed=SpeedObjective.get_sailbot_speed(
-                    heading=self.__heading.heading,
-                    wind_direction=wind_direction,
-                    wind_speed=wind_speed,
-                )
+            speed=get_sailbot_speed(
+                heading=self.__heading.heading,
+                apparent_wind_direction=apparent_wind_direction,
+                apparent_wind_speed=apparent_wind_speed,
+                boat_speed=self.__mean_speed.speed,
+            )
         )
 
 

src/local_pathfinding/local_pathfinding/objectives.py

@@ -1,13 +1,14 @@
 """Our custom OMPL optimization objectives."""
 
 import math
+from typing import Tuple
 
 import custom_interfaces.msg as ci
 import numpy as np
 from ompl import base as ob
-from local_pathfinding.coord_systems import bound_to_180
 
 import local_pathfinding.coord_systems as cs
+from local_pathfinding.coord_systems import bound_to_180
 
 # Upwind downwind cost multipliers
 UPWIND_MULTIPLIER = 3000.0
@@ -32,8 +33,12 @@
 ANGLES = [0, 20, 30, 45, 90, 135, 180]  # The column labels
 
 
-def get_true_wind_direction(apparent_wind_direction: float, apparent_wind_speed: float,
-                            heading_degrees: float, boat_speed_over_ground: float) -> float:
+def get_true_wind(
+    apparent_wind_direction: float,
+    apparent_wind_speed: float,
+    heading_degrees: float,
+    boat_speed_over_ground: float,
+) -> Tuple[float, float]:
     """Calculates the true wind direction based on the boat's heading and speed.
     Args:
         apparent_wind_direction (float): The direction of the wind in degrees (-180, 180]. This
@@ -60,7 +65,7 @@ def get_true_wind_direction(apparent_wind_direction: float, apparent_wind_speed:
     true_east = apparent_wind_east - boat_wind_east
     true_north = apparent_wind_north - boat_wind_north
 
-    return math.atan2(true_east, true_north)
+    return (math.atan2(true_east, true_north), math.hypot(true_north, true_east))
 
 
 class Objective(ob.StateCostIntegralObjective):
@@ -172,7 +177,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """
         s1_xy = cs.XY(s1.getX(), s1.getY())
         s2_xy = cs.XY(s2.getX(), s2.getY())
-        threshold = math.pi/9  # 20 degrees around the angle to next waypoint
+        threshold = math.pi / 9  # 20 degrees around the angle to next waypoint
 
         # calculate the difference in angle between s1 and s2
         raw_angle_s1_s2 = math.atan2(s2_xy.y - s1_xy.y, s2_xy.x - s1_xy.x)
@@ -219,15 +224,18 @@ class WindObjective(Objective):
         wind_direction (float): The direction of the wind in radians (-pi, pi]
     """
 
-    def __init__(self, space_information, wind_direction_degrees: float, wind_speed: float,
-                 heading_degrees: float, speed: float):
+    def __init__(
+        self,
+        space_information,
+        wind_direction_degrees: float,
+        wind_speed: float,
+        heading_degrees: float,
+        speed: float,
+    ):
         super().__init__(space_information)
         assert -180 < wind_direction_degrees <= 180
-        self.wind_direction = get_true_wind_direction(
-            wind_direction_degrees,
-            wind_speed,
-            heading_degrees,
-            speed
+        self.wind_direction, _ = get_true_wind(
+            wind_direction_degrees, wind_speed, heading_degrees, speed
         )
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
@@ -465,13 +473,9 @@ def get_sailing_objective(
     )
     objective.addObjective(
         objective=WindObjective(
-            space_information,
-            wind_direction_degrees,
-            wind_speed,
-            heading_degrees,
-            speed
+            space_information, wind_direction_degrees, wind_speed, heading_degrees, speed
         ),
-        weight=5.0
+        weight=5.0,
     )
     objective.addObjective(
         objective=SpeedObjective(

src/local_pathfinding/test/test_objectives.py

@@ -8,7 +8,7 @@
 import local_pathfinding.objectives as objectives
 import local_pathfinding.ompl_path as ompl_path
 from local_pathfinding.local_path import LocalPathState
-from local_pathfinding.objectives import get_true_wind_direction
+from local_pathfinding.objectives import get_true_wind
 
 # Upwind downwind cost multipliers
 UPWIND_MULTIPLIER = 3000.0
@@ -149,14 +149,14 @@ def test_get_sailbot_speed(
 
 
 @pytest.mark.parametrize(
-    "wind_direction_degrees,wind_speed,heading_degrees,speed,expected_direction",
+    "wind_direction_degrees,wind_speed,heading_degrees,speed,expected_direction, expected_speed",
     [
-        (0, 0, 0, 0, 0),
-        (10, 0, 10, 10, 10),
-        (179, 17, 179, 9, 179),
-        (180, 17, 179, 9, 179.65),
-        (140, 17, 45, 9, 111.06),
-        (80, 5, -70, 8, -35.74),
+        (0, 0, 0, 0, 0, 0),
+        (10, 0, 10, 10, 10, 10),
+        (179, 17, 179, 9, 179, 26),
+        (180, 17, 179, 9, 179.65, 26),
+        (140, 17, 45, 9, 111.06, 18.52),
+        (80, 5, -70, 8, -35.74, 4.44),
     ],
 )
 def test_get_true_wind_direction(
@@ -165,12 +165,15 @@ def test_get_true_wind_direction(
     heading_degrees: float,
     speed: float,
     expected_direction: float,
+    expected_speed: float,
 ):
-    true_wind_direction = get_true_wind_direction(
+    true_wind_direction, true_wind_speed = get_true_wind(
         wind_direction_degrees, wind_speed, heading_degrees, speed
     )
 
     # Convert radians to degrees for easier comparison
     true_wind_direction_degrees = math.degrees(true_wind_direction)
 
-    assert true_wind_direction_degrees == pytest.approx(expected=expected_direction, abs=1e-2)
+    assert true_wind_direction_degrees == pytest.approx(
+        expected=expected_direction, abs=1e-2
+    ) and true_wind_speed == pytest.approx(expected=expected_speed, abs=1e-2)