From 7d9960d6eb5ccb177d8786d6b32ec8ddd16a2766 Mon Sep 17 00:00:00 2001
From: kristatraboulay <144058657+kristatraboulay@users.noreply.github.com>
Date: Sat, 23 Nov 2024 10:36:16 -0800
Subject: [PATCH] Update wind sensor in physics engine node (#444)

* End of meeting

* Updated boat state with wind from SimWindSensor instance

* Minor changes/ comments

* Made PR recommended changes

* Updated wind standard deviation and removed List import

* Updated wind sensor wind directly in boat state

* Updated wind parameter directly with boat state
---
 .../physics_engine/physics_engine_node.py     | 33 +++++++++++--------
 1 file changed, 20 insertions(+), 13 deletions(-)

diff --git a/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py b/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py
index 2427d12dd..b2573610f 100644
--- a/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py
+++ b/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py
@@ -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,10 @@ def __init_private_attributes(self):
             generator=MVGaussianGenerator(current_mean, current_cov)
         )
 
+        # No delay in this instance
+        sim_wind = self.__wind_generator.next()
+        self.__sim_wind_sensor = SimWindSensor(sim_wind, enable_noise=True)
+
     def __init_callback_groups(self):
         """Initializes the callback groups. Whether multithreading is enabled or not will affect
         how callbacks are executed.
@@ -318,6 +323,21 @@ def __publish(self):
         self.__publish_kinematics()
         self.__publish_counter += 1
 
+    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.
+        """
+        # Wind parameter in line below introduces noise
+        self.__sim_wind_sensor.wind = self.__wind_generator.next()
+        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 +606,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: