Fix #765: Add error handling for OMPL path hard crashes and stop sign… by jb3982 · Pull Request #780 · UBCSailbot/sailbot_workspace

jb3982 · 2026-02-03T09:03:29Z

Description

Resolves PATH: Stoping the boat #765

This PR addresses the issue where the pathfinding node would crash when OMPL fails to compute a valid state space solution. The changes introduce error handling and a new stop signaling mechanism.

Changes made:

Added sail boolean field to DesiredHeading message to signal when the boat should stop
Implemented try-except blocks in ompl_path.get_path() to catch OMPL exceptions
Added error handling in local_path.update_if_needed() for both new and old path retrievals
Updated node_navigate.desired_heading_callback() to detect pathfinding failures and set sail=False
Documented the new sail field behavior and error handling in custom_interfaces README.md

Behavior:

When pathfinding fails, DesiredHeading is published with sail=False, heading=0.0, and steering=0
This prevents hard crashes and provides an explicit stop signal to downstream nodes

Verification

Created temporary error handling test file (test_error_handling.py) locally and verified all tests pass
- Tests verify exceptions are caught when get_path() fails
- Tests confirm update_if_needed() returns (None, waypoint_index) on failure
- Tests validate desired_heading_callback() sets sail=False when heading is None

Resources

Controller logic for stop state #761

…al logic

raghumanimehta

Good job! Need a couple of changes before I approve this!

You don't need to set any other field except sail in case of failure. Just set the said flag. The other teams handle the rest.

raghumanimehta · 2026-02-03T09:33:58Z

src/local_pathfinding/local_pathfinding/ompl_path.py

Change the return type to Optional[ci.Path]. Handle this none return everywhere.

raghumanimehta · 2026-02-03T09:34:43Z

src/local_pathfinding/local_pathfinding/ompl_path.py

+            return ci.Path(waypoints=waypoints)
+        except Exception as e:
+            self._logger.error(f"Exception occurred while getting path from OMPL: {e}")
+            return ci.Path()


return None here. It makes little sense that empty path is bad.

raghumanimehta · 2026-02-03T09:36:42Z

src/local_pathfinding/local_pathfinding/node_navigate.py

+            msg.sail = False
+        else:
+            msg.heading.heading = desired_heading
+            msg.steering = 0


Why's the steering 0 here?

Also, I don't think we change steering. Confirm this, please.

jb3982 · 2026-02-04T08:05:19Z

In the function update_if_needed, the old path case returns wp_index but we calculate updated_wp_index for the old path. so, I think its a bug or maybe I'm wrong. Can you please clarify on this.
I've attached an image of it below:

raghumanimehta · 2026-02-04T21:12:07Z

In the function update_if_needed, the old path case returns wp_index but we calculate updated_wp_index for the old path. so, I think its a bug or maybe I'm wrong. Can you please clarify on this. I've attached an image of it below:

Yeah, should be updated_wp_index. Thanks for catching that. Please make this chnage in the code.

raghumanimehta

There are other things that are missing still. Consider the scenarios:

What happens when the boat was set tosail == False, when would this be next checked? Have you tested this?
What happens if the OMPL failed becuase of other reasons? I believe we could handle this by after the sail = False, try again immediately a couple of times. If we can't get a path, wait for, let's say, 10 minutes before attempting to generate a new path.
What are your thoughts on these points? @jb3982 @SPDonaghy @FireBoyAJ24

raghumanimehta · 2026-02-04T21:18:30Z

src/local_pathfinding/local_pathfinding/local_path.py

+            )
+        except Exception as e:
+            self._logger.error(f"Failed to get new OMPL path: {e}")
+            return None, local_waypoint_index


Should perhaps return None for both.

raghumanimehta · 2026-02-04T21:18:41Z

src/local_pathfinding/local_pathfinding/local_path.py

+            )
+        except Exception as e:
+            self._logger.error(f"Failed to get old OMPL path: {e}")
+            return None, local_waypoint_index


Same as above comment.

Also you could perhaps abstract this to a function. DRY principle

raghumanimehta · 2026-02-04T21:20:52Z

src/local_pathfinding/local_pathfinding/local_path.py

        self._ompl_path = ompl_path
-        self.path = self._ompl_path.get_path()
+        local_path = self._ompl_path.get_path()
+        if local_path is None:


You should update self.path still. You have logged the error but in the next cycle, the reference to self.path would have a valid path which is incorrect behaviour.

self.path = local_path runs after the if block, since there is no return statement. So, won't it set the self.path to None and in this would trigger if old_ompl_path is None or self.path is None: check. where it would call self._update(omple_path)

src/local_pathfinding/local_pathfinding/node_navigate.py

jb3982 · 2026-02-12T06:40:41Z

There are other things that are missing still. Consider the scenarios:

What happens when the boat was set tosail == False, when would this be next checked? Have you tested this?

What happens if the OMPL failed becuase of other reasons? I believe we could handle this by after the sail = False, try again immediately a couple of times. If we can't get a path, wait for, let's say, 10 minutes before attempting to generate a new path.
What are your thoughts on these points? @jb3982 @SPDonaghy @FireBoyAJ24

The current flow is like this:
- desired_heading_callback runs and callsget_desired_heading, which then callsupdate_if_needed
- If OMPL fails => desidered_heading = None => sets sail = False
- On next tick, desired_heading_callback runs again => If OMPL succeeds in finding path => publishes sail = True
  So the boat's sailing state is re-evaluated every cycle automatically.
Just to confirm I understand correctly: you're proposing something like:
1. On OMPL failure, immediately retry 2-3 times
2. If all retries fail, set sail = False and enter a cooldown period (~10 min)
3. After the cooldown, resume normal pathfinding attempts

I think this is a good idea. Could we use a shorter cooldown than 10 minutes though? Short enough so that our boat isn't drifting aimlessly for too long. What do you think is reasonable?

raghumanimehta · 2026-02-12T07:07:39Z

There are other things that are missing still. Consider the scenarios:

What happens when the boat was set tosail == False, when would this be next checked? Have you tested this?

What happens if the OMPL failed becuase of other reasons? I believe we could handle this by after the sail = False, try again immediately a couple of times. If we can't get a path, wait for, let's say, 10 minutes before attempting to generate a new path.
What are your thoughts on these points? @jb3982 @SPDonaghy @FireBoyAJ24

The current flow is like this:

desired_heading_callback runs and callsget_desired_heading, which then callsupdate_if_needed

If OMPL fails => desidered_heading = None => sets sail = False

On next tick, desired_heading_callback runs again => If OMPL succeeds in finding path => publishes sail = True
So the boat's sailing state is re-evaluated every cycle automatically.

Just to confirm I understand correctly: you're proposing something like:

On OMPL failure, immediately retry 2-3 times

If all retries fail, set sail = False and enter a cooldown period (~10 min)

After the cooldown, resume normal pathfinding attempts

I think this is a good idea. Could we use a shorter cooldown than 10 minutes though? Short enough so that our boat isn't drifting aimlessly for too long. What do you think is reasonable?

Yes. No, just set it to 10 minutes for now. We can discuss this later.

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

d8599f9

…al logic

jb3982 requested review from a team and raghumanimehta as code owners February 3, 2026 09:03

raghumanimehta requested changes Feb 3, 2026

View reviewed changes

raghumanimehta requested review from Jng468, SPDonaghy, alberto-escobar, lross03 and raghumanimehta February 3, 2026 09:38

jb3982 added the path Pathfinding team label Feb 3, 2026

Made the asked changes

8123036

raghumanimehta requested changes Feb 4, 2026

View reviewed changes

Merge branch 'main' into jb3982/765-Fixing_Pathfinding_error_handling

3796cea

Made some minor changes in local_path.py

f86fa9b

Merge branch 'main' into jb3982/765-Fixing_Pathfinding_error_handling

530fefa

Comments

Conversation

jb3982 commented Feb 3, 2026

Description

Verification

Resources

Uh oh!

raghumanimehta left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

jb3982 commented Feb 4, 2026

Uh oh!

raghumanimehta commented Feb 4, 2026

Uh oh!

raghumanimehta left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

jb3982 commented Feb 12, 2026

Uh oh!

raghumanimehta commented Feb 12, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants