merge main into user/remote_trans branch

.devcontainer/Dockerfile

@@ -1,4 +1,4 @@
-FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:moved-network-deps
+FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:add-pybind11
 
 # Copy configuration files (e.g., .vimrc) from config/ to the container's home directory
 ARG USERNAME=ros

docs/current/sailbot_workspace/usage/setup.md

@@ -21,7 +21,7 @@ allowing it to be installed on Windows and macOS in addition to Linux.
 
 [^1]: [Wikipedia Docker page](https://en.wikipedia.org/wiki/Docker_(software)){target=_blank}
 [^2]: [Get Docker](https://docs.docker.com/get-docker/){target=_blank}
-[^3]: [What is the difference between Docker Desktop for Linux and Docker Engine](https://docs.docker.com/desktop/faqs/linuxfaqs/#what-is-the-difference-between-docker-desktop-for-linux-and-docker-engine){target=_blank}
+[^3]: [What is the difference between Docker Desktop for Linux and Docker Engine](https://www.docker.com/blog/how-to-check-docker-version){target=_blank}
 
 === ":material-microsoft-windows: Windows"
 

scripts/README.md

@@ -67,7 +67,7 @@ Script to setup, build, and run all ROS packages.
 
 ## `setup.sh`
 
-Script to handle ROS setup.
+Script to handle ROS setup and to further update /home/ros/.bashrc.
 
 ## `test.sh`
 

scripts/build.sh

@@ -41,3 +41,8 @@ colcon build \
         --merge-install \
         --symlink-install \
         --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DSTATIC_ANALYSIS=$STATIC_ANALYSIS" "-DUNIT_TEST=$UNIT_TEST" "--no-warn-unused-cli"
+
+if [[ "$PACKAGE" == "local_pathfinding" || "$PACKAGE" == "" ]]; then
+    echo "Building ompl python bindings..."
+    /workspaces/sailbot_workspace/src/local_pathfinding/src/build/py_bindings.sh
+fi

scripts/setup.sh

@@ -21,3 +21,10 @@ if wget -q --spider --timeout=1 http://google.com; then
     rosdep update --rosdistro $ROS_DISTRO
 fi
 rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO
+
+echo "" >> $HOME/.bashrc
+echo "# set up links  for ompl python bindings" >> $HOME/.bashrc
+echo "sudo ln -sf /usr/share/libompl.so /usr/lib/libompl.so" >> $HOME/.bashrc
+echo "export LD_LIBRARY_PATH=/usr/share:$LD_LIBRARY_PATH" >> $HOME/.bashrc
+
+source $HOME/.bashrc

scripts/test.sh

@@ -1,5 +1,7 @@
 #!/bin/bash
 set -e
+export LD_LIBRARY_PATH=/usr/share:$LD_LIBRARY_PATH
+echo "LD_LIBRARY_PATH: $LD_LIBRARY_PATH"
 
 function signal_handler() {
     if [ -n "$(pgrep -f virtual_iridium)" ]; then

src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py

@@ -33,6 +33,7 @@
 
 import boat_simulator.common.constants as Constants
 from boat_simulator.common.generators import MVGaussianGenerator
+from boat_simulator.common.sensors import SimWindSensor
 from boat_simulator.common.types import Scalar
 from boat_simulator.nodes.physics_engine.fluid_generation import FluidGenerator
 from boat_simulator.nodes.physics_engine.model import BoatState
@@ -182,6 +183,10 @@ def __init_private_attributes(self):
             generator=MVGaussianGenerator(current_mean, current_cov)
         )
 
+        # No delay in this instance
+        sim_wind = self.__wind_generator.next()
+        self.__sim_wind_sensor = SimWindSensor(sim_wind, enable_noise=True)
+
     def __init_callback_groups(self):
         """Initializes the callback groups. Whether multithreading is enabled or not will affect
         how callbacks are executed.
@@ -318,6 +323,21 @@ def __publish(self):
         self.__publish_kinematics()
         self.__publish_counter += 1
 
+    def __update_boat_state(self):
+        """
+        Generates the next vectors for wind_generator and current_generator and updates the
+        boat_state with the new wind and current vectors along with the rudder_angle and
+        sail_trim_tab_angle.
+        """
+        # Wind parameter in line below introduces noise
+        self.__sim_wind_sensor.wind = self.__wind_generator.next()
+        self.__boat_state.step(
+            self.__sim_wind_sensor.wind,
+            self.__current_generator.next(),
+            self.__rudder_angle,
+            self.__sail_trim_tab_angle,
+        )
+
     def __publish_gps(self):
         """Publishes mock GPS data."""
         # TODO Update to publish real data
@@ -586,19 +606,6 @@ def __sail_action_feedback_callback(
             .double_value,
         )
 
-    def __update_boat_state(self):
-        """
-        Generates the next vectors for wind_generator and current_generator and updates the
-        boat_state with the new wind and current vectors along with the rudder_angle and
-        sail_trim_tab_angle.
-        """
-        self.__boat_state.step(
-            self.__wind_generator.next(),
-            self.__current_generator.next(),
-            self.__rudder_angle,
-            self.__sail_trim_tab_angle,
-        )
-
     # CLASS PROPERTY PUBLIC GETTERS
     @property
     def is_multithreading_enabled(self) -> bool:

src/global_launch/config/README.md

@@ -250,7 +250,7 @@ specified within an array: one for the `x` component, and one for the `y` compon
 `wind_sensor.generator_type` is `gaussian`.
 - _Datatype_: `double` array, length 2
 - _Range_: `(0.0, MAX_DOUBLE)`
-    - If a standard deviation of zero is desired, then consider using the constant generator instead.
+  - If a standard deviation of zero is desired, then consider using the constant generator instead.
 
 **`wind_sensor.generator_type`**
 
@@ -262,7 +262,7 @@ specified within an array: one for the `x` component, and one for the `y` compon
 
 - _Description_: The mean value for the wind generated, expressed in kilometers per hour (km/h), for the multivariate
 Gaussian generator.
-- _Datatype_: `double` array, length 3
+- _Datatype_: `double` array, length 2
 - _Range_: `(0.0, MAX_DOUBLE)`
 
 **`wind_generation.mvgaussian_params.cov`**
@@ -276,7 +276,7 @@ since ROS parameters do not support native 2D array types.
 
 - _Description_: The mean value for the current generated, expressed in kilometers per hour (km/h), for the multivariate
 Gaussian generator.
-- _Datatype_: `double` array, length 3
+- _Datatype_: `double` array, length 2
 - _Range_: `(0.0, MAX_DOUBLE)`
 
 **`current_generation.mvgaussian_params.cov`**

src/global_launch/config/globals.yaml

@@ -62,12 +62,12 @@ physics_engine_node:
             value: [1.0, 0.0]
       wind_generation:
          mvgaussian_params:
-            mean: [5.0, 5.0, 0.0]
-            cov: "[[25.0, 10.0, 5.0], [10.0, 15.0, 2.0], [5.0, 2.0, 20.0]]"
+            mean: [5.0, 5.0]
+            cov: "[[25.0, 10.0], [10.0, 15.0]]"
       current_generation:
          mvgaussian_params:
-            mean: [1.0, 0.5, 0.0]
-            cov: "[[0.5, 0.1, 0.05], [0.1, 0.3, 0.02], [0.05, 0.02, 0.2]]"
+            mean: [1.0, 0.5]
+            cov: "[[0.5, 0.1], [0.1, 0.3]]"
 data_collection_node:
    ros__parameters:
       file_name: 'ros_data_collection'

src/local_pathfinding/local_pathfinding/local_path.py

@@ -2,9 +2,9 @@
 
 from typing import List, Optional, Tuple
 
+from custom_interfaces.msg import GPS, AISShips, HelperLatLon, Path, WindSensor
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
-from custom_interfaces.msg import GPS, AISShips, HelperLatLon, Path, WindSensor
 from local_pathfinding.ompl_path import OMPLPath
 
 

src/local_pathfinding/local_pathfinding/objectives.py

@@ -4,8 +4,8 @@
 from enum import Enum, auto
 
 import numpy as np
+import pyompl
 from custom_interfaces.msg import HelperLatLon
-from ompl import base as ob
 
 import local_pathfinding.coord_systems as cs
 
@@ -56,7 +56,7 @@ class SpeedObjectiveMethod(Enum):
     SAILBOT_TIME = auto()
 
 
-class Objective(ob.StateCostIntegralObjective):
+class Objective(pyompl.StateCostIntegralObjective):
     """All of our optimization objectives inherit from this class.
 
     Notes:
@@ -73,7 +73,7 @@ def __init__(self, space_information):
         super().__init__(si=space_information, enableMotionCostInterpolation=True)
         self.space_information = space_information
 
-    def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
+    def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyompl.Cost:
         raise NotImplementedError
 
 
@@ -82,7 +82,8 @@ class DistanceObjective(Objective):
 
     Attributes:
         method (DistanceMethod): The method of the distance objective function
-        ompl_path_objective (ob.PathLengthOptimizationObjective): The OMPL path length objective.
+        ompl_path_objective (pyompl.PathLengthOptimizationObjective): The OMPL path length
+        objective.
             Only defined if the method is OMPL path length.
         reference (HelperLatLon): The XY origin when converting from latlon to XY.
             Only defined if the method is latlon.
@@ -97,19 +98,21 @@ def __init__(
         super().__init__(space_information)
         self.method = method
         if self.method == DistanceMethod.OMPL_PATH_LENGTH:
-            self.ompl_path_objective = ob.PathLengthOptimizationObjective(self.space_information)
+            self.ompl_path_objective = pyompl.PathLengthOptimizationObjective(
+                self.space_information
+            )
         elif self.method == DistanceMethod.LATLON:
             self.reference = reference
 
-    def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
+    def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyompl.Cost:
         """Generates the distance between two points
 
         Args:
             s1 (SE2StateInternal): The starting point of the local start state
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The distance between two points object
+            pyompl.Cost: The distance between two points object
 
         Raises:
             ValueError: If the distance method is not supported
@@ -118,12 +121,12 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         s2_xy = cs.XY(s2.getX(), s2.getY())
         if self.method == DistanceMethod.EUCLIDEAN:
             distance = DistanceObjective.get_euclidean_path_length_objective(s1_xy, s2_xy)
-            cost = ob.Cost(distance)
+            cost = pyompl.Cost(distance)
         elif self.method == DistanceMethod.LATLON:
             distance = DistanceObjective.get_latlon_path_length_objective(
                 s1_xy, s2_xy, self.reference
             )
-            cost = ob.Cost(distance)
+            cost = pyompl.Cost(distance)
         elif self.method == DistanceMethod.OMPL_PATH_LENGTH:
             cost = self.ompl_path_objective.motionCost(s1_xy, s2_xy)
         else:
@@ -191,15 +194,15 @@ def __init__(
         self.heading = math.radians(heading_degrees)
         self.method = method
 
-    def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
+    def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyompl.Cost:
         """Generates the turning cost between s1, s2, heading or the goal position
 
         Args:
             s1 (SE2StateInternal): The starting point of the local start state
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The minimum turning angle in degrees
+            pyompl.Cost: The minimum turning angle in degrees
 
         Raises:
             ValueError: If the minimum turning method is not supported
@@ -214,7 +217,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             angle = self.heading_path_turn_cost(s1_xy, s2_xy, self.heading)
         else:
             ValueError(f"Method {self.method} not supported")
-        return ob.Cost(angle)
+        return pyompl.Cost(angle)
 
     @staticmethod
     def goal_heading_turn_cost(s1: cs.XY, goal: cs.XY, heading: float) -> float:
@@ -306,7 +309,7 @@ def __init__(self, space_information, wind_direction_degrees: float):
         assert -180 < wind_direction_degrees <= 180
         self.wind_direction = math.radians(wind_direction_degrees)
 
-    def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
+    def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyompl.Cost:
         """Generates the cost associated with the upwind and downwind directions of the boat in
         relation to the wind.
 
@@ -315,11 +318,11 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The cost of going upwind or downwind
+            pyompl.Cost: The cost of going upwind or downwind
         """
         s1_xy = cs.XY(s1.getX(), s1.getY())
         s2_xy = cs.XY(s2.getX(), s2.getY())
-        return ob.Cost(WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction))
+        return pyompl.Cost(WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction))
 
     @staticmethod
     def wind_direction_cost(s1: cs.XY, s2: cs.XY, wind_direction: float) -> float:
@@ -435,15 +438,15 @@ def __init__(
         self.wind_speed = wind_speed
         self.method = method
 
-    def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
+    def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyompl.Cost:
         """Generates the cost associated with the speed of the boat.
 
         Args:
             s1 (SE2StateInternal): The starting point of the local start state
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The cost of going upwind or downwind
+            pyompl.Cost: The cost of going upwind or downwind
         """
 
         s1_xy = cs.XY(s1.getX(), s1.getY())
@@ -454,18 +457,18 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         )
 
         if sailbot_speed == 0:
-            return ob.Cost(10000)
+            return pyompl.Cost(10000)
 
         if self.method == SpeedObjectiveMethod.SAILBOT_TIME:
             distance = DistanceObjective.get_euclidean_path_length_objective(s1_xy, s2_xy)
             time = distance / sailbot_speed
 
-            cost = ob.Cost(time)
+            cost = pyompl.Cost(time)
 
         elif self.method == SpeedObjectiveMethod.SAILBOT_PIECEWISE:
-            cost = ob.Cost(self.get_piecewise_cost(sailbot_speed))
+            cost = pyompl.Cost(self.get_piecewise_cost(sailbot_speed))
         elif self.method == SpeedObjectiveMethod.SAILBOT_CONTINUOUS:
-            cost = ob.Cost(self.get_continuous_cost(sailbot_speed))
+            cost = pyompl.Cost(self.get_continuous_cost(sailbot_speed))
         else:
             ValueError(f"Method {self.method} not supported")
         return cost
@@ -569,8 +572,9 @@ def get_sailing_objective(
     heading_degrees: float,
     wind_direction_degrees: float,
     wind_speed: float,
-) -> ob.OptimizationObjective:
-    objective = ob.MultiOptimizationObjective(si=space_information)
+) -> pyompl.OptimizationObjective:
+
+    objective = pyompl.MultiOptimizationObjective(si=space_information)
     objective.addObjective(
         objective=DistanceObjective(space_information, DistanceMethod.LATLON),
         weight=1.0,