Simulator dimension mismatches & fluid generation config (#435)

* Fix issues in model and physics node

* Fix dimension mismatching

* Fix comments for constants

* Allow wind & current generation config

* Allow config of fluid cov in globals.yaml

* Prevent data collection from making file when not in use

* Fix formatting issues with README for ci

* Update fluid generation unit tests

src/boat_simulator/boat_simulator/common/constants.py

@@ -38,16 +38,34 @@ class PhysicsEnginePublisherTopics:
 
 @dataclass
 class BoatProperties:
-    sail_lift_coeffs: Dict[Scalar, Scalar]  # Degrees, Dimensionless
-    sail_drag_coeffs: Dict[Scalar, Scalar]  # Degrees, Dimensionless
-    sail_areas: Dict[Scalar, Scalar]  # Degrees, Square meters (m^2)
-    rudder_drag_coeffs: Dict[Scalar, Scalar]  # Degrees, Dimensionless
-    rudder_areas: Dict[Scalar, 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: Dict[Scalar, Scalar]
+    # A lookup table that maps angles of attack (in degrees) to their corresponding drag
+    # coefficients.
+    sail_drag_coeffs: Dict[Scalar, Scalar]
+    # A lookup table that maps angles of attack (in degrees) to their corresponding sail areas
+    # (in square meters).
+    sail_areas: Dict[Scalar, Scalar]
+    # A lookup table that maps angles of attack (in degrees) to their corresponding drag
+    # coefficients for the rudder.
+    rudder_drag_coeffs: Dict[Scalar, Scalar]
+    # A lookup table that maps angles of attack (in degrees) to their corresponding rudder areas
+    # (in square meters).
+    rudder_areas: Dict[Scalar, 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
 
 
 # Directly accessible constants
@@ -88,17 +106,17 @@ 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={0.0: 0.0, 5.0: 0.2, 10.0: 0.5, 15.0: 0.7, 20.0: 1.0},
     sail_drag_coeffs={0.0: 0.1, 5.0: 0.12, 10.0: 0.15, 15.0: 0.18, 20.0: 0.2},
     sail_areas={0.0: 20.0, 5.0: 19.8, 10.0: 19.5, 15.0: 19.2, 20.0: 18.8},
     rudder_drag_coeffs={0.0: 0.2, 5.0: 0.22, 10.0: 0.25, 15.0: 0.28, 20.0: 0.3},
     rudder_areas={0.0: 2.0, 5.0: 1.9, 10.0: 1.8, 15.0: 1.7, 20.0: 1.6},
-    sail_dist=5.0,
-    rudder_dist=1.5,
+    sail_dist=0.5,
+    rudder_dist=1.0,
     hull_drag_factor=0.05,
-    mass=200.0,
-    inertia=np.array([[10, 0, 0], [0, 30, 0], [0, 0, 20]], dtype=np.float32),
+    mass=1500.0,
+    inertia=np.array([[125, 0, 0], [0, 1125, 0], [0, 0, 500]], dtype=np.float32),
 )

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,7 +5,7 @@
 import numpy as np
 from numpy.typing import NDArray
 
-from boat_simulator.common.constants import BoatProperties
+from boat_simulator.common.constants import BOAT_PROPERTIES
 from boat_simulator.common.types import Scalar
 from boat_simulator.nodes.physics_engine.kinematics_computation import BoatKinematics
 from boat_simulator.nodes.physics_engine.kinematics_data import KinematicsData
@@ -28,7 +28,7 @@ def __init__(self, timestep: Scalar):
             timestep (Scalar): The time interval for calculations, expressed in seconds (s).
         """
         self.__kinematics_computation = BoatKinematics(
-            timestep, BoatProperties.mass, BoatProperties.inertia
+            timestep, BOAT_PROPERTIES.mass, BOAT_PROPERTIES.inertia
         )
 
     def step(
@@ -88,7 +88,8 @@ def __compute_net_force_and_torque(
                 the relative reference frame, expressed in newtons (N), and the second element
                 represents the net torque, expressed in newton-meters (N•m).
         """
-        raise NotImplementedError()
+        # TODO Implement this function
+        return (np.array([0, 0, 0]), np.array([0, 0, 0]))
 
     @property
     def global_position(self) -> NDArray:

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/nodes/physics_engine/test_fluids.py

@@ -11,9 +11,9 @@ class TestFluidGenerator:
     @pytest.mark.parametrize(
         "vector",
         [
-            (np.array([1, 0])),
-            (np.array([0, 1])),
-            (np.array([1, 0])),
+            (np.array([1, 0, 1])),
+            (np.array([0, 1, 0])),
+            (np.array([1, 0, 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]), 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]])),
+            (np.array([1, 2, 0]), np.array([[2, 1, 1], [1, 2, 2], [3, 1, 2]])),
+            (np.array([4, 5, 3]), np.array([[3, 1, 2], [1, 3, 2], [1, 2, 2]])),
+            (np.array([100, 50, 20]), np.array([[10, 5, 5], [5, 10, 1], [6, 2, 5]])),
+            (np.array([120, 130, 40]), np.array([[10, 5, 0], [5, 10, 2], [1, 3, 5]])),
         ],
     )
     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])),
-            (np.array([0, 1])),
-            (np.array([-1, 0])),
-            (np.array([0, -1])),
-            (np.array([1, 1])),
-            (np.array([-1, -1])),
+            (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])),
         ],
     )
     def test_speed(self, vector):
@@ -60,12 +60,12 @@ def test_speed(self, vector):
     @pytest.mark.parametrize(
         "vector, expected_direction",
         [
-            (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),
+            (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),
         ],
     )
     def test_direction(self, vector, expected_direction):

src/global_launch/config/README.md

@@ -215,6 +215,12 @@ value. Otherwise, the trim tab angle is determined by the wingsail controller.
 - _Datatype_: `double`
 - _Range_: `(0.0, MAX_DOUBLE)`
 
+**`wingsail.actuation_request_period_sec`**
+
+- _Description_: How often the sail action server requests a wingsail actuation.
+- _Datatype_: `double`
+- _Range_: `(0.0, MAX_DOUBLE)`
+
 **`wind_sensor.constant_params.value`**
 
 - _Description_: Specifies the constant vector returned by the constant generator that represents the wind velocity in kmph.
@@ -252,10 +258,32 @@ specified within an array: one for the `x` component, and one for the `y` compon
 - _Datatype_: `string`
 - _Acceptable Values_: `gaussian`, `constant`
 
-**`wingsail.actuation_request_period_sec`**
+**`wind_generation.mvgaussian_params.mean`**
 
-- _Description_: How often the sail action server requests a wingsail actuation.
-- _Datatype_: `double`
+- _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
+- _Range_: `(0.0, MAX_DOUBLE)`
+
+**`wind_generation.mvgaussian_params.cov`**
+
+- _Description_: The covariance matrix for the generated wind, represented as a string formatted as a 2D `double` array,
+since ROS parameters do not support native 2D array types.
+- _Datatype_: `string`
+- _Range_: `(0.0, MAX_DOUBLE)`
+
+**`current_generation.mvgaussian_params.mean`**
+
+- _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
+- _Range_: `(0.0, MAX_DOUBLE)`
+
+**`current_generation.mvgaussian_params.cov`**
+
+- _Description_: The covariance matrix for the generated current, represented as a string formatted as a 2D `double`
+array, since ROS parameters do not support native 2D array types.
+- _Datatype_: `string`
 - _Range_: `(0.0, MAX_DOUBLE)`
 
 ### `data_collection_node`

src/global_launch/config/globals.yaml

@@ -60,6 +60,14 @@ physics_engine_node:
             corr_xy: 0.
          constant_params:
             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]]"
+      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]]"
 data_collection_node:
    ros__parameters:
       file_name: 'ros_data_collection'