Merge branch 'main' of https://github.com/UBCSailbot/sailbot_workspace …

…into user/stevenxu27/404-Complete-Boat-State-Class

.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

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/fluid_generation.py

@@ -12,15 +12,15 @@ class FluidGenerator:
     """This class provides functionality to generate velocity vectors representing fluid movements.
 
     Attributes:
-        `generator` (VectorGenerator): The vector generator used to generate 3D fluid velocities.
+        `generator` (VectorGenerator): The vector generator used to generate 2D fluid velocities.
         `velocity` (NDArray): The most recently generated fluid velocity vector, expressed in
             meters per second (m/s). It is expected to be a 3D vector.
     """
 
     def __init__(self, generator: VectorGenerator):
         self.__generator = generator
         self.__velocity = np.array(self.__generator.next())
-        assert self.__velocity.shape == (3,)
+        assert self.__velocity.shape == (2,)
 
     def next(self) -> NDArray:
         """Generates the next velocity vector for the fluid simulation.

src/boat_simulator/tests/unit/nodes/physics_engine/test_fluids.py

@@ -11,9 +11,9 @@ class TestFluidGenerator:
     @pytest.mark.parametrize(
         "vector",
         [
-            (np.array([1, 0, 1])),
-            (np.array([0, 1, 0])),
-            (np.array([1, 0, 0])),
+            (np.array([1, 0])),
+            (np.array([0, 1])),
+            (np.array([1, 0])),
         ],
     )
     def test_velocity_constant(self, vector):
@@ -26,10 +26,10 @@ def test_velocity_constant(self, vector):
     @pytest.mark.parametrize(
         "mean, cov",
         [
-            (np.array([1, 2, 0]), np.array([[2, 1, 1], [1, 2, 0.9], [1, 0.9, 1]])),
-            (np.array([4, 5, 3]), np.array([[3, 1, 1], [1, 3, 1], [1, 1, 2]])),
-            (np.array([100, 50, 20]), np.array([[10, 5, 5], [5, 10, 4.5], [5, 4.5, 5]])),
-            (np.array([120, 130, 40]), np.array([[10, 5, 1], [5, 10, 2], [1, 2, 5]])),
+            (np.array([1, 2]), np.array([[2, 1], [1, 2]])),
+            (np.array([4, 5]), np.array([[3, 1], [1, 3]])),
+            (np.array([100, 50]), np.array([[10, 5], [5, 10]])),
+            (np.array([120, 130]), np.array([[10, 5], [5, 10]])),
         ],
     )
     def test_velocity_random(self, mean, cov):
@@ -42,12 +42,12 @@ def test_velocity_random(self, mean, cov):
     @pytest.mark.parametrize(
         "vector",
         [
-            (np.array([1, 0, 1])),
-            (np.array([0, 1, 0])),
-            (np.array([-1, 0, 1])),
-            (np.array([0, -1, 0])),
-            (np.array([1, 1, 1])),
-            (np.array([-1, -1, -1])),
+            (np.array([1, 0])),
+            (np.array([0, 1])),
+            (np.array([-1, 0])),
+            (np.array([0, -1])),
+            (np.array([1, 1])),
+            (np.array([-1, -1])),
         ],
     )
     def test_speed(self, vector):
@@ -60,12 +60,12 @@ def test_speed(self, vector):
     @pytest.mark.parametrize(
         "vector, expected_direction",
         [
-            (np.array([1, 0, 1]), 0),
-            (np.array([0, 1, -3]), 90),
-            (np.array([-1, 0, -1]), -180),
-            (np.array([0, -1, 0]), -90),
-            (np.array([1, 1, 4]), 45),
-            (np.array([-1, -1, 6]), -135),
+            (np.array([1, 0]), 0),
+            (np.array([0, 1]), 90),
+            (np.array([-1, 0]), -180),
+            (np.array([0, -1]), -90),
+            (np.array([1, 1]), 45),
+            (np.array([-1, -1]), -135),
         ],
     )
     def test_direction(self, vector, expected_direction):

src/global_launch/config/README.md

@@ -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,