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 List, Optional
 
 import numpy as np
 import rclpy
@@ -33,6 +33,7 @@
 
 import boat_simulator.common.constants as Constants
 from boat_simulator.common.generators import MVGaussianGenerator
+from boat_simulator.common.sensors import SimWindSensor
 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
@@ -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,6 +317,7 @@ 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?)
@@ -318,6 +326,23 @@ def __publish(self):
         self.__publish_kinematics()
         self.__publish_counter += 1
 
+    def __update_sim_wind_sensor(self):
+        """Updates wind parameter of wind sensor"""
+        self.__sim_wind_sensor.wind = self.__wind_generator.next()
+
+    def __update_boat_state(self):
+        """
+        Generates the next vectors for wind_generator and current_generator and updates the
+        boat_state with the new wind and current vectors along with the rudder_angle and
+        sail_trim_tab_angle.
+        """
+        self.__boat_state.step(
+            self.__sim_wind_sensor.wind,
+            self.__current_generator.next(),
+            self.__rudder_angle,
+            self.__sail_trim_tab_angle,
+        )
+
     def __publish_gps(self):
         """Publishes mock GPS data."""
         # TODO Update to publish real data
@@ -586,19 +611,6 @@ def __sail_action_feedback_callback(
             .double_value,
         )
 
-    def __update_boat_state(self):
-        """
-        Generates the next vectors for wind_generator and current_generator and updates the
-        boat_state with the new wind and current vectors along with the rudder_angle and
-        sail_trim_tab_angle.
-        """
-        self.__boat_state.step(
-            self.__wind_generator.next(),
-            self.__current_generator.next(),
-            self.__rudder_angle,
-            self.__sail_trim_tab_angle,
-        )
-
     # CLASS PROPERTY PUBLIC GETTERS
     @property
     def is_multithreading_enabled(self) -> bool: