Update wind sensor in physics engine node

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

@@ -4,7 +4,7 @@
 
 import json
 import sys
-from typing import Optional
+from typing import Optional, List
 
 import numpy as np
 import rclpy
@@ -36,6 +36,7 @@
 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
+from boat_simulator.common.sensors import SimWindSensor
 
 from .decorators import require_all_subs_active
 
@@ -182,6 +183,12 @@ def __init_private_attributes(self):
             generator=MVGaussianGenerator(current_mean, current_cov)
         )
 
+        # Used arbitrary non-zero stdev centred about 0 for now
+        # No delay in this instance
+        sim_wind = self.__wind_generator.next()
+        sim_wind_noise_stdev = List[Scalar] = [-10.0, 10.0]
+        self.__sim_wind_sensor = SimWindSensor(sim_wind, sim_wind_noise_stdev, enable_noise=True)
+
     def __init_callback_groups(self):
         """Initializes the callback groups. Whether multithreading is enabled or not will affect
         how callbacks are executed.
@@ -310,13 +317,18 @@ def __init_timer_callbacks(self):
     # PUBLISHER CALLBACKS
     def __publish(self):
         """Synchronously publishes data to all publishers at once."""
+        self.__update_sim_wind_sensor()
         self.__update_boat_state()
         # TODO Get updated boat state and publish (should this be separate from publishing?)
         # TODO Get wind sensor data and publish (should this be separate from publishing?)
         self.__publish_gps()
         self.__publish_wind_sensors()
         self.__publish_kinematics()
         self.__publish_counter += 1
+
+    def __update_sim_wind_sensor(self):
+        """Updates wind parameter of wind sensor"""
+        self.update(wind=self.__wind_generator.next())
 
     def __publish_gps(self):
         """Publishes mock GPS data."""
@@ -593,7 +605,7 @@ def __update_boat_state(self):
         sail_trim_tab_angle.
         """
         self.__boat_state.step(
-            self.__wind_generator.next(),
+            self.__sim_wind_sensor.wind,
             self.__current_generator.next(),
             self.__rudder_angle,
             self.__sail_trim_tab_angle,