Raghumanimehta/807-partial:Timeout implementation + cleanup of update_if_needed, LocalPath and NodeNavigate by raghumanimehta · Pull Request #839 · UBCSailbot/sailbot_workspace

raghumanimehta · 2026-03-02T06:34:35Z

Description

Resolves a part of the conditions to switch path issue, not mentioning it here otherwise github would close it.
I had intended the whole conditions to switch paths be one PR but it seems making small incremental changes will allow more people to asynchronously work on this eg. @MilcToast worked on the wind thresholding.
This PR adds a field path_generation_time to LocalPathState to keep a track of when the path was generated.
I also added a function is_path_expired in LocalPath that checks if a path is expired or not using the field mentioned in the previous point.
The funciton has been tested
I also updated the LocalPath's update_if_needed to have a cleaner flow. It still largely follows the old algorithm.
I have updated the documentation in node_navigate.py and local_path.py to reflect that waypoint index is not the targetted waypoint but the waypoint we have already passed. Think of the situation of what happens when OMPL is called. The first item (0 index) is the waypoint at which OMPL was called and the boat is moving away from this waypoint. Following this intuition, the waypoint index is the waypoint that has already been visited. The variable name has been updated to reflect this behaviour.
I also added some error checking to ensure that we never access out of bound access in the waypoints array stored (localPath array from OMPL).

Verification

Unit tests

Resources

The point about timeout in https://ubcsailbot.atlassian.net/wiki/spaces/prjt22/pages/1890025723/Conditions+to+Update+Local+Path

…s_rework

src/local_pathfinding/local_pathfinding/local_path.py

SPDonaghy · 2026-03-05T05:05:36Z

src/local_pathfinding/local_pathfinding/local_path.py

+            prev_lp_wp_index += 1
+
+            if prev_lp_wp_index > len(path.waypoints):
+                raise ValueError("waypoint idx > len(path.waypoints). Must generate new path")


So when could this happen? As it stands, this scenario is not recoverable right? because update_if_needed does not catch this.

Yes, update_if_needed doesn't catch it yet. It will in a separate PR.

When could this happen?
I have noticed it happen when you let PATH run for sometime and you are at the end of local path. There was a mismatch in what docstring and code thought waypoint index was. Read the PR description to learn more. Please let me know if it doesn't make sense.

I have updated a TODO for now to remember to handle this in the future.

I read the PR description, but am still fuzzy on when exactly this can happen and if its expected to be a transient issue that should be caught and handled or if its a sign something is very wrong and we cannot recover/should try rebooting.

This is definitely something that warrants some added tests to verify that this scenario doesn't occur or if it does, that it's handled the right way. I'm wary of this because it doesn't convincingly solve the underlying problem of how this could occur and instead passes the problem to update_if_needed.

What do you mean by that?
I believe the tests you are talking about will come only after we have a finalized update_if_needed. In the meanwhile, we can have unit tests to test that these errors are thrown.

I don't think this issue is a sign that something is gone very wrong. I still believe it was an off by one error.

Please let me know if you have any good ideas on how we can test this (without doing what I suggested in the previous comment).

One more thing, I still believe we should be handling this how I suggested in this PR. I don't feel good about indexing an array without checks (which was the case earlier).

src/local_pathfinding/local_pathfinding/local_path.py

…s_rework

AMaharaj16 · 2026-03-07T21:05:13Z

src/local_pathfinding/local_pathfinding/local_path.py

+            or (self.path is None)
+            or (self.in_collision_zone())
+            or (self.is_path_expired())
+            or (self._prev_lp_wp_index > len(self.path.waypoints))


This last case requires the boat to traverse the waypoint that comes after the last waypoint in self.path.waypoints. Do you think this should be >= instead? That way it reruns ompl when we finish the current local path, or is this handled well enough by received_new_global_waypoint in update_if_needed?

…s_rework Update branch with main

AMaharaj16 · 2026-03-16T04:27:59Z

src/local_pathfinding/test/test_ompl_path.py

+    current_wp_latlon = waypoints[target_wp_index - 1]
+    next_wp_latlon = waypoints[target_wp_index]

    def mid_point(start_latlon: HelperLatLon, end_latlon: HelperLatLon):


Although this is for testing, it isn't immediately clear what this function does. Add a short docstring explaining what this function does.

src/local_pathfinding/local_pathfinding/local_path.py

AMaharaj16 · 2026-03-16T08:21:41Z

src/local_pathfinding/local_pathfinding/local_path.py


-        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)
+        if self.is_path_expired():


Isn't this case already covered in must_change_path() called above?

Yes. fixed.

AMaharaj16 · 2026-03-16T08:25:50Z

src/local_pathfinding/local_pathfinding/local_path.py

+            return (
+                True,
+                f"Target waypoint index out of bounds: {self._target_lp_wp_index} >= {len(self.path.waypoints)}", # noqa
+            )


Should is_significant_wind_change() be added here?

Eventually, yes. This work was done before Jeremy had made the PR for that function. We will add it here in a separate PR.
Think of this PR as a fix of a bug with the waypoint latlon and implementation of a part of the reworked conditions to switch paths.

src/local_pathfinding/local_pathfinding/ompl_path.py

…s_rework

raghumanimehta added 4 commits February 17, 2026 15:49

added unused timer to check for timeuots

bb7ac62

timer logic added

3c1799c

tested path generation timeout logic

b2ea7bf

Merge branch 'main' into raghumanimehta/807_conditions_to_switch_path…

df18ee5

…s_rework

raghumanimehta requested review from AMaharaj16 and SPDonaghy March 2, 2026 06:34

raghumanimehta added the path Pathfinding team label Mar 2, 2026

AMaharaj16 reviewed Mar 2, 2026

View reviewed changes

src/local_pathfinding/local_pathfinding/local_path.py Outdated Show resolved Hide resolved

raghumanimehta added 3 commits March 2, 2026 02:41

big changes

9980e93

tests fixed

fa946ae

linting fix

84835d9

raghumanimehta changed the title ~~Raghumanimehta/807 conditions to switch paths rework~~ Raghumanimehta/807-partial:Timeout implementation + cleanup of update_if_needed, LocalPath and NodeNavigate Mar 2, 2026

raghumanimehta added 4 commits March 2, 2026 13:44

renamed function

666ddc7

added a comment

a6e536f

added second comment

48094f0

test fixed

a50c57a

raghumanimehta requested a review from AMaharaj16 March 2, 2026 22:30

SPDonaghy added path Pathfinding team and removed path Pathfinding team labels Mar 2, 2026

SPDonaghy assigned raghumanimehta Mar 2, 2026

SPDonaghy added this to SOFT Project Mar 2, 2026

github-project-automation bot moved this to Backlog in SOFT Project Mar 2, 2026

SPDonaghy reviewed Mar 5, 2026

View reviewed changes

AMaharaj16 reviewed Mar 5, 2026

View reviewed changes

src/local_pathfinding/local_pathfinding/local_path.py Outdated Show resolved Hide resolved

AMaharaj16 reviewed Mar 5, 2026

View reviewed changes

src/local_pathfinding/local_pathfinding/local_path.py Outdated Show resolved Hide resolved

raghumanimehta added 5 commits March 5, 2026 00:01

docs update

24f08bc

value error -> index error in calculate_desired_heading_and_wp_index

d292bc2

Path in collision zone comment update

bf0ad2d

readibility addressed

67d902b

remved asserts

df3f06f

raghumanimehta and others added 4 commits March 5, 2026 00:25

added comment to remember to handle the error thrown

ea320f8

Merge branch 'main' into raghumanimehta/807_conditions_to_switch_path…

29e5a9a

…s_rework

removed MUST comment

a64bda7

updated docs

84d4bc5

AMaharaj16 reviewed Mar 7, 2026

View reviewed changes

raghumanimehta added 4 commits March 7, 2026 13:22

updated the local waypoint index saga

e23c750

Merge branch 'main' into raghumanimehta/807_conditions_to_switch_path…

7eaec33

…s_rework Update branch with main

simplified logging

5db7d00

docs linter fix

c7b0dbf

raghumanimehta requested review from AMaharaj16 and SPDonaghy March 10, 2026 18:36

AMaharaj16 reviewed Mar 16, 2026

View reviewed changes

src/local_pathfinding/local_pathfinding/local_path.py Outdated Show resolved Hide resolved

AMaharaj16 reviewed Mar 16, 2026

View reviewed changes

src/local_pathfinding/local_pathfinding/ompl_path.py Show resolved Hide resolved

comments

96e6aba

raghumanimehta requested a review from a team as a code owner March 17, 2026 21:43

raghumanimehta and others added 4 commits March 17, 2026 14:52

fixed in_collision_zone

90bd316

removed redundant code

43c81d6

Merge branch 'main' into raghumanimehta/807_conditions_to_switch_path…

845c49d

…s_rework

Merge branch 'main' into raghumanimehta/807_conditions_to_switch_path…

dd1886a

…s_rework

raghumanimehta merged commit a5cb3cc into main Mar 23, 2026
6 checks passed

raghumanimehta deleted the raghumanimehta/807_conditions_to_switch_paths_rework branch March 23, 2026 21:55

github-project-automation bot moved this from Backlog to Done in SOFT Project Mar 23, 2026

Conversation

raghumanimehta commented Mar 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Verification

Resources

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

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!

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!

Uh oh!

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!

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!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

raghumanimehta commented Mar 2, 2026 •

edited

Loading