Merge branch 'main' into Jay/update-codeowner-package-json-fix

ubcsailbotsoftware · web-flow · commit 9004013cf1cf · 2025-07-23T13:05:10.000-07:00
diff --git a/src/local_pathfinding/local_pathfinding/local_path.py b/src/local_pathfinding/local_pathfinding/local_path.py
@@ -22,6 +22,8 @@ class LocalPathState:
         ais_ships (List[HelperAISShip]): Information about nearby ships.
         global_path (List[Tuple[float, float]]): Path to the destination that Sailbot is
                                                  navigating along.
+        target_global_waypoint (ci.HelperLatLon): The global waypoint that we are heading towards.
+            The global waypoint is the same as the reference latlon.
         wind_speed (float): Wind speed.
         wind_direction (int): Wind direction.
         planner (str): Planner to use for the OMPL query.
@@ -34,6 +36,7 @@ def __init__(
         gps: ci.GPS,
         ais_ships: ci.AISShips,
         global_path: ci.Path,
+        target_global_waypoint: ci.HelperLatLon,
         filtered_wind_sensor: ci.WindSensor,
         planner: str,
     ):
@@ -55,7 +58,7 @@ def __init__(
         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]
+        self.reference_latlon = target_global_waypoint
 
         if not planner:
             raise ValueError("planner must not be None")
@@ -87,6 +90,9 @@ def update_if_needed(
         gps: ci.GPS,
         ais_ships: ci.AISShips,
         global_path: ci.Path,
+        received_new_global_waypoint: bool,
+        # ^ Placeholder; will be used for conditions to update local path
+        target_global_waypoint: ci.HelperLatLon,
         filtered_wind_sensor: ci.WindSensor,
         planner: str,
         land_multi_polygon: MultiPolygon = None,
@@ -101,7 +107,9 @@ def update_if_needed(
             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)
+        state = LocalPathState(
+            gps, ais_ships, global_path, target_global_waypoint, filtered_wind_sensor, planner
+        )
         self.state = state
         ompl_path = OMPLPath(
             parent_logger=self._logger,
diff --git a/src/local_pathfinding/local_pathfinding/node_navigate.py b/src/local_pathfinding/local_pathfinding/node_navigate.py
@@ -14,7 +14,10 @@
 import local_pathfinding.obstacles as ob
 from local_pathfinding.local_path import LocalPath
 
-WAYPOINT_REACHED_THRESH_KM = 0.5
+LOCAL_WAYPOINT_REACHED_THRESH_KM = 0.5
+GLOBAL_WAYPOINT_REACHED_THRESH_KM = 3
+PATHFINDING_RANGE_KM = 30
+REALLY_FAR_M = 100000000
 GEODESIC = Geod(ellps="WGS84")
 
 
@@ -119,7 +122,9 @@ def __init__(self):
 
         # attributes
         self.local_path = LocalPath(parent_logger=self.get_logger())
-        self.current_waypoint_index = 0
+        self.local_waypoint_index = 0
+        self.global_waypoint_index = -1
+        self.saved_target_global_waypoint = None
         self.use_mock_land = self.get_parameter("use_mock_land").get_parameter_value().bool_value
         self.mode = self.get_parameter("mode").get_parameter_value().string_value
         self.planner = self.get_parameter("path_planner").get_parameter_value().string_value
@@ -154,6 +159,8 @@ def global_path_callback(self, msg: ci.Path):
             f"Received data from {self.global_path_sub.topic}: {gp.path_to_dict(msg)}"
         )
         self.global_path = msg
+        if self.saved_target_global_waypoint is None:
+            self.saved_target_global_waypoint = self.global_path.waypoints[-1]
 
     def filtered_wind_sensor_callback(self, msg: ci.WindSensor):
         self.get_logger().debug(f"Received data from {self.filtered_wind_sensor_sub.topic}: {msg}")
@@ -251,26 +258,89 @@ def get_desired_heading(self) -> float:
             float: The desired heading
         """
 
-        received_new_path = self.local_path.update_if_needed(
+        # Extra logic for when the global waypoint changes due to receiving a new global path
+        received_new_global_waypoint = False
+        if (
+            self.global_path.waypoints[self.global_waypoint_index]
+            != self.saved_target_global_waypoint
+        ):
+            received_new_global_waypoint = True
+            self.global_waypoint_index = self.determine_start_point_in_new_global_path(
+                self.global_path, self.gps.lat_lon
+            )
+            self.saved_target_global_waypoint = self.global_path.waypoints[
+                self.global_waypoint_index
+            ]
+
+        # Check if we're close enough to the global waypoint to head to the next one
+        _, _, distance_to_waypoint_m = GEODESIC.inv(
+            self.gps.lat_lon.longitude,
+            self.gps.lat_lon.latitude,
+            self.saved_target_global_waypoint.longitude,
+            self.saved_target_global_waypoint.latitude,
+        )
+        if distance_to_waypoint_m < GLOBAL_WAYPOINT_REACHED_THRESH_KM:
+            received_new_global_waypoint = True
+            self.global_waypoint_index -= 1  # Since global waypoints are in reverse order
+
+            # At the end of the global path, alternate between the last two waypoints
+            if self.global_waypoint_index < 0:
+                self.global_waypoint_index = 1
+
+            self.saved_target_global_waypoint = self.global_path.waypoints[
+                self.global_waypoint_index
+            ]
+
+        received_new_local_path = self.local_path.update_if_needed(
             self.gps,
             self.ais_ships,
             self.global_path,
+            received_new_global_waypoint,
+            self.saved_target_global_waypoint,
             self.filtered_wind_sensor,
             self.planner,
             self.land_multi_polygon,
         )
 
-        if received_new_path:
-            self.current_waypoint_index = 0
+        if received_new_local_path:
+            self.local_waypoint_index = 0
 
-        desired_heading, self.current_waypoint_index = (
+        desired_heading, self.local_waypoint_index = (
             self.calculate_desired_heading_and_waypoint_index(
-                self.local_path.path, self.current_waypoint_index, self.gps.lat_lon
+                self.local_path.path, self.local_waypoint_index, self.gps.lat_lon
             )
         )
 
         return desired_heading
 
+    @staticmethod
+    def determine_start_point_in_new_global_path(
+        global_path: ci.Path, boat_lat_lon: ci.HelperLatLon
+    ):
+        """Used when we receive a new global path.
+        Finds the index of the first waypoint within 'pathfinding range' of gps location.
+        If none are found, it returns the index of the nearest waypoint."""
+
+        closest_m, index_of_closest = REALLY_FAR_M, -1
+        for waypoint_index in range(len(global_path.waypoints) - 1, -1, -1):
+            # Note: the global waypoints are in reverse order (index 0 is final waypoint)
+            waypoint = global_path.waypoints[waypoint_index]
+
+            _, _, distance_to_waypoint_m = GEODESIC.inv(
+                boat_lat_lon.longitude,
+                boat_lat_lon.latitude,
+                waypoint.longitude,
+                waypoint.latitude,
+            )
+
+            if distance_to_waypoint_m < closest_m:
+                closest_m, index_of_closest = distance_to_waypoint_m, waypoint_index
+
+            if distance_to_waypoint_m < cs.km_to_meters(PATHFINDING_RANGE_KM):
+                return waypoint_index
+
+        return index_of_closest
+
     @staticmethod
     def calculate_desired_heading_and_waypoint_index(
         path: ci.Path, waypoint_index: int, boat_lat_lon: ci.HelperLatLon
@@ -280,7 +350,7 @@ def calculate_desired_heading_and_waypoint_index(
             boat_lat_lon.longitude, boat_lat_lon.latitude, waypoint.longitude, waypoint.latitude
         )
 
-        if cs.meters_to_km(distance_to_waypoint_m) < WAYPOINT_REACHED_THRESH_KM:
+        if cs.meters_to_km(distance_to_waypoint_m) < LOCAL_WAYPOINT_REACHED_THRESH_KM:
             # If we reached the current local waypoint, aim for the next one
             waypoint_index += 1
             waypoint = path.waypoints[waypoint_index]
@@ -339,3 +409,4 @@ def _log_inactive_subs_warning(self):
 
 if __name__ == "__main__":
     main()
+    main()
diff --git a/src/local_pathfinding/local_pathfinding/ompl_path.py b/src/local_pathfinding/local_pathfinding/ompl_path.py
@@ -225,8 +225,7 @@ def _init_simple_setup(self, local_path_state, land_multi_polygon) -> og.SimpleS
         start_x = start_position_in_xy.x
         start_y = start_position_in_xy.y
 
-        goal_position = self.state.global_path.waypoints[-1]
-        goal_position_in_xy = cs.latlon_to_xy(self.state.reference_latlon, goal_position)
+        goal_position_in_xy = cs.XY(0, 0)  # Global waypoint is used as the reference point
         goal_polygon = self.create_buffer_around_position(goal_position_in_xy)
         goal_x, goal_y = goal_position_in_xy
 
diff --git a/src/local_pathfinding/test/test_local_path.py b/src/local_pathfinding/test/test_local_path.py
@@ -18,6 +18,8 @@ def test_LocalPath_update_if_needed():
                 HelperLatLon(latitude=1.0, longitude=1.0),
             ]
         ),
+        received_new_global_waypoint=False,
+        target_global_waypoint=HelperLatLon(latitude=1.0, longitude=1.0),
         filtered_wind_sensor=WindSensor(),
         planner="rrtstar",
     )
@@ -37,6 +39,7 @@ def test_LocalPathState_parameter_checking():
                         HelperLatLon(latitude=1.0, longitude=1.0),
                     ]
                 ),
+                target_global_waypoint=HelperLatLon(latitude=1.0, longitude=1.0),
                 filtered_wind_sensor=WindSensor(),
                 planner="rrtstar",
             ),
@@ -53,6 +56,7 @@ def test_LocalPathState_parameter_checking():
                         HelperLatLon(latitude=1.0, longitude=1.0),
                     ]
                 ),
+                target_global_waypoint=HelperLatLon(latitude=1.0, longitude=1.0),
                 filtered_wind_sensor=WindSensor(),
                 planner="rrtstar",
             ),
@@ -64,6 +68,7 @@ def test_LocalPathState_parameter_checking():
                 gps=GPS(),
                 ais_ships=AISShips(),
                 global_path=Path(waypoints=[]),
+                target_global_waypoint=HelperLatLon(),
                 filtered_wind_sensor=WindSensor(),
                 planner="rrtstar",
             ),
@@ -75,6 +80,7 @@ def test_LocalPathState_parameter_checking():
                 gps=GPS(),
                 ais_ships=AISShips(),
                 global_path=None,
+                target_global_waypoint=None,
                 filtered_wind_sensor=WindSensor(),
                 planner="rrtstar",
             ),
@@ -91,6 +97,7 @@ def test_LocalPathState_parameter_checking():
                         HelperLatLon(latitude=1.0, longitude=1.0),
                     ]
                 ),
+                target_global_waypoint=HelperLatLon(latitude=1.0, longitude=1.0),
                 filtered_wind_sensor=None,
                 planner="rrtstar",
             ),
@@ -106,6 +113,7 @@ def test_LocalPathState_parameter_checking():
                         HelperLatLon(latitude=1.0, longitude=1.0),
                     ]
                 ),
+                target_global_waypoint=HelperLatLon(latitude=1.0, longitude=1.0),
                 filtered_wind_sensor=WindSensor(),
                 planner=None,
             ),
diff --git a/src/local_pathfinding/test/test_node_navigate.py b/src/local_pathfinding/test/test_node_navigate.py
@@ -3,6 +3,12 @@
 
 import local_pathfinding.node_navigate as nn
 
+LOCAL_WAYPOINT_REACHED_THRESH_KM = 0.5
+GLOBAL_WAYPOINT_REACHED_THRESH_KM = 3
+PATHFINDING_RANGE_KM = 30
+ONE_DEGREE_KM = 111  # One degree longitude at equator = 111km
+PATH_RANGE_DEG = PATHFINDING_RANGE_KM / ONE_DEGREE_KM
+
 
 @pytest.mark.parametrize(
     "path, waypoint_index, boat_lat_lon, correct_heading, new_wp_index",
@@ -55,3 +61,59 @@ def test_calculate_desired_heading_and_waypoint_index(
     )
     assert calculated_answer[0] == pytest.approx(correct_heading, abs=3e-1)
     assert calculated_answer[1] == new_wp_index
+
+
+@pytest.mark.parametrize(
+    "global_path, boat_lat_lon, correct_index",
+    [
+        (
+            ci.Path(waypoints=[ci.HelperLatLon(latitude=0.0, longitude=0.0)]),
+            ci.HelperLatLon(latitude=0.0, longitude=-0.1),
+            0,
+        ),
+        (
+            ci.Path(
+                waypoints=[
+                    ci.HelperLatLon(latitude=0.0, longitude=0.0),
+                    ci.HelperLatLon(latitude=0.0, longitude=1 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=0.0, longitude=2 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=0.0, longitude=3 * PATH_RANGE_DEG),
+                ]
+            ),
+            ci.HelperLatLon(latitude=0.98 * PATH_RANGE_DEG, longitude=2 * PATH_RANGE_DEG),
+            2,
+        ),
+        (
+            ci.Path(
+                waypoints=[
+                    ci.HelperLatLon(latitude=0.0, longitude=0.0),
+                    ci.HelperLatLon(latitude=0.0, longitude=1 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=0.0, longitude=2 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=0.0, longitude=3 * PATH_RANGE_DEG),
+                ]
+            ),
+            ci.HelperLatLon(latitude=1.02 * PATH_RANGE_DEG, longitude=1 * PATH_RANGE_DEG),
+            1,
+        ),
+        (
+            ci.Path(
+                waypoints=[
+                    ci.HelperLatLon(latitude=1.0 * PATH_RANGE_DEG, longitude=1 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=1.0 * PATH_RANGE_DEG, longitude=2 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=0.0, longitude=2 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=0.0, longitude=1 * PATH_RANGE_DEG),
+                    ci.HelperLatLon(latitude=0.0, longitude=0.0),
+                ]
+            ),
+            ci.HelperLatLon(latitude=0.98 * PATH_RANGE_DEG, longitude=1 * PATH_RANGE_DEG),
+            3,
+        ),
+    ],
+)
+def test_find_next_global_waypoint_index(
+    global_path: ci.Path, boat_lat_lon: ci.HelperLatLon, correct_index: int
+):
+    calculated_answer = nn.Sailbot.determine_start_point_in_new_global_path(
+        global_path, boat_lat_lon
+    )
+    assert calculated_answer == correct_index
diff --git a/src/local_pathfinding/test/test_ompl_objectives.py b/src/local_pathfinding/test/test_ompl_objectives.py
@@ -26,6 +26,7 @@
                 HelperLatLon(latitude=1.0, longitude=1.0),
             ]
         ),
+        target_global_waypoint=HelperLatLon(latitude=1.0, longitude=1.0),
         filtered_wind_sensor=WindSensor(),
         planner="rrtstar",
     ),
diff --git a/src/local_pathfinding/test/test_ompl_path.py b/src/local_pathfinding/test/test_ompl_path.py
@@ -30,10 +30,11 @@
         ais_ships=AISShips(),
         global_path=Path(
             waypoints=[
-                HelperLatLon(latitude=0.0, longitude=0.0),
-                HelperLatLon(latitude=1.0, longitude=1.0),
+                HelperLatLon(latitude=1.0, longitude=2.0),
+                HelperLatLon(latitude=3.0, longitude=4.0),
             ]
         ),
+        target_global_waypoint=HelperLatLon(latitude=1.0, longitude=2.0),
         filtered_wind_sensor=WindSensor(),
         planner="rrtstar",
     ),
@@ -50,10 +51,11 @@ def fresh_ompl_path():
             ais_ships=AISShips(),
             global_path=Path(
                 waypoints=[
-                    HelperLatLon(latitude=0.0, longitude=0.0),
-                    HelperLatLon(latitude=1.0, longitude=1.0),
+                    HelperLatLon(latitude=1.0, longitude=2.0),
+                    HelperLatLon(latitude=3.0, longitude=4.0),
                 ]
             ),
+            target_global_waypoint=HelperLatLon(latitude=1.0, longitude=2.0),
             filtered_wind_sensor=WindSensor(),
             planner="rrtstar",
         ),
@@ -124,6 +126,7 @@ def test_init_obstacles():
                 goal_position,
             ]
         ),
+        target_global_waypoint=goal_position,
         filtered_wind_sensor=WindSensor(),
         planner="rrtstar",
     )
@@ -204,6 +207,7 @@ def test_init_obstacles():
                 goal_position,
             ]
         ),
+        target_global_waypoint=HelperLatLon(latitude=0.0, longitude=0.0),
         filtered_wind_sensor=WindSensor(),
         planner="rrtstar",
     )

Original file line number	Diff line number	Diff line change
`@@ -26,6 +26,7 @@`
`26`	`26`	`HelperLatLon(latitude=1.0, longitude=1.0),`
`27`	`27`	`]`
`28`	`28`	`),`
	`29`	`+ target_global_waypoint=HelperLatLon(latitude=1.0, longitude=1.0),`
`29`	`30`	`filtered_wind_sensor=WindSensor(),`
`30`	`31`	`planner="rrtstar",`
`31`	`32`	`),`