added ability to corrupt wind sensors

README.md

@@ -61,6 +61,13 @@ All of the arguments below are optional for `all.launch`. Below is a non-exhaust
 * `random_seed` - Int value that we set the random seed to. Default: `""`, which results in the random seed being set by time.
 
 * `wandb` - Bool value that sets if data should be logged to [wandb](https://wandb.ai/ubcsailbot) for analysis. Need to login with UBC Sailbot credentials to log. Default: `false`.
+* `broken_wind_sensors` - String that specifies if the MOCK wind sensor data should be corrupted to simulate broken wind sensors, and in what way the wind data should be corrupted. Default: `not_broken` (the default setting leaves wind sensor data unmodified). In the following descriptions of the other modes, "random" means using numpy.random functions seeded with the `random_seed` so tests can be reproducible. The other modes for corrupting wind data "randomly" choose one of the 3 wind sensors, and modify the sensor's measurements in the following ways:
+  * `stuck_at_zero`: The chosen sensor will measure a wind speed of 0 knots and a wind angle of 0 degrees.
+  * `stuck_at_rand_constant`: The chosen sensor will measure a "random" wind speed and a "random" angle.
+  * `stuck_at_last_value`: The chosen sensor will operate normally for a "random" number of loops, then the wind speed and wind angle will stop measuring new values, and keep measuring the last value prior to breaking.
+  * `rand_const_error`: The chosen sensor will measure a wind angle of `(real angle) + theta`, 
+
+     and a wind speed of `a*(real speed) + b`. Theta, a, and b are "randomly" chosen constants.
 
 #### Run a specific saved pathfinding scenario
 

launch/MOCK_sensors.launch

@@ -21,6 +21,10 @@
     <arg name="smooth_changes" default="true" />
     <param name="smooth_changes" type="bool" value="$(arg smooth_changes)" />
 
+    <!-- Specifies if sensors should give generated corrupt data -->
+    <arg name="broken_wind_sensors" default="not_broken" />
+    <param name="broken_wind_sensors" type="string" value="$(arg broken_wind_sensors)" />
+
     <node pkg="local_pathfinding" type="MOCK_sensors.py" name="MOCK_sensors" />
 </launch>
 

launch/all.launch

@@ -48,6 +48,7 @@
     <arg name="wind_file" default="" />
     <arg name="AIS_token" default="" />
     <arg name="log_exactais" default="false" />
+    <arg name="broken_wind_sensors" default="not_broken" />
     <include file="$(find local_pathfinding)/launch/launch_all_mocks.launch">
       <arg name="speedup" value="$(arg speedup)"/>
       <arg name="global_wind_speed_kmph" value="$(arg global_wind_speed_kmph)" />
@@ -68,6 +69,7 @@
       <arg name="wind_file" value="$(arg wind_file)"/>
       <arg name="AIS_token" default="$(arg AIS_token)" />
       <arg name="log_exactais" default="$(arg log_exactais)" />
+      <arg name="broken_wind_sensors" default="$(arg broken_wind_sensors)" />
     </include>
 
     <!-- Wandb logger -->

launch/launch_all_mocks.launch

@@ -8,13 +8,15 @@
     <arg name="ocean_current_speed" default="0.0" />
     <arg name="ocean_current_direction" default="0.0" />
     <arg name="smooth_changes" default="true" />
+    <arg name="broken_wind_sensors" default="not_broken" />
     <include file="$(find local_pathfinding)/launch/MOCK_sensors.launch">
       <arg name="speedup" value="$(arg speedup)"/>
       <arg name="global_wind_speed_kmph" value="$(arg global_wind_speed_kmph)" />
       <arg name="global_wind_direction_degrees" value="$(arg global_wind_direction_degrees)" />
       <arg name="ocean_current_speed" value="$(arg ocean_current_speed)" />
       <arg name="ocean_current_direction" value="$(arg ocean_current_direction)" />
       <arg name="smooth_changes" value="$(arg smooth_changes)" />
+      <arg name="broken_wind_sensors" value="$(arg broken_wind_sensors)" />
     </include>
 
     <!-- Sets the num_ais_ships -->

python/MOCK_sensors.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 import numpy as np
 import rospy
+import random
 
 import sailbot_msg.msg as msg
 import geopy.distance
@@ -22,6 +23,12 @@
 START_GLOBAL_WIND_SPEED_KMPH = 10
 STDEV_GPS = 0.00001
 STDEV_WIND = 0.1
+MAX_BROKEN_WINDSPEED_KNOTS = 30
+MAX_ANGLE_ERROR_DEG = 60
+MAX_WINDSPEED_MULTIPLIER_ERROR = 2  # Should be >= 1
+MAX_WINDSPEED_ADDER_ERROR_KNOTS = 5
+MIN_LOOPS_TO_BREAK = 30
+MAX_LOOPS_TO_BREAK = 100
 
 
 class MOCK_SensorManager:
@@ -37,6 +44,7 @@ def __init__(self, startLat, startLon, startHeadingDegrees, startSpeedKmph, star
         self.measuredWindSpeedKmph, self.measuredWindDirectionDegrees = globalWindToMeasuredWind(
             self.globalWindSpeedKmph, self.globalWindDirectionDegrees, self.speedKmph, self.headingDegrees)
         self.publishPeriodSeconds = SENSORS_PUBLISH_PERIOD_SECONDS
+        self.brokenSensorData = None
 
         # Setup ROS node inputs and outputs
         self.publisher = rospy.Publisher("sensors", msg.Sensors, queue_size=4)
@@ -122,8 +130,148 @@ def publish(self):
 
         if rospy.get_param('sensor_noise', default=False):
             data = self.add_noise(data)
+
+        brokenWindSensorStatus = rospy.get_param('broken_wind_sensors', default="not_broken")
+        if brokenWindSensorStatus != "not_broken":
+            rospy.logwarn_throttle(30, "Broken Wind Sensor Status: " + brokenWindSensorStatus)
+            self.breakWindSensors(data, brokenWindSensorStatus)
+
         self.publisher.publish(data)
 
+    def breakWindSensors(self, data, brokenWindSensorStatus):
+        # Corrupts wind sensor data depending on the setting specified by string brokenSensorStatus
+
+        if (brokenWindSensorStatus == "stuck_at_zero"):
+            # This setting chooses a wind sensor which reads an angle of 0 and speed 0
+
+            if(self.brokenSensorData is None):
+                randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str)
+                random.seed(randomSeed)
+                self.brokenSensorData = random.randint(1, 3)
+
+            self.corrupt_wind_sensor(data, self.brokenSensorData, 0, 0)
+
+            rospy.logwarn_throttle(3, "MOCK wind_sensor_{} publishing corrupt data: ".format(self.brokenSensorData)
+                                   + "angle and speed stuck at 0")
+
+        elif (brokenWindSensorStatus == "stuck_at_rand_constant"):
+            # This setting chooses a wind sensor which reads a constant random speed and direction
+
+            if(self.brokenSensorData is None):
+                randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str)
+                random.seed(randomSeed)
+
+                self.brokenSensorData = {'brokenSensorID': random.randint(1, 3),
+                                         'brokenSensorAngle_deg': random.random() * 360,
+                                         'brokenSensorSpeed_knots': random.random() * MAX_BROKEN_WINDSPEED_KNOTS}
+
+            self.corrupt_wind_sensor(data, self.brokenSensorData['brokenSensorID'],
+                                     self.brokenSensorData['brokenSensorAngle_deg'],
+                                     self.brokenSensorData['brokenSensorSpeed_knots'])
+
+            rospy.logwarn_throttle(3, "MOCK wind_sensor_{} ".format(self.brokenSensorData['brokenSensorID'])
+                                   + "publishing corrupt data:\n"
+                                   + "angle stuck at {} degrees,".format(self.brokenSensorData['brokenSensorAngle_deg'])
+                                   + " speed stuck at {}".format(self.brokenSensorData['brokenSensorSpeed_knots'])
+                                   + " knots.")
+
+        elif(brokenWindSensorStatus == "rand_const_error"):
+            # This setting chooses a wind sensor and corrupts its data as follows:
+            # the angle published is the original angle plus a random constant theta
+            # the speed published is given by a*(original speed) + b; a and b are random constants
+            # a is within [1 / MAX_WINDSPEED_MULTIPLIER_ERROR, MAX_WINDSPEED_MULTIPLIER_ERROR]
+            # b is within [-MAX_WINDSPEED_ADDER_ERROR_KNOTS, MAX_WINDSPEED_ADDER_ERROR_KNOTS]
+            # theta is within [-MAX_ANGLE_ERROR_DEG, MAX_ANGLE_ERROR_DEG]
+
+            if(self.brokenSensorData is None):
+                randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str)
+                random.seed(randomSeed)
+
+                if random.random < 0.5:
+                    multiplier = 1 - random.random() * (1 - 1 / (MAX_WINDSPEED_MULTIPLIER_ERROR))
+                else:
+                    multiplier = 1 + random.random() * (MAX_WINDSPEED_MULTIPLIER_ERROR - 1)
+
+                self.brokenSensorData = {'brokenSensorID': random.randint(1, 3),
+                                         'theta': (2*random.random() - 1) * MAX_ANGLE_ERROR_DEG,
+                                         'windspeedMultiplier': multiplier,
+                                         'windspeedAdder': (2*random.random() - 1) * MAX_WINDSPEED_ADDER_ERROR_KNOTS}
+
+            actualAngle, actualSpeed = self.get_wind_measurement(data, self.brokenSensorData['brokenSensorID'])
+
+            newAngle = actualAngle + self.brokenSensorData['theta']
+            newSpeed = max(0, self.brokenSensorData['windspeedMultiplier'] * actualSpeed
+                           + self.brokenSensorData['windspeedAdder'])
+
+            # Bound the newAngle to [-180, 360) to improve interpretability
+            if(newAngle < -180):
+                newAngle = 360 + newAngle
+            if(newAngle >= 360):
+                newAngle = newAngle - 360
+
+            self.corrupt_wind_sensor(data, self.brokenSensorData['brokenSensorID'], newAngle, newSpeed)
+
+            rospy.logwarn_throttle(3, "MOCK wind_sensor_{} ".format(self.brokenSensorData['brokenSensorID'])
+                                   + "publishing corrupt data:\nmeasured "
+                                   + "angle: (real angle) + ({}) degrees, ".format(self.brokenSensorData['theta'])
+                                   + "measured speed: ({}) * (real speed) + ({}) knots".format(
+                                       self.brokenSensorData['windspeedMultiplier'],
+                                       self.brokenSensorData['windspeedAdder']))
+
+        elif(brokenWindSensorStatus == "stuck_at_last_value"):
+            # This setting chooses a sensor, waits a few loops, then starts publishing the same data for that sensor.
+            # This will simulate the sensor ceasing to update its measurements.
+
+            if(self.brokenSensorData is None):
+                randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str)
+                random.seed(randomSeed)
+
+                self.brokenSensorData = {'brokenSensorID': random.randint(1, 3),
+                                         'loopsUntilBroken': random.randint(MIN_LOOPS_TO_BREAK, MAX_LOOPS_TO_BREAK),
+                                         'loopsSoFar': 0, 'lastAngle': None, 'lastSpeed': None}
+
+            if self.brokenSensorData['loopsSoFar'] <= self.brokenSensorData['loopsUntilBroken']:
+                if self.brokenSensorData['loopsSoFar'] == self.brokenSensorData['loopsUntilBroken']:
+                    # store the last angle and speed that are correct before sensor stops updating
+                    self.brokenSensorData['lastAngle'], self.brokenSensorData['lastSpeed'] = self.get_wind_measurement(
+                        data, self.brokenSensorData['brokenSensorID'])
+
+                self.brokenSensorData['loopsSoFar'] += 1
+            else:
+                self.corrupt_wind_sensor(data, self.brokenSensorData['brokenSensorID'],
+                                         self.brokenSensorData['lastAngle'],
+                                         self.brokenSensorData['lastSpeed'])
+
+                rospy.logwarn_throttle(3, "MOCK wind_sensor_{} ".format(self.brokenSensorData['brokenSensorID'])
+                                       + "stopped updating measurements. Publishing corrupt data:\n"
+                                       + "angle stuck at {} degrees, ".format(self.brokenSensorData['lastAngle'])
+                                       + "speed stuck at {} knots".format(self.brokenSensorData['lastSpeed']))
+
+    def corrupt_wind_sensor(self, data, brokenSensorNumber, newAngle, newSpeed):
+        # Helper function for corrupting wind data.
+        # Overwrites the angle and speed of the wind sensor specified by brokenSensorNumber
+
+        if(brokenSensorNumber == 1):
+            data.wind_sensor_1_angle_degrees = newAngle
+            data.wind_sensor_1_speed_knots = newSpeed
+        elif(brokenSensorNumber == 2):
+            data.wind_sensor_2_angle_degrees = newAngle
+            data.wind_sensor_2_speed_knots = newSpeed
+        elif(brokenSensorNumber == 3):
+            data.wind_sensor_3_angle_degrees = newAngle
+            data.wind_sensor_3_speed_knots = newSpeed
+
+    def get_wind_measurement(self, data, sensorNumber):
+        # Helper function for corrupting wind data.
+        # Returns a tuple containing the angle and speed of the sensor specified by sensorNumber
+
+        if sensorNumber == 1:
+            return data.wind_sensor_1_angle_degrees, data.wind_sensor_1_speed_knots
+        elif sensorNumber == 2:
+            return data.wind_sensor_2_angle_degrees, data.wind_sensor_2_speed_knots
+        elif sensorNumber == 3:
+            return data.wind_sensor_3_angle_degrees, data.wind_sensor_3_speed_knots
+
     def add_noise(self, data):
         # Add noise to sensor data using numpy.random.normal. GPS and wind sensors have different standard deviations,
         # with GPS readings being much more accurate and consistent. Unused sensors are commented out.