From 8740aa5b48837ad19e2c7d6f7800486a5470dd8f Mon Sep 17 00:00:00 2001 From: Jamen Kaye Date: Sat, 20 Nov 2021 14:47:31 -0800 Subject: [PATCH 1/7] added ability to corrupt wind sensors --- launch/MOCK_sensors.launch | 4 + launch/all.launch | 2 + launch/launch_all_mocks.launch | 2 + python/MOCK_sensors.py | 152 +++++++++++++++++++++++++++++++++ 4 files changed, 160 insertions(+) diff --git a/launch/MOCK_sensors.launch b/launch/MOCK_sensors.launch index 68f8383..919aa12 100644 --- a/launch/MOCK_sensors.launch +++ b/launch/MOCK_sensors.launch @@ -21,6 +21,10 @@ + + + + diff --git a/launch/all.launch b/launch/all.launch index 1a31b2a..50f1e15 100644 --- a/launch/all.launch +++ b/launch/all.launch @@ -48,6 +48,7 @@ + @@ -68,6 +69,7 @@ + diff --git a/launch/launch_all_mocks.launch b/launch/launch_all_mocks.launch index 852c22c..8038697 100644 --- a/launch/launch_all_mocks.launch +++ b/launch/launch_all_mocks.launch @@ -8,6 +8,7 @@ + @@ -15,6 +16,7 @@ + diff --git a/python/MOCK_sensors.py b/python/MOCK_sensors.py index 4bd88fb..55a390a 100755 --- a/python/MOCK_sensors.py +++ b/python/MOCK_sensors.py @@ -1,6 +1,8 @@ #!/usr/bin/env python +# from re import I import numpy as np import rospy +import random import sailbot_msg.msg as msg import geopy.distance @@ -22,6 +24,12 @@ START_GLOBAL_WIND_SPEED_KMPH = 10 STDEV_GPS = 0.00001 STDEV_WIND = 0.1 +MAX_BROKEN_WINDSPEED_KTS = 50 +MAX_ANGLE_ERROR = 60 +MAX_WINDSPEED_MULTIPLIER_ERROR = 0.5 +MAX_WINDSPEED_ADDER_ERROR = 5 +MIN_LOOPS_UNTIL_BROKEN = 30 +MAX_LOOPS_UNTIL_BROKEN = 100 class MOCK_SensorManager: @@ -37,6 +45,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 = "uninitialized" # Setup ROS node inputs and outputs self.publisher = rospy.Publisher("sensors", msg.Sensors, queue_size=4) @@ -122,8 +131,151 @@ 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("Broken Wind Sensor Status: " + brokenWindSensorStatus) + + if (brokenWindSensorStatus == "stuck_at_zero"): + # This setting chooses a wind sensor which reads an angle of 0 and speed 0 + # brokenSensorData is used to store an integer 1, 2, or 3 representing which sensor is broken + + if(self.brokenSensorData == "uninitialized"): + # set up brokenSensorData so publish(self) remembers which sensor is stuck at zero + 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, "angle", 0) + self.corrupt_wind_sensor(data, self.brokenSensorData, "speed", 0) + + rospy.logwarn("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 + # brokenSensorData is a list containing: brokenSensorID, the constant angle, the constant direction. + + if(self.brokenSensorData == "uninitialized"): + randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str) + random.seed(randomSeed) + + brokenSensorID = random.randint(1, 3) + brokenSensorAngle_deg = random.random() * 360 + brokenSensorSpeed_kts = random.random() * MAX_BROKEN_WINDSPEED_KTS + self.brokenSensorData = [brokenSensorID, brokenSensorAngle_deg, brokenSensorSpeed_kts] + + self.corrupt_wind_sensor(data, self.brokenSensorData[0], "angle", self.brokenSensorData[1]) + self.corrupt_wind_sensor(data, self.brokenSensorData[0], "speed", self.brokenSensorData[2]) + + rospy.logwarn("MOCK wind_sensor_{} ".format(self.brokenSensorData[0]) + + "publishing corrupt data:\nangle stuck at {} degrees, ".format(self.brokenSensorData[1]) + + "speed stuck at {} knots.".format(self.brokenSensorData[2])) + + 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 + + if(self.brokenSensorData == "uninitialized"): + randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str) + random.seed(randomSeed) + + brokenSensorID = random.randint(1, 3) + theta = (2*random.random() - 1) * MAX_ANGLE_ERROR + windspeedMultiplier = 1 + (2*random.random() - 1) * MAX_WINDSPEED_MULTIPLIER_ERROR + windspeedAdder = (2*random.random() - 1) * MAX_WINDSPEED_ADDER_ERROR + self.brokenSensorData = [brokenSensorID, theta, windspeedMultiplier, windspeedAdder] + + actualAngle = self.get_wind_measurement(data, self.brokenSensorData[0], "angle") + actualSpeed = self.get_wind_measurement(data, self.brokenSensorData[0], "speed") + + newAngle = actualAngle + self.brokenSensorData[1] + newSpeed = max(0, self.brokenSensorData[2] * actualSpeed + self.brokenSensorData[3]) + + # Bound the newAngle to [0, 360) + if(newAngle < 0): + newAngle = 360 + newAngle + if(newAngle >= 360): + newAngle = newAngle - 360 + + self.corrupt_wind_sensor(data, self.brokenSensorData[0], "angle", newAngle) + self.corrupt_wind_sensor(data, self.brokenSensorData[0], "speed", newSpeed) + + rospy.logwarn("MOCK wind_sensor_{} publishing corrupt data:\n".format(self.brokenSensorData[0]) + + "measured angle: (real angle) + ({}) degrees, ".format(self.brokenSensorData[1]) + + "measured speed: ({}) * (real speed) + ({}) knots".format(self.brokenSensorData[2], + self.brokenSensorData[3])) + + elif(brokenWindSensorStatus == "stuck_at_last_value"): + # This setting chooses a sensor, waits a few loops, then starts publishing the same data for that sensor + + if(self.brokenSensorData == "uninitialized"): + randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str) + random.seed(randomSeed) + + brokenSensorID = random.randint(1, 3) + loopsUntilBroken = random.randint(MIN_LOOPS_UNTIL_BROKEN, MAX_LOOPS_UNTIL_BROKEN) + self.brokenSensorData = [brokenSensorID, loopsUntilBroken, 0, 0, 0] + # elements at index 2, 3, and 4 are loop counter, last angle, and last speed + + if self.brokenSensorData[2] < self.brokenSensorData[1]: + self.brokenSensorData[2] += 1 + elif self.brokenSensorData[2] == self.brokenSensorData[1]: + self.brokenSensorData[2] += 1 + + # if counters are equal, store the current angle and speed of sensor about to break + self.brokenSensorData[3] = self.get_wind_measurement(data, self.brokenSensorData[0], "angle") + self.brokenSensorData[4] = self.get_wind_measurement(data, self.brokenSensorData[0], "speed") + + if(self.brokenSensorData[2] > self.brokenSensorData[1]): + self.corrupt_wind_sensor(data, self.brokenSensorData[0], "angle", self.brokenSensorData[3]) + self.corrupt_wind_sensor(data, self.brokenSensorData[0], "speed", self.brokenSensorData[4]) + + rospy.logwarn("MOCK wind_sensor_{} stopped updating measurements. ".format(self.brokenSensorData[0]) + + "Publishing corrupt data:\nangle stuck at {} degrees, ".format(self.brokenSensorData[3]) + + "speed stuck at {} knots".format(self.brokenSensorData[4])) + self.publisher.publish(data) + def corrupt_wind_sensor(self, data, brokenSensorNumber, speedOrAngle, value): + # Helper function for corrupting wind data. + # example: if brokenSensorNumber = 3, speedOrAngle = "speed", value = 25, then the function will write + # 25 to data.wind_sensor_3_speed_knots. + + if(speedOrAngle == "speed"): + if(brokenSensorNumber == 1): + data.wind_sensor_1_speed_knots = value + elif(brokenSensorNumber == 2): + data.wind_sensor_2_speed_knots = value + elif(brokenSensorNumber == 3): + data.wind_sensor_3_speed_knots = value + elif(speedOrAngle == "angle"): + if(brokenSensorNumber == 1): + data.wind_sensor_1_angle_degrees = value + elif(brokenSensorNumber == 2): + data.wind_sensor_2_angle_degrees = value + elif(brokenSensorNumber == 3): + data.wind_sensor_3_angle_degrees = value + + def get_wind_measurement(self, data, sensorNumber, speedOrAngle): + # Helper function for corrupting wind data. + # example: if sensorNumber = 3, speedOrAngle = "speed", then the function returns the speed of wind sensor 3 + if(speedOrAngle == "speed"): + if sensorNumber == 1: + return data.wind_sensor_1_speed_knots + elif sensorNumber == 2: + return data.wind_sensor_2_speed_knots + elif sensorNumber == 3: + return data.wind_sensor_3_speed_knots + elif speedOrAngle == "angle": + if sensorNumber == 1: + return data.wind_sensor_1_angle_degrees + elif sensorNumber == 2: + return data.wind_sensor_2_angle_degrees + elif sensorNumber == 3: + return data.wind_sensor_3_angle_degrees + 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. From 9ee466fe5d084f1f4f7a29dbc7a59ea0b329d949 Mon Sep 17 00:00:00 2001 From: Jamen Kaye <71156197+jamenkaye@users.noreply.github.com> Date: Sat, 27 Nov 2021 14:07:36 -0800 Subject: [PATCH 2/7] Update README.md Added a description of how to use `broken_wind_sensors` parameter. --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/README.md b/README.md index 16cfadd..7139b06 100644 --- a/README.md +++ b/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 wind sensor data should be corrupted to simulate broken sensors, and in what way the wind data should be corrupted. Default: `not_broken` (the default setting leaves wind 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 From 7e1a894b477a2365cf270555220d5c5f19a16d57 Mon Sep 17 00:00:00 2001 From: Jamen Kaye <71156197+jamenkaye@users.noreply.github.com> Date: Wed, 1 Dec 2021 23:02:07 -0800 Subject: [PATCH 3/7] Update README.md minor edit --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7139b06..2886842 100644 --- a/README.md +++ b/README.md @@ -61,7 +61,7 @@ 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 wind sensor data should be corrupted to simulate broken sensors, and in what way the wind data should be corrupted. Default: `not_broken` (the default setting leaves wind 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: +* `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. From fe3caf6952429a1cc1340ff9c6a1e9763a75fb2d Mon Sep 17 00:00:00 2001 From: Jamen Kaye Date: Thu, 2 Dec 2021 00:08:53 -0800 Subject: [PATCH 4/7] Addressed feedback, better documentation, small improvments --- python/MOCK_sensors.py | 200 ++++++++++++++++++++--------------------- 1 file changed, 98 insertions(+), 102 deletions(-) diff --git a/python/MOCK_sensors.py b/python/MOCK_sensors.py index 55a390a..cdc71ef 100755 --- a/python/MOCK_sensors.py +++ b/python/MOCK_sensors.py @@ -1,5 +1,4 @@ #!/usr/bin/env python -# from re import I import numpy as np import rospy import random @@ -24,12 +23,12 @@ START_GLOBAL_WIND_SPEED_KMPH = 10 STDEV_GPS = 0.00001 STDEV_WIND = 0.1 -MAX_BROKEN_WINDSPEED_KTS = 50 -MAX_ANGLE_ERROR = 60 -MAX_WINDSPEED_MULTIPLIER_ERROR = 0.5 -MAX_WINDSPEED_ADDER_ERROR = 5 -MIN_LOOPS_UNTIL_BROKEN = 30 -MAX_LOOPS_UNTIL_BROKEN = 100 +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: @@ -45,7 +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 = "uninitialized" + self.brokenSensorData = None # Setup ROS node inputs and outputs self.publisher = rospy.Publisher("sensors", msg.Sensors, queue_size=4) @@ -134,147 +133,144 @@ def publish(self): brokenWindSensorStatus = rospy.get_param('broken_wind_sensors', default="not_broken") if brokenWindSensorStatus != "not_broken": - rospy.logwarn("Broken Wind Sensor Status: " + brokenWindSensorStatus) + 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 - # brokenSensorData is used to store an integer 1, 2, or 3 representing which sensor is broken - if(self.brokenSensorData == "uninitialized"): - # set up brokenSensorData so publish(self) remembers which sensor is stuck at zero + 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, "angle", 0) - self.corrupt_wind_sensor(data, self.brokenSensorData, "speed", 0) + self.corrupt_wind_sensor(data, self.brokenSensorData, 0, 0) - rospy.logwarn("MOCK wind_sensor_{} publishing corrupt data: ".format(self.brokenSensorData) - + "angle and speed stuck at 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 - # brokenSensorData is a list containing: brokenSensorID, the constant angle, the constant direction. - if(self.brokenSensorData == "uninitialized"): + if(self.brokenSensorData is None): randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str) random.seed(randomSeed) - brokenSensorID = random.randint(1, 3) - brokenSensorAngle_deg = random.random() * 360 - brokenSensorSpeed_kts = random.random() * MAX_BROKEN_WINDSPEED_KTS - self.brokenSensorData = [brokenSensorID, brokenSensorAngle_deg, brokenSensorSpeed_kts] + 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[0], "angle", self.brokenSensorData[1]) - self.corrupt_wind_sensor(data, self.brokenSensorData[0], "speed", self.brokenSensorData[2]) + self.corrupt_wind_sensor(data, self.brokenSensorData['brokenSensorID'], + self.brokenSensorData['brokenSensorAngle_deg'], + self.brokenSensorData['brokenSensorSpeed_knots']) - rospy.logwarn("MOCK wind_sensor_{} ".format(self.brokenSensorData[0]) - + "publishing corrupt data:\nangle stuck at {} degrees, ".format(self.brokenSensorData[1]) - + "speed stuck at {} knots.".format(self.brokenSensorData[2])) + 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 == "uninitialized"): + if(self.brokenSensorData is None): randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str) random.seed(randomSeed) - brokenSensorID = random.randint(1, 3) - theta = (2*random.random() - 1) * MAX_ANGLE_ERROR - windspeedMultiplier = 1 + (2*random.random() - 1) * MAX_WINDSPEED_MULTIPLIER_ERROR - windspeedAdder = (2*random.random() - 1) * MAX_WINDSPEED_ADDER_ERROR - self.brokenSensorData = [brokenSensorID, theta, windspeedMultiplier, windspeedAdder] + 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 = self.get_wind_measurement(data, self.brokenSensorData[0], "angle") - actualSpeed = self.get_wind_measurement(data, self.brokenSensorData[0], "speed") + actualAngle, actualSpeed = self.get_wind_measurement(data, self.brokenSensorData['brokenSensorID']) - newAngle = actualAngle + self.brokenSensorData[1] - newSpeed = max(0, self.brokenSensorData[2] * actualSpeed + self.brokenSensorData[3]) + newAngle = actualAngle + self.brokenSensorData['theta'] + newSpeed = max(0, self.brokenSensorData['windspeedMultiplier'] * actualSpeed + + self.brokenSensorData['windspeedAdder']) - # Bound the newAngle to [0, 360) - if(newAngle < 0): + # 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[0], "angle", newAngle) - self.corrupt_wind_sensor(data, self.brokenSensorData[0], "speed", newSpeed) + self.corrupt_wind_sensor(data, self.brokenSensorData['brokenSensorID'], newAngle, newSpeed) - rospy.logwarn("MOCK wind_sensor_{} publishing corrupt data:\n".format(self.brokenSensorData[0]) - + "measured angle: (real angle) + ({}) degrees, ".format(self.brokenSensorData[1]) - + "measured speed: ({}) * (real speed) + ({}) knots".format(self.brokenSensorData[2], - self.brokenSensorData[3])) + 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 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 == "uninitialized"): + if(self.brokenSensorData is None): randomSeed = get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str) random.seed(randomSeed) - brokenSensorID = random.randint(1, 3) - loopsUntilBroken = random.randint(MIN_LOOPS_UNTIL_BROKEN, MAX_LOOPS_UNTIL_BROKEN) - self.brokenSensorData = [brokenSensorID, loopsUntilBroken, 0, 0, 0] - # elements at index 2, 3, and 4 are loop counter, last angle, and last speed + 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[2] < self.brokenSensorData[1]: - self.brokenSensorData[2] += 1 - elif self.brokenSensorData[2] == self.brokenSensorData[1]: - self.brokenSensorData[2] += 1 + 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']) - # if counters are equal, store the current angle and speed of sensor about to break - self.brokenSensorData[3] = self.get_wind_measurement(data, self.brokenSensorData[0], "angle") - self.brokenSensorData[4] = self.get_wind_measurement(data, self.brokenSensorData[0], "speed") + self.brokenSensorData['loopsSoFar'] += 1 + else: + self.corrupt_wind_sensor(data, self.brokenSensorData['brokenSensorID'], + self.brokenSensorData['lastAngle'], + self.brokenSensorData['lastSpeed']) - if(self.brokenSensorData[2] > self.brokenSensorData[1]): - self.corrupt_wind_sensor(data, self.brokenSensorData[0], "angle", self.brokenSensorData[3]) - self.corrupt_wind_sensor(data, self.brokenSensorData[0], "speed", self.brokenSensorData[4]) - - rospy.logwarn("MOCK wind_sensor_{} stopped updating measurements. ".format(self.brokenSensorData[0]) - + "Publishing corrupt data:\nangle stuck at {} degrees, ".format(self.brokenSensorData[3]) - + "speed stuck at {} knots".format(self.brokenSensorData[4])) - - self.publisher.publish(data) + 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, speedOrAngle, value): + def corrupt_wind_sensor(self, data, brokenSensorNumber, newAngle, newSpeed): # Helper function for corrupting wind data. - # example: if brokenSensorNumber = 3, speedOrAngle = "speed", value = 25, then the function will write - # 25 to data.wind_sensor_3_speed_knots. - - if(speedOrAngle == "speed"): - if(brokenSensorNumber == 1): - data.wind_sensor_1_speed_knots = value - elif(brokenSensorNumber == 2): - data.wind_sensor_2_speed_knots = value - elif(brokenSensorNumber == 3): - data.wind_sensor_3_speed_knots = value - elif(speedOrAngle == "angle"): - if(brokenSensorNumber == 1): - data.wind_sensor_1_angle_degrees = value - elif(brokenSensorNumber == 2): - data.wind_sensor_2_angle_degrees = value - elif(brokenSensorNumber == 3): - data.wind_sensor_3_angle_degrees = value - - def get_wind_measurement(self, data, sensorNumber, speedOrAngle): + # 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. - # example: if sensorNumber = 3, speedOrAngle = "speed", then the function returns the speed of wind sensor 3 - if(speedOrAngle == "speed"): - if sensorNumber == 1: - return data.wind_sensor_1_speed_knots - elif sensorNumber == 2: - return data.wind_sensor_2_speed_knots - elif sensorNumber == 3: - return data.wind_sensor_3_speed_knots - elif speedOrAngle == "angle": - if sensorNumber == 1: - return data.wind_sensor_1_angle_degrees - elif sensorNumber == 2: - return data.wind_sensor_2_angle_degrees - elif sensorNumber == 3: - return data.wind_sensor_3_angle_degrees + # 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, From ea4f0988535fe2496d3d2d8fedbf3fbe95d89ee6 Mon Sep 17 00:00:00 2001 From: Jamen Kaye <71156197+jamenkaye@users.noreply.github.com> Date: Sat, 15 Jan 2022 11:29:03 -0800 Subject: [PATCH 5/7] Update README.md Addresses feedback from @patrick-5546 to reword 'the chosen wind sensor' to convey that it is randomly selected not chosen. --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 2886842..26c4e82 100644 --- a/README.md +++ b/README.md @@ -61,11 +61,11 @@ 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`, +* `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" select one of the 3 wind sensors, and modify the sensor's measurements in the following ways: + * `stuck_at_zero`: The "randomly" selected sensor will measure a wind speed of 0 knots and a wind angle of 0 degrees. + * `stuck_at_rand_constant`: The "randomly" selected sensor will measure a "random" wind speed and a "random" angle. + * `stuck_at_last_value`: The "randomly" selected 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 "randomly" selected 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. From dd20187765fb931fd4293ae981dcf44ff2c9dfa4 Mon Sep 17 00:00:00 2001 From: Jamen Kaye Date: Sat, 15 Jan 2022 17:36:11 -0800 Subject: [PATCH 6/7] Addressed feedback, fixed int division error, small changes --- python/MOCK_sensors.py | 61 ++++++++++++++++++++++-------------------- 1 file changed, 32 insertions(+), 29 deletions(-) diff --git a/python/MOCK_sensors.py b/python/MOCK_sensors.py index cdc71ef..761483f 100755 --- a/python/MOCK_sensors.py +++ b/python/MOCK_sensors.py @@ -1,7 +1,6 @@ #!/usr/bin/env python import numpy as np import rospy -import random import sailbot_msg.msg as msg import geopy.distance @@ -25,7 +24,7 @@ STDEV_WIND = 0.1 MAX_BROKEN_WINDSPEED_KNOTS = 30 MAX_ANGLE_ERROR_DEG = 60 -MAX_WINDSPEED_MULTIPLIER_ERROR = 2 # Should be >= 1 +MAX_WINDSPEED_MULTIPLIER_ERROR = 1.5 # Should be >= 1 MAX_WINDSPEED_ADDER_ERROR_KNOTS = 5 MIN_LOOPS_TO_BREAK = 30 MAX_LOOPS_TO_BREAK = 100 @@ -141,13 +140,16 @@ def publish(self): def breakWindSensors(self, data, brokenWindSensorStatus): # Corrupts wind sensor data depending on the setting specified by string brokenSensorStatus + # Initialization code used by all types of brokenWindSensorStatus + if(self.brokenSensorData is None): + np.random.seed(get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str)) + brokenSensorID = np.random.randint(1, high=4) + 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.brokenSensorData = brokenSensorID self.corrupt_wind_sensor(data, self.brokenSensorData, 0, 0) @@ -158,12 +160,9 @@ def breakWindSensors(self, data, brokenWindSensorStatus): # 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.brokenSensorData = {'brokenSensorID': brokenSensorID, + 'brokenSensorAngle_deg': np.random.ranf() * 360, + 'brokenSensorSpeed_knots': np.random.ranf() * MAX_BROKEN_WINDSPEED_KNOTS} self.corrupt_wind_sensor(data, self.brokenSensorData['brokenSensorID'], self.brokenSensorData['brokenSensorAngle_deg'], @@ -178,30 +177,32 @@ def breakWindSensors(self, data, brokenWindSensorStatus): 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] + # the speed published is given by multiplier * (original speed) + adder + # (multiplier and adder are random constants) + # multiplier is within [1 / MAX_WINDSPEED_MULTIPLIER_ERROR, MAX_WINDSPEED_MULTIPLIER_ERROR] + # adder 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)) + if np.random.randint(1, high=3) < 2: + # multiplier will be between 1/MAX_WINDSPEED_MULTIPLIER_ERROR and 1 + multiplier = 1.0 - np.random.ranf() * (1.0 - 1.0 / MAX_WINDSPEED_MULTIPLIER_ERROR) else: - multiplier = 1 + random.random() * (MAX_WINDSPEED_MULTIPLIER_ERROR - 1) + # multiplier will be between 1 and MAX_WINDSPEED_MULTIPLIER_ERROR + multiplier = 1.0 + np.random.ranf() * (MAX_WINDSPEED_MULTIPLIER_ERROR - 1.0) - self.brokenSensorData = {'brokenSensorID': random.randint(1, 3), - 'theta': (2*random.random() - 1) * MAX_ANGLE_ERROR_DEG, + self.brokenSensorData = {'brokenSensorID': brokenSensorID, + 'theta': (2.0 * np.random.ranf() - 1.0) * MAX_ANGLE_ERROR_DEG, 'windspeedMultiplier': multiplier, - 'windspeedAdder': (2*random.random() - 1) * MAX_WINDSPEED_ADDER_ERROR_KNOTS} + 'windspeedAdder': (2.0 * np.random.ranf() - 1.0) + * 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']) + newSpeed = np.fabs(self.brokenSensorData['windspeedMultiplier'] * actualSpeed + + self.brokenSensorData['windspeedAdder']) # Bound the newAngle to [-180, 360) to improve interpretability if(newAngle < -180): @@ -223,11 +224,9 @@ def breakWindSensors(self, data, brokenWindSensorStatus): # 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), + self.brokenSensorData = {'brokenSensorID': brokenSensorID, + 'loopsUntilBroken': np.random.randint(MIN_LOOPS_TO_BREAK, + high=MAX_LOOPS_TO_BREAK), 'loopsSoFar': 0, 'lastAngle': None, 'lastSpeed': None} if self.brokenSensorData['loopsSoFar'] <= self.brokenSensorData['loopsUntilBroken']: @@ -247,6 +246,10 @@ def breakWindSensors(self, data, brokenWindSensorStatus): + "angle stuck at {} degrees, ".format(self.brokenSensorData['lastAngle']) + "speed stuck at {} knots".format(self.brokenSensorData['lastSpeed'])) + else: + rospy.logwarn_throttle(1.5, "Unknown 'broken_wind_sensors' argument: \"{}\".".format(brokenWindSensorStatus) + + " Wind sensor data unchanged.") + 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 From aaf0a6705b5cd4875b3cbb3d52e326435b8ea24d Mon Sep 17 00:00:00 2001 From: Jamen Kaye Date: Fri, 28 Jan 2022 12:11:36 -0800 Subject: [PATCH 7/7] addressed latest feedback --- python/MOCK_sensors.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/python/MOCK_sensors.py b/python/MOCK_sensors.py index 761483f..3e336d8 100755 --- a/python/MOCK_sensors.py +++ b/python/MOCK_sensors.py @@ -131,10 +131,15 @@ def publish(self): data = self.add_noise(data) brokenWindSensorStatus = rospy.get_param('broken_wind_sensors', default="not_broken") - if brokenWindSensorStatus != "not_broken": + if brokenWindSensorStatus in ["stuck_at_zero", "stuck_at_rand_constant", + "rand_const_error", "stuck_at_last_value"]: rospy.logwarn_throttle(30, "Broken Wind Sensor Status: " + brokenWindSensorStatus) self.breakWindSensors(data, brokenWindSensorStatus) + elif brokenWindSensorStatus != "not_broken": + rospy.logwarn_throttle(1.5, "Unknown 'broken_wind_sensors' argument: \"{}\".".format(brokenWindSensorStatus) + + " Wind sensor data unchanged.") + self.publisher.publish(data) def breakWindSensors(self, data, brokenWindSensorStatus): @@ -143,7 +148,7 @@ def breakWindSensors(self, data, brokenWindSensorStatus): # Initialization code used by all types of brokenWindSensorStatus if(self.brokenSensorData is None): np.random.seed(get_rosparam_or_default_if_invalid('random_seed', default=None, rosparam_type_cast=str)) - brokenSensorID = np.random.randint(1, high=4) + brokenSensorID = np.random.randint(1, 4) if (brokenWindSensorStatus == "stuck_at_zero"): # This setting chooses a wind sensor which reads an angle of 0 and speed 0 @@ -185,11 +190,11 @@ def breakWindSensors(self, data, brokenWindSensorStatus): if(self.brokenSensorData is None): - if np.random.randint(1, high=3) < 2: - # multiplier will be between 1/MAX_WINDSPEED_MULTIPLIER_ERROR and 1 + if np.random.randint(0, 2): + # 50% chance multiplier will be between 1/MAX_WINDSPEED_MULTIPLIER_ERROR and 1 multiplier = 1.0 - np.random.ranf() * (1.0 - 1.0 / MAX_WINDSPEED_MULTIPLIER_ERROR) else: - # multiplier will be between 1 and MAX_WINDSPEED_MULTIPLIER_ERROR + # 50% chance multiplier will be between 1 and MAX_WINDSPEED_MULTIPLIER_ERROR multiplier = 1.0 + np.random.ranf() * (MAX_WINDSPEED_MULTIPLIER_ERROR - 1.0) self.brokenSensorData = {'brokenSensorID': brokenSensorID, @@ -225,8 +230,7 @@ def breakWindSensors(self, data, brokenWindSensorStatus): if(self.brokenSensorData is None): self.brokenSensorData = {'brokenSensorID': brokenSensorID, - 'loopsUntilBroken': np.random.randint(MIN_LOOPS_TO_BREAK, - high=MAX_LOOPS_TO_BREAK), + 'loopsUntilBroken': np.random.randint(MIN_LOOPS_TO_BREAK, MAX_LOOPS_TO_BREAK), 'loopsSoFar': 0, 'lastAngle': None, 'lastSpeed': None} if self.brokenSensorData['loopsSoFar'] <= self.brokenSensorData['loopsUntilBroken']: @@ -246,10 +250,6 @@ def breakWindSensors(self, data, brokenWindSensorStatus): + "angle stuck at {} degrees, ".format(self.brokenSensorData['lastAngle']) + "speed stuck at {} knots".format(self.brokenSensorData['lastSpeed'])) - else: - rospy.logwarn_throttle(1.5, "Unknown 'broken_wind_sensors' argument: \"{}\".".format(brokenWindSensorStatus) - + " Wind sensor data unchanged.") - 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