Fix #805: first working draft by jb3982 · Pull Request #871 · UBCSailbot/sailbot_workspace

jb3982 · 2026-04-01T01:12:16Z

Description

Resolves PATH/SIM Integrating #805

This PR adds SIM + PATH integration for end-to-end testing.

Main changes:

Added sim mode to global launch to run the integrated stack.
Centralized cross-package coordination in global launch (package launch files remain scoped).
Updated PATH launch for sim mode to run only PATH-specific simulation nodes.
Fixed desired_heading instability in PATH:
- Root cause: stale global_waypoint_index relative to updated global_path.waypoints
- Added index validation and re-sync logic
- Prevented crash path that caused publisher count to drop to 0
Updated PATH visualizer payload in sim mode (publishes full LPathData instead of minimal production payload).
Enabled mock land injection for PATH testing in sim mode.
Added/updated NET sim-mode handling (SYSTEM_MODE::SIM, transceiver mode branches).

Verification

Launched integrated stack using global launch:
- ros2 launch $ROS_WORKSPACE/src/global_launch/main_launch.py mode:=sim
Confirmed desired_heading continues publishing (publisher count no longer drops to 0 after a few iterations).
Confirmed navigate no longer crashes from global-waypoint index mismatch.
Confirmed PATH visualizer receives data and renders path output in sim mode.
Confirmed simmode NET nodes initialize with sim-compatible mode handling.
Run extended-duration test (>= 15 min) to monitor waypoint fluctuation and stability over time.
Manually compare calculated values against visualizer output to validate consistency.

Resources

Issue: PATH/SIM Integrating #805

raghumanimehta

Please remove all the unnecessary comments.

I have not looked through the entire PR. Use comments only to explain something non-obvious.

raghumanimehta · 2026-04-01T01:14:58Z

 @dataclass
 class LowLevelControlSubscriptionTopics:
-    GPS: str = "mock_gps"
+    GPS: str = "gps"  # added for #805


Please remove these comments. Why were they added in the first place?

These comments were temporary markers that I added to track modified lines while working across multiple large files (made it easier for me to look for the edits I made). They weren’t intended to be part of the final PR. I’ll clean them up in the next commit.

Git is used prceisely for tracking diffs.

raghumanimehta · 2026-04-01T01:15:07Z

 @dataclass
 class PhysicsEnginePublisherTopics:
-    GPS: str = "mock_gps"
+    GPS: str = "gps"  # added for #805


comment violation.

raghumanimehta · 2026-04-01T01:15:31Z

        """
-        rel_wind_vel = glo_wind_vel[:2] - self.global_velocity[:2]
-        rel_water_vel = glo_water_vel[:2] - self.global_velocity[:2]  # slice into 2d vector
+        rel_wind_vel = glo_wind_vel - self.global_velocity[:2]


What was the bug here? Why was this changed?

I believe the glo_wind_vel vector is already 2D; don't really think it's a bug but I also don't think it does anything

This change was made while getting the simulator running and applying the required adjustments to ensure SIM could run correctly.

The current code works for me -- was something not working?

raghumanimehta · 2026-04-01T01:15:47Z

@@ -1,2 +1,3 @@
 HelperHeading heading
 uint8 steering
+bool sail  # for issue# 805


comment violation? Why was this added here?

This field was added to resolve a mismatch in the DesiredHeading topic that was preventing the system from running correctly (PATH was pausing due to inconsistent fields). So, a temporary sail field with a default True value was introduced to allow the system to run as a stub. The inline comment was only meant as a temporary note to indicate that this change was made for issue #805. I’ll remove that.

raghumanimehta · 2026-04-01T01:16:30Z

-        choices=["production", "development"],
-        description="System mode. Decides whether the system is ran with development or production"
-        + " interfaces",
+        choices=["production", "development", "sim"],  # added "sim" for issue#805


Please remove these comments "for issue ..".

and what is sim mode for, why not reuse development

I’ll remove the “for issue #805” and other similar comments in the next commit.
And I’ve addressed the reasoning for “sim” mode in the general comment's reply below.

SPDonaghy · 2026-04-02T06:48:24Z

Hey just getting into this now. I like the intent of making a lot of solid improvements, but I think its a few too many ideas for one PR and this probably should have been split into 2-3 separate PRs because it's faster to review/approve them when each one is doing less.

Also, wondering why we need a sim mode if the development mode already ran the system with mocks?

SPDonaghy · 2026-04-02T06:51:26Z

-        choices=["production", "development"],
-        description="System mode. Decides whether the system is ran with development or production"
-        + " interfaces",
+        choices=["production", "development", "sim"],  # added "sim" for issue#805


and what is sim mode for, why not reuse development

SPDonaghy · 2026-04-02T06:52:33Z

    mode = LaunchConfiguration("mode").perform(context)

-    launch_description_entities = []
+    launch_description_entities: List[Node] = []


we don't typically use type hints for lvalues like this in the codebase. Did you feel it was necessary?

This wasn’t necessary, I added it while resolving a type-related error during my earlier changes, but it’s no longer needed after re-structuring the launch logic. And I missed reverting it before submitting the PR. I’ll remove it.

SPDonaghy · 2026-04-02T06:55:16Z

        launch_description_entities.append(get_mock_gps_node_description(context))
        launch_description_entities.append(get_navigate_observer_node_description(context))
+    elif mode == "sim":
+        # Mock nodes for global_path and ais_ships.


I think both of these comments can be removed because the first one is evident from the lines of code below and the second one should be known to the reader, or if not known, this doesn't really feel like the appropriate spot to teach the reader that.

These comments were temporary notes I added while working through the changes to keep track of my progress. I’ll remove them.

SPDonaghy · 2026-04-02T07:01:01Z

        self.global_path = msg
-        if self.saved_target_global_waypoint is None:
-            self.saved_target_global_waypoint = self.global_path.waypoints[-1]
+        if not self._has_valid_global_path():


I think there's enough going on here with the path and index stuff that is not relevant to integrating pathfinding with simulator that I suggest this all be moved to a separate PR that also includes new unit tests to assert that the bug these changes are attempting to remove is actually fixed.

This change was tied to the issue where only the desired_heading topic was not being published. The root cause was that the size of the global_path waypoints list was not stable across updates.
Since the path is regenerated based on GPS updates and interpolation, the number of waypoints can vary between callbacks (due to distance approximation and recomputation). We were implicitly assuming stable indexing, which led to invalid indices and caused node_navigate to stop progressing, preventing desired_heading from being published.
These changes were made so that waypoint indexing and target selection remain consistent even when the waypoint list size changes, allowing desired_heading to be published reliably.
I agree this is a broader issue within the SIM–PATH integration and should be backed by unit tests. I’ve verified the fix locally with consistent publishing, and I’ll run longer tests to further validate the behaviour.

SPDonaghy · 2026-04-02T07:05:58Z

-        self.queue.put(vz.VisualizerState(msgs=self.msgs))
-        self.get_logger().info(f"sent new visualizer state with {len(self.msgs)} messages.")
-        self.get_logger().info(f"queue: {self.queue.qsize()}")
+        try:


Did you experience an exception occur at some point from here? I hadn't heard of this happening before.

I did encounter error around this path while debugging the issue with desired_heading not being published, so I added the try-except temporarily to guard against failures from self.queue.put(...), if any, during testing.
Since the root issue was elsewhere, this is no longer needed and I’ll have it removed.

SPDonaghy · 2026-04-02T07:07:27Z

 {
 static const std::string PROD     = "production";
 static const std::string DEV      = "development";
+static const std::string SIM      = "sim";  //  added for issue#805


SPDonaghy · 2026-04-02T07:07:35Z

                    RCLCPP_ERROR(this->get_logger(), "%s", err.what());
                    throw err;
                }
+            } else if (mode == SYSTEM_MODE::SIM) { // added for issue#805


SPDonaghy · 2026-04-02T07:08:51Z

-              ros_topics::DESIRED_HEADING, QUEUE_SIZE,
-              [this](msg::DesiredHeading desired_heading_) { subDesiredHeadingCb(desired_heading_); });
-
+            //  edited for issue#805


SPDonaghy · 2026-04-02T07:09:20Z

+                    ros_topics::DESIRED_HEADING, QUEUE_SIZE,
+                    [this](msg::DesiredHeading desired_heading_) { subDesiredHeadingCb(desired_heading_); });
+            }
+            // till here, if statement


sorry, not sure what this means

That was a temporary marker I added while restructuring this block. I’ll have this and other redundant comments removed in the next commit.

SPDonaghy · 2026-04-02T07:09:50Z

                    publishBoatSimInput(boat_sim_input_msg_);
                    // Add any other necessary looping callbacks
                });
+            } else if (mode == SYSTEM_MODE::SIM) { //  added forissue#805


jb3982 · 2026-04-03T05:41:40Z

Hey just getting into this now. I like the intent of making a lot of solid improvements, but I think its a few too many ideas for one PR and this probably should have been split into 2-3 separate PRs because it's faster to review/approve them when each one is doing less.

Also, wondering why we need a sim mode if the development mode already ran the system with mocks?

First of all thanks for the feedback. I agree this PR is larger than ideal.
I initially planned to split out the SIM/NET-related changes, but since I had a working integrated version and the deadline was close, I pushed it as a single PR. I’ll keep PRs more focused going forward.

Regarding sim mode:
My goal was to get the whole system running (PATH, SIM, CTRL, NET) through a single launch command. Earlier this required manually starting each module in separate terminals, which made iteration and restarts a hassel.
In this setup, PATH runs a subset of nodes (mock global path, mock AIS and navigate observer) while receiving data from SIM instead of the usual development mode inputs.
Initially this logic was placed inside PATH’s main_launch.py, but that meant we were launching the whole system from a sub module, which looked like a bad design choice. So, I moved it to the global launch.
The sim mode was introduced to keep this behavior separate from development, since development mode assumes a different set of node configuration.

SPDonaghy · 2026-04-04T17:50:13Z

Thanks for the response @jb3982 let me know when this is ready for another pass. Make sure to resolve any conversations if you have addressed the feedback in them.

Can you please separate the logic for global path waypoint tracking into a separate pr? Thanks

desired_heading fix changes will be handled in a separate PR

jb3982 requested review from a team, FireBoyAJ24, Jng468, SPDonaghy, alberto-escobar, lross03, raghumanimehta and stevenxu27 as code owners April 1, 2026 01:12

raghumanimehta requested changes Apr 1, 2026

View reviewed changes

SPDonaghy reviewed Apr 2, 2026

View reviewed changes

AMaharaj16 added sim Boat Simulator team path Pathfinding team labels Apr 3, 2026

“jb3982” added 2 commits April 7, 2026 18:03

Fix #805: first working draft

598e801

Cleaned up comments.

4d79870

desired_heading fix changes will be handled in a separate PR

jb3982 force-pushed the jb3982/#805-Integrating_SIM_and_PATH branch from 9d23058 to 4d79870 Compare April 8, 2026 05:40

Conversation

jb3982 commented Apr 1, 2026 • edited by bubbles5228 Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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!

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!

Choose a reason for hiding this comment

Uh oh!

SPDonaghy commented Apr 2, 2026

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!

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!

jb3982 commented Apr 3, 2026

Uh oh!

SPDonaghy commented Apr 4, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

jb3982 commented Apr 1, 2026 •

edited by bubbles5228

Loading