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

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

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

@@ -374,4 +374,6 @@ ENV DEBIAN_FRONTEND=
 # install dev python3 dependencies
 RUN pip3 install \
     # for juypter notebooks
-    ipykernel
+    ipykernel \
+    # to generate ompl python bindings
+    pybind11

scripts/test.sh

@@ -20,6 +20,8 @@ if [ -d $NET_DIR ]; then
     pushd $NET_DIR
     ./scripts/sailbot_db sailbot_db --clear
     ./scripts/sailbot_db sailbot_db --populate
+    python3 scripts/rockblock_web_server.py &
+    ROCKBLOCK_SERVER_PID=$!
     popd
 fi
 

src/boat_simulator/boat_simulator/common/constants.py

@@ -37,17 +37,35 @@ class PhysicsEnginePublisherTopics:
 
 @dataclass
 class BoatProperties:
-    sail_lift_coeffs: NDArray  # Degrees, Dimensionless
-    sail_drag_coeffs: NDArray  # Degrees, Dimensionless
-    sail_areas: Scalar  # Degrees, Square meters (m^2)
-    rudder_lift_coeffs: NDArray  # Degrees, Dimensionless
-    rudder_drag_coeffs: NDArray  # Degrees, Dimensionless
-    rudder_areas: Scalar  # Degrees, Square meters (m^2)
-    sail_dist: Scalar  # Meters (m)
-    rudder_dist: Scalar  # Meters (m)
-    hull_drag_factor: Scalar  # Dimensionless
-    mass: Scalar  # Kilograms (kg)
-    inertia: NDArray  # Kilograms-meters squared (kg•m^2)
+    # A lookup table that maps angles of attack (in degrees) to their corresponding lift
+    # coefficients.
+    sail_lift_coeffs: NDArray
+    # A lookup table that maps angles of attack (in degrees) to their corresponding drag
+    # coefficients.
+    sail_drag_coeffs: NDArray
+    # A lookup table that maps angles of attack (in degrees) to their corresponding sail areas
+    # (in square meters).
+    sail_areas: Scalar
+    # A lookup table that maps angles of attack (in degrees) to their corresponding drag
+    # coefficients for the rudder.
+    rudder_lift_coeffs: NDArray  
+    rudder_drag_coeffs: NDArray
+    # A lookup table that maps angles of attack (in degrees) to their corresponding rudder areas
+    # (in square meters).
+    rudder_areas: Scalar
+    # A scalar representing the distance from the center of effort of the sail to the pivot point
+    # (in meters).
+    sail_dist: Scalar
+    # A scalar representing the distance from the center of effort of the rudder to the pivot
+    # point (in meters).
+    rudder_dist: Scalar
+    # A dimensionless scalar representing the drag factor of the hull as a function of the boat's
+    # velocity.
+    hull_drag_factor: Scalar
+    # The mass of the boat (in kilograms).
+    mass: Scalar
+    # The inertia of the boat (in kilograms-meters squared).
+    inertia: NDArray
     air_density: Scalar  # Kilograms per meter cubed (kg/m^3)
     water_density: Scalar  # Kilograms per meter cubed (kg/m^3)
     centre_of_gravity: NDArray  # Meters (m)
@@ -92,7 +110,7 @@ class BoatProperties:
 # Max sail actuator control angle range in degrees, min angle [0], max angle [1]
 SAIL_MAX_ANGLE_RANGE = (-7, 7)
 
-# Predetermined values for BoatProperties
+# Constants related to the physical and mechanical properties of Polaris
 # TODO These are placeholder values which should be replaced when we have real values.
 BOAT_PROPERTIES = BoatProperties(
     sail_lift_coeffs=np.array([[0.0, 0.0], [5.0, 0.2], [10.0, 0.5], [15.0, 0.7], [20.0, 1.0]]),

src/boat_simulator/boat_simulator/common/sensors.py

@@ -80,7 +80,7 @@ def read(self, key: str) -> Any:
             )
 
 
-class WindSensor(Sensor):
+class SimWindSensor(Sensor):
     """
     Abstraction for wind sensor.
 
@@ -138,7 +138,7 @@ def wind(self, wind: ScalarOrArray):
         self.wind_next_value = wind
 
 
-class GPS(Sensor):
+class SimGPS(Sensor):
     """
     Abstraction for GPS.
 

src/boat_simulator/boat_simulator/nodes/data_collection/data_collection_node.py

@@ -32,9 +32,9 @@ def shutdown_handler(signum: int, frame: Any) -> None:
 
 def main(args=None):
     rclpy.init(args=args)
-    node = DataCollectionNode()
     if is_collection_enabled():
         try:
+            node = DataCollectionNode()
             # TODO Explore alternatives to using the signal library, such as ROS event handlers
             signal.signal(signal.SIGINT, shutdown_handler)
             rclpy.spin(node)

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 2D fluid velocities.
+        `generator` (VectorGenerator): The vector generator used to generate 3D fluid velocities.
         `velocity` (NDArray): The most recently generated fluid velocity vector, expressed in
-            meters per second (m/s). It is expected to be a 2D vector.
+            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 == (2,)
+        assert self.__velocity.shape == (3,)
 
     def next(self) -> NDArray:
         """Generates the next velocity vector for the fluid simulation.

src/boat_simulator/boat_simulator/nodes/physics_engine/model.py

@@ -5,6 +5,7 @@
 import numpy as np
 from numpy.typing import NDArray
 
+from boat_simulator.common.constants import BOAT_PROPERTIES
 from boat_simulator.common.constants import BOAT_PROPERTIES
 from boat_simulator.common.types import Scalar
 from boat_simulator.nodes.physics_engine.fluid_forces import MediumForceComputation

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

@@ -2,6 +2,7 @@
 
 """The ROS node for the physics engine."""
 
+import json
 import sys
 from typing import Optional
 
@@ -92,8 +93,8 @@ def __init__(self, multithreading_enabled: bool):
         self.__is_multithreading_enabled = multithreading_enabled
 
         self.get_logger().debug("Initializing node...")
-        self.__init_private_attributes()
         self.__declare_ros_parameters()
+        self.__init_private_attributes()
         self.__init_callback_groups()
         self.__init_subscriptions()
         self.__init_publishers()
@@ -102,24 +103,6 @@ def __init__(self, multithreading_enabled: bool):
         self.get_logger().debug("Node initialization complete. Starting execution...")
 
     # INITIALIZATION HELPERS
-    def __init_private_attributes(self):
-        """Initializes the private attributes of this class that are not set anywhere else during
-        the initialization process.
-        """
-        self.__publish_counter = 0
-        self.__rudder_angle = 0
-        self.__sail_trim_tab_angle = 0
-        self.__desired_heading = None
-        self.__boat_state = BoatState(
-            0.5, Constants.BOAT_PROPERTIES.mass, Constants.BOAT_PROPERTIES.inertia
-        )
-        self.__wind_generator = FluidGenerator(
-            generator=MVGaussianGenerator(np.array([5, 5]), np.array([[2, 1], [1, 2]]))
-        )
-        self.__current_generator = FluidGenerator(
-            generator=MVGaussianGenerator(np.array([1, 1]), np.array([[2, 1], [1, 2]]))
-        )
-
     def __declare_ros_parameters(self):
         """Declares ROS parameters from the global configuration file that will be used in this
         node. This node will monitor for any changes to these parameters during execution and will
@@ -142,6 +125,10 @@ def __declare_ros_parameters(self):
                 ("wind_sensor.gaussian_params.std_dev", rclpy.Parameter.Type.DOUBLE_ARRAY),
                 ("wind_sensor.gaussian_params.corr_xy", rclpy.Parameter.Type.DOUBLE),
                 ("wind_sensor.constant_params.value", rclpy.Parameter.Type.DOUBLE_ARRAY),
+                ("wind_generation.mvgaussian_params.mean", rclpy.Parameter.Type.DOUBLE_ARRAY),
+                ("wind_generation.mvgaussian_params.cov", rclpy.Parameter.Type.STRING),
+                ("current_generation.mvgaussian_params.mean", rclpy.Parameter.Type.DOUBLE_ARRAY),
+                ("current_generation.mvgaussian_params.cov", rclpy.Parameter.Type.STRING),
             ],
         )
 
@@ -151,6 +138,50 @@ def __declare_ros_parameters(self):
             value_str = str(parameter.value)
             self.get_logger().debug(f"Got parameter {name} with value {value_str}")
 
+    def __init_private_attributes(self):
+        """Initializes the private attributes of this class that are not set anywhere else during
+        the initialization process.
+        """
+        self.__publish_counter = 0
+        self.__rudder_angle = 0
+        self.__sail_trim_tab_angle = 0
+        self.__desired_heading = None
+        self.__boat_state = BoatState(self.pub_period)
+
+        wind_mean = np.array(
+            self.get_parameter("wind_generation.mvgaussian_params.mean")
+            .get_parameter_value()
+            .double_array_value
+        )
+        # Parse the covariance matrix from a string into a 2D array, as ROS parameters do not
+        # support native 2D array types.
+        wind_cov = np.array(
+            json.loads(
+                self.get_parameter("wind_generation.mvgaussian_params.cov")
+                .get_parameter_value()
+                .string_value
+            )
+        )
+        self.__wind_generator = FluidGenerator(generator=MVGaussianGenerator(wind_mean, wind_cov))
+
+        current_mean = np.array(
+            self.get_parameter("current_generation.mvgaussian_params.mean")
+            .get_parameter_value()
+            .double_array_value
+        )
+        # Parse the covariance matrix from a string into a 2D array, as ROS parameters do not
+        # support native 2D array types.
+        current_cov = np.array(
+            json.loads(
+                self.get_parameter("current_generation.mvgaussian_params.cov")
+                .get_parameter_value()
+                .string_value
+            )
+        )
+        self.__current_generator = FluidGenerator(
+            generator=MVGaussianGenerator(current_mean, current_cov)
+        )
+
     def __init_callback_groups(self):
         """Initializes the callback groups. Whether multithreading is enabled or not will affect
         how callbacks are executed.

src/boat_simulator/tests/unit/common/test_gps_sensor.py

@@ -1,14 +1,14 @@
-from boat_simulator.common.sensors import GPS
+from boat_simulator.common.sensors import SimGPS
 import numpy as np
 
 
-class TestGPS:
+class TestSimGPS:
     def test_gps_init(self):
         lat_lon = np.array([1, 0])
         speed = 100
         heading = 1.09
 
-        gps = GPS(
+        gps = SimGPS(
             lat_lon=lat_lon,
             speed=speed,
             heading=heading,
@@ -23,7 +23,7 @@ def test_gps_read_no_noise(self):
         speed = np.random.randint(0, 100)
         heading = np.random.rand()
 
-        gps = GPS(lat_lon=lat_lon, speed=speed, heading=heading, enable_noise=False)
+        gps = SimGPS(lat_lon=lat_lon, speed=speed, heading=heading, enable_noise=False)
 
         assert np.all(gps.read("lat_lon") == lat_lon)
         assert gps.read("speed") == speed
@@ -35,7 +35,7 @@ def test_gps_gaussian_noise(self):
         heading = np.random.rand()
         mean = 0
 
-        gps = GPS(
+        gps = SimGPS(
             lat_lon=lat_lon,
             speed=speed,
             heading=heading,
@@ -62,7 +62,7 @@ def test_gps_sensor_update(self):
         speed = 0
         heading = 0
 
-        gps = GPS(
+        gps = SimGPS(
             lat_lon=lat_lon,
             speed=speed,
             heading=heading,
@@ -88,7 +88,7 @@ def test_gps_sensor_update_delay(self):
         heading = 0
 
         # Initialized data is read without delay
-        gps = GPS(lat_lon=lat_lon, speed=speed0, heading=heading, enable_delay=True)
+        gps = SimGPS(lat_lon=lat_lon, speed=speed0, heading=heading, enable_delay=True)
         assert gps.read("speed") == speed0
 
         NUM_UPDATES = 3

src/boat_simulator/tests/unit/common/test_wind_sensor.py

@@ -1,19 +1,19 @@
-from boat_simulator.common.sensors import WindSensor
+from boat_simulator.common.sensors import SimWindSensor
 import numpy as np
 
 
-class TestWindSensor:
+class TestSimWindSensor:
     def test_wind_sensor_init(self):
         init_data = np.array([1, 0])
-        ws = WindSensor(
+        ws = SimWindSensor(
             wind=init_data,
         )
 
         assert np.all(ws.wind == init_data)
 
     def test_wind_sensor_read_no_noise(self):
         init_data = np.array([1, 0])
-        ws = WindSensor(
+        ws = SimWindSensor(
             wind=init_data,
         )
         read_data = ws.read("wind")
@@ -23,7 +23,7 @@ def test_wind_sensor_read_mv_gaussian_noise(self):
         init_data = np.array([0, 0])
         mean = np.array([0, 0])
         cov = np.eye(2)
-        ws = WindSensor(wind=init_data, enable_noise=True)
+        ws = SimWindSensor(wind=init_data, enable_noise=True)
 
         NUM_READINGS = 10000
         reading = np.zeros(shape=(NUM_READINGS, mean.size))
@@ -38,7 +38,7 @@ def test_wind_sensor_read_mv_gaussian_noise(self):
 
     def test_wind_sensor_update_no_delay(self):
         init_data = np.array([0, 0])
-        ws = WindSensor(wind=init_data)
+        ws = SimWindSensor(wind=init_data)
 
         NUM_READINGS = 100
         for i in range(NUM_READINGS):
@@ -54,7 +54,7 @@ def test_wind_sensor_update_with_delay(self):
 
         init_data = np.array([0, 0])
 
-        ws = WindSensor(wind=init_data, enable_delay=True)
+        ws = SimWindSensor(wind=init_data, enable_delay=True)
 
         wind = ws.read("wind")
         # Initialized data is read without delay