Merge branch 'main' into user/adambrett40/601-can-manual-mode

Author: adambrett40

.github/CODEOWNERS

@@ -19,17 +19,17 @@
 /src/controller @eomielan
 
 # diagnostics
-/src/diagnostics @samdai01
+/src/diagnostics @Jng468
 
 # docs
-/docs/ @jamenkaye
+/docs/ @FireBoyAJ24
 
 # local_pathfinding
-/notebooks/local_pathfinding @jamenkaye @FireBoyAJ24 @SPDonaghy
-/src/local_pathfinding @jamenkaye @FireBoyAJ24 @SPDonaghy
+/notebooks/local_pathfinding @FireBoyAJ24 @SPDonaghy
+/src/local_pathfinding @FireBoyAJ24 @SPDonaghy
 
 # network_systems
 /src/network_systems @Jng468 @vaibhavambastha @lross03
 
 # website
-/src/website @JordanChen123
+/src/website @JordanChen123 @fyang151

.gitignore

@@ -32,3 +32,6 @@ venv/
 .cache/
 # Build
 site/
+
+# Website
+**/package-lock.json

scripts/deployment/README.md

@@ -2,6 +2,71 @@
 
 Deploying our software to our autonomous sailboat's main computer.
 
+
+# Deployment on the RaspberryPi and the Remote Server (Digital Ocean Droplet)
+
+Comment out the website's docker compose file in the `devcontainer.json` to prevent the database from running.
+```
+	"dockerComposeFile": [
+		"docker-compose.yml",
+		// "website/docker-compose.website.yml", // website <- COMMENT OUT
+
+		// Uncomment the files containing the programs you need
+		// "docs/docker-compose.docs.yml", // docs
+	],
+```
+
+Comment out the following extensions to prevent the raspberryPi from throttling in `devcontainer.json` as shown below.
+
+```
+			"extensions": [
+				// Comment out the extensions when on the RPI
+				// // template repo
+				// "althack.ament-task-provider",
+				// "DotJoshJohnson.xml",
+				// "ms-azuretools.vscode-docker",
+				// "ms-python.python",
+				// "ms-vscode.cpptools",
+				// "redhat.vscode-yaml",
+				// "smilerobotics.urdf",
+				// "streetsidesoftware.code-spell-checker",
+				// "twxs.cmake",
+				// "ms-python.flake8",
+				// "yzhang.markdown-all-in-one",
+				// // "zachflower.uncrustify",
+
+				// // UBCSailbot
+				// "awesomektvn.toggle-semicolon",
+				// "bierner.github-markdown-preview",
+				// "christian-kohler.path-intellisense",
+				// "cschlosser.doxdocgen",
+				// "DavidAnson.vscode-markdownlint",
+				// "eamodio.gitlens",
+				// // "github.copilot",
+				// "github.vscode-github-actions",
+				// "Gruntfuggly.todo-tree",
+				// "jebbs.plantuml",
+				// "jeff-hykin.better-cpp-syntax",
+				// "KevinRose.vsc-python-indent",
+				// "llvm-vs-code-extensions.vscode-clangd",
+				// "matepek.vscode-catch2-test-adapter",
+				// "mechatroner.rainbow-csv",
+				// "mongodb.mongodb-vscode",
+				// "ms-iot.vscode-ros",
+				// "ms-python.black-formatter",
+				// "ms-python.isort",
+				// "ms-python.vscode-pylance",
+				// "ms-toolsai.jupyter",
+				// "ms-vsliveshare.vsliveshare",
+				// "ms-python.mypy-type-checker",
+				// "njpwerner.autodocstring",
+				// "randomfractalsinc.geo-data-viewer",
+				// "stevejpurves.cucumber",
+				// "streetsidesoftware.code-spell-checker",
+				// "vscode-icons-team.vscode-icons",
+				// "zxh404.vscode-proto3"
+```
+
 ## Scripts
 
 ### `setup_boot.sh`

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,

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()

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
 

src/local_pathfinding/local_pathfinding/visualizer.py

@@ -83,6 +83,14 @@ def __init__(self, msgs: List[ci.LPathData]):
             *[self._split_coordinates(waypoints) for waypoints in self.all_wp_xy]
         )
 
+        # AIS ships
+        self.ais_ships = self.curr_msg.ais_ships.ships
+        ais_ship_latlons = [ship.lat_lon for ship in self.ais_ships]
+        self.ais_ship_ids = [ship.id for ship in self.ais_ships]
+        ais_ship_xy = cs.latlon_list_to_xy_list(self.reference_latlon, ais_ship_latlons)
+        self.ais_pos_x, self.ais_pos_y = self._split_coordinates(ais_ship_xy)
+        self.ais_headings = [ship.cog.heading for ship in self.ais_ships]
+
         # TODO: Include other LPathData attributes for plotting their data
 
         # Process land obstacles
@@ -508,6 +516,35 @@ def live_update_plot(state: VisualizerState) -> go.Figure:
     y_min = min(state.final_local_wp_y) - 10
     y_max = max(state.final_local_wp_y) + 10
 
+    # Display AIS Ships
+    for x_val, y_val, heading, ais_id in zip(
+        state.ais_pos_x, state.ais_pos_y, state.ais_headings, state.ais_ship_ids
+    ):
+        fig.add_trace(
+            go.Scatter(
+                x=[x_val],
+                y=[y_val],
+                mode="markers",
+                name=f"AIS {str(ais_id)}",
+                hovertemplate=(
+                    f"<b>🚢 AIS Ship {str(ais_id)}</b><br>"
+                    f"X: {x_val:.2f}<br>"
+                    f"Y: {y_val:.2f}<br>"
+                    f"Heading: {heading:.1f}°<extra></extra>"
+                ),
+                marker=dict(
+                    symbol="arrow-wide",
+                    line_color="orange",
+                    color="orange",
+                    line_width=2,
+                    size=15,
+                    angleref="up",
+                    angle=cs.true_bearing_to_plotly_cartesian(heading),
+                ),
+                showlegend=False,
+            )
+        )
+
     # Update Layout
     fig.update_layout(
         title="Path Planning",

src/local_pathfinding/mock_ais_files/ais_ships.csv

@@ -1,6 +1,6 @@
 id,latitude,longitude,heading,speed,rot,width,height
-1,45.0,0.0,90.0,1.0,1,1.0,1.0
-2,45.0,2.0,90.0,15.0,2,2.0,2.0
-3,45.0,3.0,3.0,3.0,3,3.0,3.0
-4,45.0,4.0,4.0,4.0,4,4.0,4.0
-5,45.0,5.0,5.0,5.0,5,5.0,5.0
+1,49.01261318191024,-135.65593922412154,90.0,40.0,30,1.0,1.0
+2,48.67919212345747,-132.94724986187234,90.0,40.0,50,2.0,2.0
+3,48.11352031871804,-135.67990496461405,30,80.0,-50,3,3.0
+4,48.1970229368345,-128.93710143805356,4.0,20.0,-30,4.0,4.0
+5,48.51027819947852,-126.15925374786544,5.0,10.0,20,5.0,5.0