Fix #765: Add error handling for OMPL path hard crashes and stop sign…

src/custom_interfaces/README.md

@@ -34,6 +34,39 @@ Update diagram by editing diagrams/src/external_interfaces.puml and the PlantUML
 | HelperROT           | HelperAISShip                      |
 | HelperSpeed         | GPS, HelperAISShip, WindSensor     |
 
+## Safety and Error Handling
+
+### DesiredHeading Message - Stop Logic
+
+The `DesiredHeading` message includes a `sail` boolean field that controls whether the boat should proceed or stop:
+
+- **`sail = True`**: Boat should proceed with the specified heading (normal operation)
+- **`sail = False`**: Boat should stop/not sail (pathfinding failure or safety condition)
+
+#### When `sail` is set to `False`
+
+The local pathfinding node sets `sail = False` when:
+
+- OMPL path planning fails to find a valid solution
+- An exception occurs during path retrieval (e.g., invalid state space)
+- No safe path can be computed due to obstacles or constraints
+
+When `sail = False`:
+
+- The `heading.heading` field is set to `0.0`
+- The `steering` field is set to `0`
+- This signals a stop condition to the boat indicating pathfinding failure.
+
+#### Error Handling Implementation
+
+The pathfinding system uses try-except blocks to gracefully handle OMPL exceptions:
+
+1. **`ompl_path.get_path()`**: Catches exceptions when retrieving waypoints from OMPL solution paths
+2. **`local_path.update_if_needed()`**: Returns `(None, waypoint_index)` when path retrieval fails
+3. **`node_navigate.desired_heading_callback()`**: Detects `None` heading and publishes `DesiredHeading` with `sail = False`
+
+This ensures the boat receives an explicit stop command rather than crashing when pathfinding fails.
+
 ## Boat Simulator Interfaces
 
 ROS messages and services used in our [boat simulator](https://github.com/UBCSailbot/sailbot_workspace/tree/main/src/boat_simulator).

src/custom_interfaces/msg/DesiredHeading.msg

@@ -1,2 +1,3 @@
 HelperHeading heading
 uint8 steering
+bool sail

src/local_pathfinding/local_pathfinding/local_path.py

@@ -212,9 +212,11 @@ def update_if_needed(
         )
         old_ompl_path = self._ompl_path
 
-        heading_new_path, wp_index = self.calculate_desired_heading_and_waypoint_index(
-            ompl_path.get_path(), 0, gps.lat_lon
+        heading_new_path, wp_index = self._try_get_heading(
+            ompl_path, 0, gps.lat_lon, "new"
         )
+        if heading_new_path is None:
+            return None, None
 
         if received_new_global_waypoint:
             self._logger.debug("Updating local path because we have a new global waypoint")
@@ -227,9 +229,11 @@ def update_if_needed(
             self._update(ompl_path)
             return heading_new_path, wp_index
 
-        heading_old_path, updated_wp_index = self.calculate_desired_heading_and_waypoint_index(
-            old_ompl_path.get_path(), local_waypoint_index, gps.lat_lon
+        heading_old_path, updated_wp_index = self._try_get_heading(
+            old_ompl_path, local_waypoint_index, gps.lat_lon, "old"
         )
+        if heading_old_path is None:
+            return None, None
         # check if the current path goes through a collision zone.
         # No need to check for new path since it's fresh and ompl doesn't generate path that
         # go through a collision zone
@@ -243,6 +247,8 @@ def update_if_needed(
         heading_diff_old_path = cs.calculate_heading_diff(self.state.heading, heading_old_path)
         heading_diff_new_path = cs.calculate_heading_diff(self.state.heading, heading_new_path)
 
+        assert updated_wp_index is not None
+        assert wp_index is not None
         old_cost = old_ompl_path.get_cost(updated_wp_index)
         new_cost = ompl_path.get_cost(wp_index)
 
@@ -281,9 +287,44 @@ def update_if_needed(
             self._logger.debug(
                 "old path is cheaper, continuing on the same path"
             )
-            return heading_old_path, wp_index
+            return heading_old_path, updated_wp_index
+
+    def _try_get_heading(
+        self,
+        ompl_path: OMPLPath,
+        waypoint_index: int,
+        boat_lat_lon: ci.HelperLatLon,
+        path_label: str,
+    ) -> tuple[Optional[float], Optional[int]]:
+        """Try to get path and calculate heading from an OMPLPath.
+
+        Args:
+            ompl_path (OMPLPath): The OMPL path to extract waypoints from.
+            waypoint_index (int): The starting waypoint index.
+            boat_lat_lon (ci.HelperLatLon): Current boat position.
+            path_label (str): Label for log messages (e.g. "new" or "old").
+
+        Returns:
+            tuple[Optional[float], Optional[int]]: (heading, waypoint_index) or
+                (None, None) on failure.
+        """
+        try:
+            path = ompl_path.get_path()
+            if path is None:
+                self._logger.error(f"{path_label} OMPL path is None")
+                return None, None
+            heading, wp_index = self.calculate_desired_heading_and_waypoint_index(
+                path, waypoint_index, boat_lat_lon
+            )
+            return heading, wp_index
+        except Exception as e:
+            self._logger.error(f"Failed to get {path_label} OMPL path: {e}")
+            return None, None
 
     def _update(self, ompl_path: OMPLPath):
 
         self._ompl_path = ompl_path
-        self.path = self._ompl_path.get_path()
+        local_path = self._ompl_path.get_path()
+        if local_path is None:
+            self._logger.error("OMPL path is None, cannot update local path")
+        self.path = local_path

src/local_pathfinding/local_pathfinding/node_navigate.py

@@ -180,7 +180,16 @@ def desired_heading_callback(self):
 
         desired_heading = self.get_desired_heading()
         msg = ci.DesiredHeading()
-        msg.heading.heading = desired_heading
+
+        # If desired_heading is None, pathfinding failed - signal boat to not sail
+        if desired_heading is None:
+            self.get_logger().error("Pathfinding failed, signaling boat to stop")
+            msg.heading.heading = 0.0
+            msg.sail = False
+        else:
+            msg.heading.heading = desired_heading
+            msg.sail = True
+
         if self.desired_heading is None or desired_heading != self.desired_heading.heading.heading:
             self.get_logger().info(f"Updating desired heading to: {msg.heading.heading:.2f}")
 
@@ -292,7 +301,7 @@ def get_desired_heading(self) -> float:
                 self.global_waypoint_index
             ]
 
-        desired_heading, self.local_waypoint_index = self.local_path.update_if_needed(
+        desired_heading, new_wp_index = self.local_path.update_if_needed(
             self.gps,
             self.ais_ships,
             self.global_path,
@@ -303,6 +312,8 @@ def get_desired_heading(self) -> float:
             self.planner,
             self.land_multi_polygon,
         )
+        if new_wp_index is not None:
+            self.local_waypoint_index = new_wp_index
         return desired_heading
 
     @staticmethod

src/local_pathfinding/local_pathfinding/ompl_path.py

@@ -10,7 +10,7 @@
 
 import os
 import pickle
-from typing import TYPE_CHECKING, Any, Union
+from typing import TYPE_CHECKING, Any, Optional, Union
 
 import custom_interfaces.msg as ci
 from ompl import base
@@ -206,7 +206,7 @@ def get_cost(self, waypoint_index: int) -> float:
         cost = solution_path.cost(obj)
         return cost.value()
 
-    def get_path(self) -> ci.Path:
+    def get_path(self) -> Optional[ci.Path]:
         """Get the collection of waypoints for the boat to follow.
 
         Returns:
@@ -219,19 +219,23 @@ def get_path(self) -> ci.Path:
             self._logger.warning("Trying to get the waypoints of an unsolved OMPLPath")
             return ci.Path()
 
-        solution_path = self._simple_setup.getSolutionPath()
-        waypoints = []
-
-        for state in solution_path.getStates():
-            waypoint_XY = cs.XY(state.getX(), state.getY())
-            waypoint_latlon = cs.xy_to_latlon(self.state.reference_latlon, waypoint_XY)
-            waypoints.append(
-                ci.HelperLatLon(
-                    latitude=waypoint_latlon.latitude, longitude=waypoint_latlon.longitude
+        try:
+            solution_path = self._simple_setup.getSolutionPath()
+            waypoints = []
+
+            for state in solution_path.getStates():
+                waypoint_XY = cs.XY(state.getX(), state.getY())
+                waypoint_latlon = cs.xy_to_latlon(self.state.reference_latlon, waypoint_XY)
+                waypoints.append(
+                    ci.HelperLatLon(
+                        latitude=waypoint_latlon.latitude, longitude=waypoint_latlon.longitude
+                    )
                 )
-            )
 
-        return ci.Path(waypoints=waypoints)
+            return ci.Path(waypoints=waypoints)
+        except Exception as e:
+            self._logger.error(f"Exception occurred while getting path from OMPL: {e}")
+            return None
 
     def update_objectives(self):
         """Update the objectives on the basis of which the path is optimized.