diff --git a/README.md b/README.md index 16cfadd..26c4e82 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 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. #### Run a specific saved pathfinding scenario 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..3e336d8 100755 --- a/python/MOCK_sensors.py +++ b/python/MOCK_sensors.py @@ -22,6 +22,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 = 1.5 # Should be >= 1 +MAX_WINDSPEED_ADDER_ERROR_KNOTS = 5 +MIN_LOOPS_TO_BREAK = 30 +MAX_LOOPS_TO_BREAK = 100 class MOCK_SensorManager: @@ -37,6 +43,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 +129,152 @@ 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 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): + # 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, 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): + self.brokenSensorData = brokenSensorID + + 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): + 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'], + 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 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): + + 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: + # 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, + 'theta': (2.0 * np.random.ranf() - 1.0) * MAX_ANGLE_ERROR_DEG, + 'windspeedMultiplier': multiplier, + '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 = np.fabs(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): + self.brokenSensorData = {'brokenSensorID': brokenSensorID, + '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']: + 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.