Remove any magic numbers and replace with raised ValueErrors (#545)

SPDonaghy · jamenkaye · FireBoyAJ24 · web-flow · commit 8f49699a5ab5 · 2025-04-05T21:12:29.000Z
* updated  lpath_data_callback docstr

* Fix typing in LocalPath.py

Change LocalPath.waypoints to List[HelperLatLon]; change LocalPathState.global_path to Path

* fixed None global path handling in LocalPathState

* validated new and updated msg types

* ensure reference_latlon initialized correctly

* converted publishing local path data to a function call in desired heading callback

* Fix if statement syntax?

* Try to fix index out of bounds error

* Try to fix index out of bounds again?

* pass obstacles up to naviagte node

* all but one ompl path test pass

* fix docstring of get waypoints and set global path in tests

* all unit tests pass

* fixed coord sys

* fixed up global path

* few more global path refactors

* local path import changes

* updated local path

* mgp and mais

* refactors done

* fixed little arror with lpath data call back

* changed get_waypoints to get_path

* publish lpathdata

* added runtime warning for inactive subs

* parsing obstacles into ros msgs

* all data publishes

* comment out state validity logging

* type checking for invalid state log

* fixed bad merge errors

* fixed some more duplicate code from merge

* added note about runtime warn form get desired

* replaced default values with valueerrors

* configure ompl path to use real globalpath

* plotted output path

* rm nb

* Minor Syntax changes

---------

Co-authored-by: Jamen Kaye &lt;jamen.kaye@gmail.com&gt;
Co-authored-by: Jamen Kaye &lt;71156197+jamenkaye@users.noreply.github.com&gt;
Co-authored-by: Akshanjay Kompelli &lt;63443017+FireBoyAJ24@users.noreply.github.com&gt;
diff --git a/src/local_pathfinding/local_pathfinding/local_path.py b/src/local_pathfinding/local_pathfinding/local_path.py
@@ -36,36 +36,28 @@ def __init__(
         filtered_wind_sensor: ci.WindSensor,
         planner: str,
     ):
-        if gps:  # TODO: remove when mock can be run
-            self.position = gps.lat_lon
-            self.speed = gps.speed.speed
-            self.heading = gps.heading.heading
-        else:
-            # this position has been verified to be close enough to land that
-            # land obstacles should be generated
-            self.position = ci.HelperLatLon(latitude=49.29, longitude=-126.32)
-            self.speed = 0.0
-            self.heading = 0.0
-
-        if ais_ships:  # TODO: remove when mock can be run
-            self.ais_ships = [ship for ship in ais_ships.ships]
-        else:
-            self.ais_ships = []  # ensures this attribute is always set, to avoid AtributeError
-
-        if filtered_wind_sensor:  # TODO: remove when mock can be run
-            self.wind_speed = filtered_wind_sensor.speed.speed
-            self.wind_direction = filtered_wind_sensor.direction
-        else:
-            self.wind_speed = 0.0
-            self.wind_direction = 0
-
-        self.global_path = global_path
-
-        if self.global_path and self.global_path.waypoints:
-            self.reference_latlon = self.global_path.waypoints[-1]
-        else:
+        if not gps:
+            raise ValueError("gps must not be None")
+        self.position = gps.lat_lon
+        self.speed = gps.speed.speed
+        self.heading = gps.heading.heading
+
+        if not ais_ships:
+            raise ValueError("ais_ships must not be None")
+        self.ais_ships = [ship for ship in ais_ships.ships]
+
+        if not filtered_wind_sensor:
+            raise ValueError("filtered_wind_sensor must not be None")
+        self.wind_speed = filtered_wind_sensor.speed.speed
+        self.wind_direction = filtered_wind_sensor.direction
+
+        if not (global_path and global_path.waypoints):
             raise ValueError("Cannot create a LocalPathState with an empty global_path")
+        self.global_path = global_path
+        self.reference_latlon = self.global_path.waypoints[-1]
 
+        if not planner:
+            raise ValueError("planner must not be None")
         self.planner = planner
 
         # obstacles are initialized by OMPLPath right before solving
@@ -106,6 +98,7 @@ def update_if_needed(
             global_path (ci.Path): Path to the destination.
             filtered_wind_sensor (ci.WindSensor): Wind data.
         """
+        # this raises ValueError if any of the parameters are not properly initialized
         state = LocalPathState(gps, ais_ships, global_path, filtered_wind_sensor, planner)
         self.state = state
         ompl_path = OMPLPath(
diff --git a/src/local_pathfinding/local_pathfinding/node_navigate.py b/src/local_pathfinding/local_pathfinding/node_navigate.py
@@ -115,7 +115,6 @@ def __init__(self):
         self.get_logger().debug(f"Got parameter: {self.planner=}")
 
     # subscriber callbacks
-
     def ais_ships_callback(self, msg: ci.AISShips):
         self.get_logger().debug(f"Received data from {self.ais_ships_sub.topic}: {msg}")
         self.ais_ships = msg
@@ -135,7 +134,6 @@ def filtered_wind_sensor_callback(self, msg: ci.WindSensor):
         self.filtered_wind_sensor = msg
 
     # publisher callbacks
-
     def desired_heading_callback(self):
         """Get and publish the desired heading."""
 
diff --git a/src/local_pathfinding/local_pathfinding/ompl_path.py b/src/local_pathfinding/local_pathfinding/ompl_path.py
@@ -28,6 +28,8 @@
 # OMPL logging: only log warnings and above
 ou.setLogLevel(ou.LOG_WARN)
 
+BOX_BUFFER_SIZE = 1.0  # km
+
 
 class OMPLPath:
     """Represents the general OMPL Path.
@@ -61,7 +63,7 @@ def __init__(
             max_runtime (float): Maximum amount of time in seconds to look for a solution path.
             local_path_state (LocalPathState): State of Sailbot.
         """
-        self._box_buffer = 1
+        self._box_buffer = BOX_BUFFER_SIZE
         self._logger = parent_logger.get_child(name="ompl_path")
         self._simple_setup = self._init_simple_setup(local_path_state)  # this needs state
 
@@ -194,15 +196,10 @@ def _init_simple_setup(self, local_path_state) -> og.SimpleSetup:
         start_x = start_position_in_xy.x
         start_y = start_position_in_xy.y
 
-        # TODO this needs to be removed when mocks are ready
-        if not self.state.global_path:
-            goal_polygon = self.create_buffer_around_position(cs.XY(0, 0))
-            goal_x, goal_y = (0.0, 0.0)
-        else:
-            goal_position = self.state.global_path.waypoints[-1]
-            goal_position_in_xy = cs.latlon_to_xy(self.state.reference_latlon, goal_position)
-            goal_polygon = self.create_buffer_around_position(goal_position_in_xy)
-            goal_x, goal_y = goal_position_in_xy
+        goal_position = self.state.global_path.waypoints[-1]
+        goal_position_in_xy = cs.latlon_to_xy(self.state.reference_latlon, goal_position)
+        goal_polygon = self.create_buffer_around_position(goal_position_in_xy)
+        goal_x, goal_y = goal_position_in_xy
 
         # create an SE2 state space: rotation and translation in a plane
         space = base.SE2StateSpace()
diff --git a/src/local_pathfinding/test/test_local_path.py b/src/local_pathfinding/test/test_local_path.py
@@ -1,9 +1,10 @@
+import pytest
 from custom_interfaces.msg import GPS, AISShips, HelperLatLon, Path, WindSensor
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
-import local_pathfinding.local_path as local_path
+import local_pathfinding.local_path as lp
 
-PATH = local_path.LocalPath(parent_logger=RcutilsLogger())
+PATH = lp.LocalPath(parent_logger=RcutilsLogger())
 
 
 def test_LocalPath_update_if_needed():
@@ -22,3 +23,90 @@ def test_LocalPath_update_if_needed():
     )
     assert PATH.path is not None, "waypoints is not initialized"
     assert len(PATH.path.waypoints) > 1, "waypoints length <= 1"
+
+
+def test_LocalPathState_parameter_checking():
+    with pytest.raises(ValueError):
+        lps = (
+            lp.LocalPathState(
+                gps=None,
+                ais_ships=AISShips(),
+                global_path=Path(
+                    waypoints=[
+                        HelperLatLon(latitude=0.0, longitude=0.0),
+                        HelperLatLon(latitude=1.0, longitude=1.0),
+                    ]
+                ),
+                filtered_wind_sensor=WindSensor(),
+                planner="rrtstar",
+            ),
+        )
+
+    with pytest.raises(ValueError):
+        lps = (
+            lp.LocalPathState(
+                gps=GPS(),
+                ais_ships=None,
+                global_path=Path(
+                    waypoints=[
+                        HelperLatLon(latitude=0.0, longitude=0.0),
+                        HelperLatLon(latitude=1.0, longitude=1.0),
+                    ]
+                ),
+                filtered_wind_sensor=WindSensor(),
+                planner="rrtstar",
+            ),
+        )
+
+    with pytest.raises(ValueError):
+        lps = (
+            lp.LocalPathState(
+                gps=GPS(),
+                ais_ships=AISShips(),
+                global_path=Path(waypoints=[]),
+                filtered_wind_sensor=WindSensor(),
+                planner="rrtstar",
+            ),
+        )
+
+    with pytest.raises(ValueError):
+        lps = (
+            lp.LocalPathState(
+                gps=GPS(),
+                ais_ships=AISShips(),
+                global_path=None,
+                filtered_wind_sensor=WindSensor(),
+                planner="rrtstar",
+            ),
+        )
+
+    with pytest.raises(ValueError):
+        lps = (
+            lp.LocalPathState(
+                gps=GPS(),
+                ais_ships=AISShips(),
+                global_path=Path(
+                    waypoints=[
+                        HelperLatLon(latitude=0.0, longitude=0.0),
+                        HelperLatLon(latitude=1.0, longitude=1.0),
+                    ]
+                ),
+                filtered_wind_sensor=None,
+                planner="rrtstar",
+            ),
+        )
+    with pytest.raises(ValueError):
+        lps = (  # noqa
+            lp.LocalPathState(
+                gps=GPS(),
+                ais_ships=AISShips(),
+                global_path=Path(
+                    waypoints=[
+                        HelperLatLon(latitude=0.0, longitude=0.0),
+                        HelperLatLon(latitude=1.0, longitude=1.0),
+                    ]
+                ),
+                filtered_wind_sensor=WindSensor(),
+                planner=None,
+            ),
+        )
