Publish Desired Heading Calculation

.devcontainer/base-dev/base-dev.Dockerfile

@@ -94,7 +94,7 @@ RUN apt-get update \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
-RUN pip3 install pygccxml pyplusplus
+RUN pip3 install pygccxml pyplusplus pybind11
 COPY --from=ompl-source /ompl /ompl
 WORKDIR /ompl
 RUN cmake \

sailbot.code-workspace

@@ -521,6 +521,7 @@
                     "integration_tests",
                     "local_pathfinding",
                     "network_systems",
+                    "sailompl",
                 ]
             },
         ]

scripts/build.sh

@@ -41,3 +41,6 @@ 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"
+
+
+export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH

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/local_pathfinding/local_pathfinding/local_path.py

@@ -2,9 +2,10 @@
 
 from typing import List, Optional, Tuple
 
+import sailompl
+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 sailompl
 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(sailompl.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: sailompl.SE2StateSpace, s2: sailompl.SE2StateSpace) -> sailompl.Cost:
         raise NotImplementedError
 
 
@@ -82,7 +82,7 @@ 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 (sailompl.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 +97,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 = sailompl.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: sailompl.SE2StateSpace, s2: sailompl.SE2StateSpace) -> sailompl.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
+            sailompl.Cost: The distance between two points object
 
         Raises:
             ValueError: If the distance method is not supported
@@ -118,12 +120,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 = sailompl.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 = sailompl.Cost(distance)
         elif self.method == DistanceMethod.OMPL_PATH_LENGTH:
             cost = self.ompl_path_objective.motionCost(s1_xy, s2_xy)
         else:
@@ -191,15 +193,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: sailompl.SE2StateSpace, s2: sailompl.SE2StateSpace) -> sailompl.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
+            sailompl.Cost: The minimum turning angle in degrees
 
         Raises:
             ValueError: If the minimum turning method is not supported
@@ -214,7 +216,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 sailompl.Cost(angle)
 
     @staticmethod
     def goal_heading_turn_cost(s1: cs.XY, goal: cs.XY, heading: float) -> float:
@@ -306,7 +308,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: sailompl.SE2StateSpace, s2: sailompl.SE2StateSpace) -> sailompl.Cost:
         """Generates the cost associated with the upwind and downwind directions of the boat in
         relation to the wind.
 
@@ -315,11 +317,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
+            sailompl.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 sailompl.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 +437,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: sailompl.SE2StateSpace, s2: sailompl.SE2StateSpace) -> sailompl.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
+            sailompl.Cost: The cost of going upwind or downwind
         """
 
         s1_xy = cs.XY(s1.getX(), s1.getY())
@@ -454,18 +456,18 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         )
 
         if sailbot_speed == 0:
-            return ob.Cost(10000)
+            return sailompl.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 = sailompl.Cost(time)
 
         elif self.method == SpeedObjectiveMethod.SAILBOT_PIECEWISE:
-            cost = ob.Cost(self.get_piecewise_cost(sailbot_speed))
+            cost = sailompl.Cost(self.get_piecewise_cost(sailbot_speed))
         elif self.method == SpeedObjectiveMethod.SAILBOT_CONTINUOUS:
-            cost = ob.Cost(self.get_continuous_cost(sailbot_speed))
+            cost = sailompl.Cost(self.get_continuous_cost(sailbot_speed))
         else:
             ValueError(f"Method {self.method} not supported")
         return cost
@@ -569,8 +571,9 @@ def get_sailing_objective(
     heading_degrees: float,
     wind_direction_degrees: float,
     wind_speed: float,
-) -> ob.OptimizationObjective:
-    objective = ob.MultiOptimizationObjective(si=space_information)
+) -> sailompl.OptimizationObjective:
+
+    objective = sailompl.MultiOptimizationObjective(si=space_information)
     objective.addObjective(
         objective=DistanceObjective(space_information, DistanceMethod.LATLON),
         weight=1.0,