Raghumanimehta/735 refactorings (#739)

* initial changes

* fixed tests

* minor change in node_mock_wind_sensor's use of get_apparent_wind

* removed import

---------

Co-authored-by: Sean Donaghy <[email protected]>

Author: raghumanimehta

src/local_pathfinding/local_pathfinding/coord_systems.py

@@ -50,8 +50,6 @@ def true_bearing_to_plotly_cartesian(true_bearing_deg: float) -> float:
     Returns:
         float:  Angle where 0 is north and values increases clockwise.
     """
-    assert -180 < true_bearing_deg <= 180
-
     plotly_cartesian = true_bearing_deg
     if -180 < true_bearing_deg < 0:
         plotly_cartesian += 360.0
@@ -81,41 +79,23 @@ def get_path_segment_true_bearing(s1: XY, s2: XY, rad: bool = False):
     return bound_to_180(math.degrees(segment_true_bearing_rad))
 
 
-def angle_to_vector_projections(vector_angle_rad: float, vector_magnitude: float) -> XY:
-    """Convert a polar vector (angle in radians, speed_knots) to east/north Cartesian components.
+def polar_to_cartesian(angle_rad: float, magnitude: float) -> XY:
+    """Convert polar coordinates to Cartesian coordinates.
 
     Args:
-        vector_angle_rad (float): Direction angle in radians. Range: (-π, π], where 0 rad = north,
-            +π/2 = east.
-        vector_magnitude (float): Vector speed (e.g., true wind speed in kmph).
+        angle_rad (float): Direction angle in radians. Range: (-pi, pi], where 0 rad = north,
+            +pi/2 = east.
+        magnitude (float): Vector magnitude (e.g., speed in kmph).
 
     Returns:
-        XY: Decomposed vector components in the global (east, north) frame:
+        XY: Cartesian vector components in the global (east, north) frame:
             - x → east component
             - y → north component
     """
-    # case 1: vector in quadrant I
-    if 0 <= vector_angle_rad <= PI / 2:
-        return XY(
-            x=vector_magnitude * math.sin(vector_angle_rad),
-            y=vector_magnitude * math.cos(vector_angle_rad),
-        )
-    # case 2: vector in quadrant IV
-    elif PI / 2 < vector_angle_rad <= PI:
-        alpha = vector_angle_rad - (PI / 2)  # alpha is with respect to positive x-axis
-        return XY(x=vector_magnitude * math.cos(alpha), y=vector_magnitude * -1 * math.sin(alpha))
-    # case 3: vector in quadrant II
-    elif -PI / 2 <= vector_angle_rad < 0:
-        alpha = abs(vector_angle_rad)  # vector_angle_rad is negative in quadrant II
-        return XY(x=vector_magnitude * -1 * math.sin(alpha), y=vector_magnitude * math.cos(alpha))
-    # case 4: vector in quadrant III
-    elif -PI <= vector_angle_rad < -PI / 2:
-        # vector_angle_rad is negative in quadrant III and alpha is with respect to negative x-axis
-        alpha = abs(vector_angle_rad) - (PI / 2)
-        return XY(
-            x=vector_magnitude * -1 * math.cos(alpha), y=vector_magnitude * -1 * math.sin(alpha)
-        )
-    return XY(x=0.0, y=0.0)  # place holder for invalid input
+    return XY(
+        x=magnitude * math.sin(angle_rad),
+        y=magnitude * math.cos(angle_rad)
+    )
 
 
 def meters_to_km(meters: float) -> float:

src/local_pathfinding/local_pathfinding/mock_nodes/node_mock_gps.py

@@ -136,7 +136,7 @@ def wind_sensor_callback(self, msg: ci.WindSensor):
         self._logger.debug(f"Received data from {self.__mock_wind_sensor_sub.topic}: {msg}")
         aw_speed_kmph: float = msg.speed.speed
         aw_direction_deg: float = msg.direction
-        tw_direction_rad, tw_speed_kmph = wcs.get_true_wind(
+        tw_dir_rad, tw_speed_kmph = wcs.get_true_wind(
             aw_direction_deg,
             aw_speed_kmph,
             self.__heading_deg.heading,
@@ -146,7 +146,7 @@ def wind_sensor_callback(self, msg: ci.WindSensor):
             speed=float(
                 TimeObjective.get_sailbot_speed(
                     math.radians(self.__heading_deg.heading),
-                    tw_direction_rad,
+                    tw_dir_rad,
                     tw_speed_kmph,
                 )
             )

src/local_pathfinding/local_pathfinding/mock_nodes/node_mock_wind_sensor.py

@@ -25,7 +25,6 @@
 from typing import List
 
 import custom_interfaces.msg as ci
-import numpy as np
 import rclpy
 from rcl_interfaces.msg import SetParametersResult
 from rclpy.node import Node
@@ -78,15 +77,16 @@ def __init__(self):
         self.add_on_set_parameters_callback(self._on_set_parameters)
 
     def mock_wind_sensor_callback(self):
-        aw_speed_kmph, aw_direction_rad = wcs.get_apparent_wind(
+        aw_dir_deg, aw_speed_kmph = wcs.get_apparent_wind(
             self._tw_dir_deg,
             self._tw_speed_kmph,
             self._boat_heading_deg,
             self._boat_speed,
+            ret_rad=False
         )
         aw_dir_boat_coord_deg = wcs.global_to_boat_coordinate(
             self._boat_heading_deg,
-            np.degrees(aw_direction_rad),
+            aw_dir_deg
         )
         msg = ci.WindSensor(
             speed=ci.HelperSpeed(speed=aw_speed_kmph),

src/local_pathfinding/local_pathfinding/ompl_objectives.py

@@ -223,7 +223,7 @@ def get_sailing_objective(
         boat_heading_degrees, apparent_wind_direction_degrees
     )
 
-    true_wind_direction_radians, true_wind_speed_kmph = wcs.get_true_wind(
+    tw_dir_rad, tw_speed_kmph = wcs.get_true_wind(
         apparent_wind_direction_degrees_global_coordinates,
         apparent_wind_speed_kmph,
         boat_heading_degrees,
@@ -234,13 +234,13 @@ def get_sailing_objective(
     multiObjective.addObjective(
         objective=WindObjective(
             space_information,
-            true_wind_direction_radians,
+            tw_dir_rad,
         ),
         weight=WIND_OBJECTIVE_WEIGHT,
     )
     multiObjective.addObjective(
         objective=TimeObjective(
-            space_information, true_wind_direction_radians, true_wind_speed_kmph
+            space_information, tw_dir_rad, tw_speed_kmph
         ),
         weight=TIME_OBJECTIVE_WEIGHT,
     )

src/local_pathfinding/local_pathfinding/visualizer.py

@@ -109,16 +109,16 @@ def __init__(self, msgs: deque[ci.LPathData]):
         aw_dir_global_rad = math.radians(aw_dir_global)
 
         # Compute apparent wind vector (in global frame)
-        self.aw_wind_vector = cs.angle_to_vector_projections(aw_dir_global_rad, aw_speed)
+        self.aw_wind_vector = cs.polar_to_cartesian(aw_dir_global_rad, aw_speed)
 
         # True wind from apparent
-        true_wind_angle_rad, true_wind_mag = wcs.get_true_wind(
+        tw_dir_rad, tw_speed_kmph = wcs.get_true_wind(
             aw_dir_global, aw_speed, boat_heading, boat_speed
         )
-        self.true_wind_vector = cs.angle_to_vector_projections(true_wind_angle_rad, true_wind_mag)
+        self.true_wind_vector = cs.polar_to_cartesian(tw_dir_rad, tw_speed_kmph)
         # Boat wind vector
         boat_wind_radians = math.radians(cs.bound_to_180(boat_heading + 180))
-        self.boat_wind_vector = cs.angle_to_vector_projections(boat_wind_radians, boat_speed)
+        self.boat_wind_vector = cs.polar_to_cartesian(boat_wind_radians, boat_speed)
 
     def _validate_message(self, msg: ci.LPathData):
         """Checks if the sailbot observer node received any messages.

src/local_pathfinding/local_pathfinding/wind_coord_systems.py

@@ -41,27 +41,29 @@ def global_to_boat_coordinate(boat_heading: float, global_wind_direction: float)
 
 
 def get_true_wind(
-    aw_direction: float,
-    aw_speed: float,
-    boat_heading: float,
-    boat_speed: float,
+    aw_dir_deg: float,
+    aw_speed_kmph: float,
+    boat_heading_deg: float,
+    boat_speed_kmph: float,
+    ret_rad: bool = True,
 ) -> tuple[float, float]:
     """Compute the true wind vector from apparent wind and boat motion.
 
     Args:
-        aw_direction (float): Apparent wind direction in degrees (-180, 180]. This is the wind
+        aw_dir_deg (float): Apparent wind direction in degrees (-180, 180]. This is the wind
             as measured relative to the boat (sensor reading).
-        aw_speed (float): Apparent wind speed (same units as boat_speed), e.g., km/h.
-        boat_heading (float): Boat heading in degrees (-180, 180].
-        boat_speed (float): Boat speed over ground (same units as aw_speed), e.g., km/h.
+        aw_speed_kmph (float): Apparent wind speed in km/h.
+        boat_heading_deg (float): Boat heading in degrees (-180, 180].
+        boat_speed_kmph (float): Boat speed over ground in km/h.
+        ret_rad (bool): If True, return wind direction in radians. If False, return in degrees.
         NOTE: All the angles are with respect to global true bearing. It is the
         responsibility of the caller to ensure this. Particularly, the apparent wind read by the
         sensor is in boat coordinates
 
     Returns:
-        tuple[float, float]: (tw_angle, tw_magnitude)
-            - tw_angle: true wind direction in radians within (-pi, pi].
-            - tw_magnitude: true wind speed
+        tuple[float, float]: (tw_dir_rad, tw_speed_kmph)
+            - tw_dir_rad: true wind direction in radians within (-pi, pi].
+            - tw_speed_kmph: true wind speed in km/h
         If the resulting vector magnitude is effectively zero (<= FLOATING_POINT_ERROR_THRESHOLD),
         returns (0.0, 0.0). NOTE: The caller is responsible for handling this case, otherwise the
         calculations will break down.
@@ -70,65 +72,79 @@ def get_true_wind(
         The function computes vector components in an east/north frame, subtracting the boat
         motion from the apparent wind to obtain the true wind.
     """
-    wind_radians = math.radians(aw_direction)
+    aw_dir_rad = math.radians(aw_dir_deg)
 
     # boat wind is in the direction of the boat heading (reverse of boat heading)
-    boat_wind_radians = math.radians(cs.bound_to_180(boat_heading + 180))
-    apparent_wind_east = aw_speed * math.sin(wind_radians)
-    apparent_wind_north = aw_speed * math.cos(wind_radians)
+    bw_dir_rad = math.radians(cs.bound_to_180(boat_heading_deg + 180))
+    aw_east_kmph = aw_speed_kmph * math.sin(aw_dir_rad)
+    aw_north_kmph = aw_speed_kmph * math.cos(aw_dir_rad)
 
-    boat_wind_east = boat_speed * math.sin(boat_wind_radians)
-    boat_wind_north = boat_speed * math.cos(boat_wind_radians)
+    bw_east_kmph = boat_speed_kmph * math.sin(bw_dir_rad)
+    bw_north_kmph = boat_speed_kmph * math.cos(bw_dir_rad)
 
-    true_east = apparent_wind_east - boat_wind_east
-    true_north = apparent_wind_north - boat_wind_north
+    tw_east_kmph = aw_east_kmph - bw_east_kmph
+    tw_north_kmph = aw_north_kmph - bw_north_kmph
 
-    magnitude = math.hypot(true_east, true_north)
-    angle = math.atan2(true_east, true_north)
+    tw_speed_kmph = math.hypot(tw_east_kmph, tw_north_kmph)
+    tw_dir_rad = math.atan2(tw_east_kmph, tw_north_kmph)
 
-    if magnitude > FLOATING_POINT_ERROR_THRESHOLD:
-        return angle, magnitude
+    if ret_rad:
+        tw_dir = tw_dir_rad
+    else:
+        tw_dir = math.degrees(tw_dir_rad)
+
+    if tw_speed_kmph > FLOATING_POINT_ERROR_THRESHOLD:
+        return tw_dir, tw_speed_kmph
     return ZERO_VECTOR_CONSTANT, 0.0
 
 
-def get_apparent_wind(tw_direction, tw_speed, boat_heading, boat_speed) -> tuple[float, float]:
+def get_apparent_wind(
+    tw_dir_deg: float,
+    tw_speed_kmph: float,
+    boat_heading_deg: float,
+    boat_speed_kmph: float,
+    ret_rad: bool = True,
+) -> tuple[float, float]:
     """Compute the apparent wind vector from true wind and boat motion.
 
     Args:
-        tw_direction (float): True wind direction in radians (measured as atan2(east, north) style)
-            or degrees if caller uses degrees consistently. (This routine converts using math.radians # noqa
-            in the original code; keep inputs consistent with usage.)
-        tw_speed (float): True wind speed (same units as boat_speed), e.g., km/h.
-        boat_heading (float): Boat heading in degrees (-180, 180].
-        boat_speed (float): Boat speed over ground (same units as tw_speed), e.g., km/h.
+        tw_dir_deg (float): True wind direction in degrees (-180, 180].
+        tw_speed_kmph (float): True wind speed in km/h.
+        boat_heading_deg (float): Boat heading in degrees (-180, 180].
+        boat_speed_kmph (float): Boat speed over ground in km/h.
+        ret_rad (bool): If True, return wind direction in radians. If False, return in degrees.
         NOTE: All the angles are with respect to global true bearing. It is the
         responsibility of the caller to ensure this.
 
     Returns:
-        tuple[float, float]: (aw_angle, aw_magnitude)
-            - aw_angle: apparent wind direction in radians within (-pi, pi]
-            - aw_magnitude: apparent wind speed (same units as inputs).
+        tuple[float, float]: (aw_dir_rad, aw_speed_kmph)
+            - aw_dir_rad: apparent wind direction in radians within (-pi, pi]
+            - aw_speed_kmph: apparent wind speed in km/h.
         If the resulting vector magnitude is effectively zero (<= FLOATING_POINT_ERROR_THRESHOLD),
         returns (0.0, 0.0). NOTE: The caller is responsible for handling this case, otherwise the
         calculations will break down.
     """
-    tw_direction = math.radians(tw_direction)
+    tw_dir_rad = math.radians(tw_dir_deg)
+
+    bw_speed_kmph = boat_speed_kmph
+    bw_dir_rad = math.radians(cs.bound_to_180(boat_heading_deg + 180.0))
+    bw_east_kmph = bw_speed_kmph * math.sin(bw_dir_rad)
+    bw_north_kmph = bw_speed_kmph * math.cos(bw_dir_rad)
 
-    bw_speed = boat_speed
-    bw_direction = math.radians(cs.bound_to_180(boat_heading + 180.0))
-    # print(math.degrees(bw_direction))
-    boat_wind_east = bw_speed * math.sin(bw_direction)
-    boat_wind_north = bw_speed * math.cos(bw_direction)
+    tw_east_kmph = tw_speed_kmph * math.sin(tw_dir_rad)
+    tw_north_kmph = tw_speed_kmph * math.cos(tw_dir_rad)
 
-    tw_speed_east = tw_speed * math.sin(tw_direction)
-    tw_speed_north = tw_speed * math.cos(tw_direction)
+    aw_east_kmph = tw_east_kmph + bw_east_kmph
+    aw_north_kmph = tw_north_kmph + bw_north_kmph
 
-    aw_speed_east = tw_speed_east + boat_wind_east
-    aw_speed_north = tw_speed_north + boat_wind_north
+    aw_speed_kmph = math.hypot(aw_east_kmph, aw_north_kmph)
+    aw_dir_rad = math.atan2(aw_east_kmph, aw_north_kmph)
 
-    magnitude = math.hypot(aw_speed_east, aw_speed_north)
-    angle = math.atan2(aw_speed_east, aw_speed_north)
+    if ret_rad:
+        aw_dir = aw_dir_rad
+    else:
+        aw_dir = math.degrees(aw_dir_rad)
 
-    if magnitude > FLOATING_POINT_ERROR_THRESHOLD:
-        return angle, magnitude
+    if aw_speed_kmph > FLOATING_POINT_ERROR_THRESHOLD:
+        return aw_dir, aw_speed_kmph
     return ZERO_VECTOR_CONSTANT, 0.0

src/local_pathfinding/test/test_coord_systems.py

@@ -119,8 +119,8 @@ def test_true_bearing_to_plotly_cartesian(true_bearing: float, plotly_cartesian:
         (NORTHEAST, 0.0, cs.XY(0.0, 0.0))
     ],
 )
-def test_angle_to_vector_projections(angle_rad: float, speed: float, expected_xy: cs.XY):
-    result = cs.angle_to_vector_projections(angle_rad, speed)
+def test_polar_to_cartesian(angle_rad: float, speed: float, expected_xy: cs.XY):
+    result = cs.polar_to_cartesian(angle_rad, speed)
 
     # Component-wise check
     assert (result.x, result.y) == pytest.approx(

src/local_pathfinding/test/test_wind_coord_systems.py

@@ -66,16 +66,16 @@ def test_get_true_wind_direction(
     expected_direction: float,
     expected_speed: float,
 ):
-    true_wind_direction, true_wind_speed = wcs.get_true_wind(
+    tw_dir_rad, tw_speed_kmph = wcs.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)
+    tw_dir_deg = math.degrees(tw_dir_rad)
 
-    assert true_wind_direction_degrees == pytest.approx(
+    assert tw_dir_deg == pytest.approx(
         expected=expected_direction, abs=1e-2
-    ) and true_wind_speed == pytest.approx(expected=expected_speed, abs=1e-2)
+    ) and tw_speed_kmph == pytest.approx(expected=expected_speed, abs=1e-2)
 
 
 @pytest.mark.parametrize(
@@ -96,14 +96,14 @@ def test_get_apparent_wind_direction(
     expected_direction: float,
     expected_speed: float,
 ):
-    aw_direction, aw_speed = wcs.get_apparent_wind(
+    aw_dir_rad, aw_speed_kmph = wcs.get_apparent_wind(
         tw_direction_degrees,
         tw_speed, heading_degrees, speed
     )
 
     # Convert radians to degrees for easier comparison
-    aw_direction_deg = math.degrees(aw_direction)
+    aw_dir_deg = math.degrees(aw_dir_rad)
 
-    assert aw_direction_deg == pytest.approx(
+    assert aw_dir_deg == pytest.approx(
         expected=expected_direction, abs=1e-2
-    ) and aw_speed == pytest.approx(expected=expected_speed, abs=1e-2)
+    ) and aw_speed_kmph == pytest.approx(expected=expected_speed, abs=1e-2)