From a40722a30c74205670e3ed9ff293eab8a4d21e84 Mon Sep 17 00:00:00 2001 From: patrick-5546 Date: Sun, 5 Dec 2021 14:41:33 -0800 Subject: [PATCH 1/5] MOCK_sensors.py publishes boat speed and direction in global frame --- python/MOCK_sensors.py | 44 +++++++++++++++++++++++++----------------- 1 file changed, 26 insertions(+), 18 deletions(-) diff --git a/python/MOCK_sensors.py b/python/MOCK_sensors.py index 4bd88fb..471f911 100755 --- a/python/MOCK_sensors.py +++ b/python/MOCK_sensors.py @@ -31,7 +31,9 @@ def __init__(self, startLat, startLon, startHeadingDegrees, startSpeedKmph, star self.lat = startLat self.lon = startLon self.headingDegrees = startHeadingDegrees - self.speedKmph = startSpeedKmph + self.trackMadeGoodDegrees = startHeadingDegrees # initialize to boat frame direction (correct when current = 0) + self.speedKmphBoatFrame = startSpeedKmph + self.speedKmphGlobalFrame = startSpeedKmph # initialize to boat frame speed (correct when current = 0) self.globalWindSpeedKmph = startGlobalWindSpeedKmph self.globalWindDirectionDegrees = startGlobalWindDirectionDegrees self.measuredWindSpeedKmph, self.measuredWindDirectionDegrees = globalWindToMeasuredWind( @@ -47,25 +49,16 @@ def __init__(self, startLat, startLon, startHeadingDegrees, startSpeedKmph, star def update(self): # TODO: Use rospy.get_param('smooth_changes', default=True) to use PID controller on heading and speed + + self.update_global_frame_fields() speedup = rospy.get_param('speedup', default=1.0) # Travel based on boat speed - kmTraveledPerPeriod = self.speedKmph * self.publishPeriodSeconds / 3600.0 + kmTraveledPerPeriod = self.speedKmphGlobalFrame * self.publishPeriodSeconds / 3600.0 kmTraveledPerPeriod *= speedup # Move greater distances with speedup distanceTraveled = geopy.distance.distance(kilometers=kmTraveledPerPeriod) destination = distanceTraveled.destination(point=(self.lat, self.lon), - bearing=headingToBearingDegrees(self.headingDegrees)) - self.lon = destination.longitude - self.lat = destination.latitude - - # Travel based on ocean current - oceanCurrentSpeedKmph = rospy.get_param('ocean_current_speed', default=0.0) - oceanCurrentDirectionDegress = rospy.get_param('ocean_current_direction', default=0.0) - oceanCurrentKmTraveledPerPeriod = oceanCurrentSpeedKmph * self.publishPeriodSeconds / 3600.0 - oceanCurrentKmTraveledPerPeriod *= speedup # Move greater distances with speedup - distanceTraveled = geopy.distance.distance(kilometers=oceanCurrentKmTraveledPerPeriod) - destination = distanceTraveled.destination(point=(self.lat, self.lon), - bearing=headingToBearingDegrees(oceanCurrentDirectionDegress)) + bearing=headingToBearingDegrees(self.trackMadeGoodDegrees)) self.lon = destination.longitude self.lat = destination.latitude @@ -76,6 +69,21 @@ def update(self): self.measuredWindSpeedKmph, self.measuredWindDirectionDegrees = globalWindToMeasuredWind( self.globalWindSpeedKmph, self.globalWindDirectionDegrees, self.speedKmph, self.headingDegrees) + def update_global_frame_fields(self): + '''Calculate boat speed and direction (track made good) in global frame as a function of speed and direction in + boat frame, and speed and direction of current + ''' + oceanCurrentSpeedKmph = rospy.get_param('ocean_current_speed', default=0.0) + oceanCurrentDirectionDegrees = rospy.get_param('ocean_current_direction', default=0.0) + + speedXGlobalFrame = oceanCurrentSpeedKmph * np.cos(np.deg2rad(oceanCurrentDirectionDegrees)) + \ + self.speedKmphBoatFrame * np.cos(np.deg2rad(self.headingDegrees)) + speedYGlobalFrame = oceanCurrentSpeedKmph * np.sin(np.deg2rad(oceanCurrentDirectionDegrees)) + \ + self.speedKmphBoatFrame * np.sin(np.deg2rad(self.headingDegrees)) + + self.speedKmphGlobalFrame = np.hypot(speedXGlobalFrame, speedYGlobalFrame) + self.trackMadeGoodDegrees = np.arctan2(speedYGlobalFrame, speedXGlobalFrame) + def publish(self): # Populate sensors, leaving commented out the ones we don't know for now data = msg.Sensors() @@ -92,8 +100,8 @@ def publish(self): # data.gps_can_timestamp_utc data.gps_can_latitude_degrees = self.lat data.gps_can_longitude_degrees = self.lon - data.gps_can_groundspeed_knots = self.speedKmph / KNOTS_TO_KMPH - data.gps_can_track_made_good_degrees = headingToBearingDegrees(self.headingDegrees) + data.gps_can_groundspeed_knots = self.speedKmphGlobalFrame / KNOTS_TO_KMPH + data.gps_can_track_made_good_degrees = headingToBearingDegrees(self.trackMadeGoodDegrees) data.gps_can_true_heading_degrees = headingToBearingDegrees(self.headingDegrees) # data.gps_can_magnetic_variation_degrees # data.gps_can_state @@ -101,8 +109,8 @@ def publish(self): # data.gps_ais_timestamp_utc data.gps_ais_latitude_degrees = self.lat data.gps_ais_longitude_degrees = self.lon - data.gps_ais_groundspeed_knots = self.speedKmph / KNOTS_TO_KMPH - data.gps_ais_track_made_good_degrees = headingToBearingDegrees(self.headingDegrees) + data.gps_ais_groundspeed_knots = self.speedKmphGlobalFrame / KNOTS_TO_KMPH + data.gps_ais_track_made_good_degrees = headingToBearingDegrees(self.trackMadeGoodDegrees) data.gps_ais_true_heading_degrees = headingToBearingDegrees(self.headingDegrees) # data.gps_ais_magnetic_variation_degrees # data.gps_ais_state From 4e95b9956bdc6474614e5eb8a552b7999e5c7d09 Mon Sep 17 00:00:00 2001 From: patrick-5546 Date: Sun, 5 Dec 2021 15:52:17 -0800 Subject: [PATCH 2/5] Incorporate track made good in global and measured wind conversions --- python/MOCK_sensors.py | 8 +++++--- python/ros_interface.py | 8 ++++++-- python/utilities.py | 34 +++++++++++++++++++++------------- 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/python/MOCK_sensors.py b/python/MOCK_sensors.py index 471f911..952f367 100755 --- a/python/MOCK_sensors.py +++ b/python/MOCK_sensors.py @@ -37,7 +37,8 @@ def __init__(self, startLat, startLon, startHeadingDegrees, startSpeedKmph, star self.globalWindSpeedKmph = startGlobalWindSpeedKmph self.globalWindDirectionDegrees = startGlobalWindDirectionDegrees self.measuredWindSpeedKmph, self.measuredWindDirectionDegrees = globalWindToMeasuredWind( - self.globalWindSpeedKmph, self.globalWindDirectionDegrees, self.speedKmph, self.headingDegrees) + self.globalWindSpeedKmph, self.globalWindDirectionDegrees, self.speedKmphGlobalFrame, self.headingDegrees, + self.trackMadeGoodDegrees) self.publishPeriodSeconds = SENSORS_PUBLISH_PERIOD_SECONDS # Setup ROS node inputs and outputs @@ -67,7 +68,8 @@ def update(self): self.globalWindDirectionDegrees = rospy.get_param('global_wind_direction_degrees', default=self.globalWindDirectionDegrees) self.measuredWindSpeedKmph, self.measuredWindDirectionDegrees = globalWindToMeasuredWind( - self.globalWindSpeedKmph, self.globalWindDirectionDegrees, self.speedKmph, self.headingDegrees) + self.globalWindSpeedKmph, self.globalWindDirectionDegrees, self.speedKmphGlobalFrame, self.headingDegrees, + self.trackMadeGoodDegrees) def update_global_frame_fields(self): '''Calculate boat speed and direction (track made good) in global frame as a function of speed and direction in @@ -82,7 +84,7 @@ def update_global_frame_fields(self): self.speedKmphBoatFrame * np.sin(np.deg2rad(self.headingDegrees)) self.speedKmphGlobalFrame = np.hypot(speedXGlobalFrame, speedYGlobalFrame) - self.trackMadeGoodDegrees = np.arctan2(speedYGlobalFrame, speedXGlobalFrame) + self.trackMadeGoodDegrees = np.rad2deg(np.arctan2(speedYGlobalFrame, speedXGlobalFrame)) def publish(self): # Populate sensors, leaving commented out the ones we don't know for now diff --git a/python/ros_interface.py b/python/ros_interface.py index 41e13d6..344f0c2 100755 --- a/python/ros_interface.py +++ b/python/ros_interface.py @@ -25,7 +25,7 @@ def __init__(self): rospy.Subscriber("sensors", Sensors, self.sensorsCallback) self.pubMeasuredWind = rospy.Publisher('windSensor', windSensor, queue_size=4) self.pubGlobalWind = rospy.Publisher('global_wind', globalWind, queue_size=4) - self.pubGPS = rospy.Publisher('GPS', GPS, queue_size=4) + self.pubGPS = rospy.Publisher('GPS', GPS, queue_size=4) # TODO: add trackMadeGoodDegrees field to GPS self.initialized = False self.data = None @@ -33,6 +33,7 @@ def __init__(self): self.gps_lat_decimalDegrees = None self.gps_lon_decimalDegrees = None self.gps_headingDegrees = None + self.gps_trackMadeGoodDegrees = None self.gps_speedKmph = None self.measured_wind_speedKmph = None self.measured_wind_direction = None @@ -64,6 +65,7 @@ def translate(self): self.gps_lat_decimalDegrees = self.data.gps_can_latitude_degrees self.gps_lon_decimalDegrees = self.data.gps_can_longitude_degrees self.gps_headingDegrees = bearingToHeadingDegrees(self.data.gps_can_true_heading_degrees) + self.gps_trackMadeGoodDegrees = bearingToHeadingDegrees(self.data.gps_can_track_made_good_degrees) self.gps_speedKmph = self.data.gps_can_groundspeed_knots * KNOTS_TO_KMPH # Wind sensor fields - sensors 1, 2, and 3 -> use sensor 1 @@ -74,6 +76,7 @@ def translate(self): rospy.loginfo('gps_lat_decimalDegrees = {}'.format(self.gps_lat_decimalDegrees)) rospy.loginfo('gps_lon_decimalDegrees = {}'.format(self.gps_lon_decimalDegrees)) rospy.loginfo('gps_headingDegrees = {}'.format(self.gps_headingDegrees)) + rospy.loginfo('gps_trackMadeGoodDegrees = {}'.format(self.gps_trackMadeGoodDegrees)) rospy.loginfo('gps_speedKmph = {}'.format(self.gps_speedKmph)) rospy.loginfo('measured_wind_speedKmph = {}'.format(self.measured_wind_speedKmph)) rospy.loginfo('measured_wind_direction = {}'.format(self.measured_wind_direction)) @@ -83,7 +86,8 @@ def get_global_wind(self): measuredWindSpeed=self.measured_wind_speedKmph, measuredWindDirectionDegrees=self.measured_wind_direction, boatSpeed=self.gps_speedKmph, - headingDegrees=self.gps_headingDegrees) + headingDegrees=self.gps_headingDegrees, + trackMadeGoodDegrees=self.gps_trackMadeGoodDegrees) return globalWind(directionDegrees=direction, speedKmph=speed) diff --git a/python/utilities.py b/python/utilities.py index 1ae79e0..fefa821 100644 --- a/python/utilities.py +++ b/python/utilities.py @@ -217,31 +217,35 @@ def getLOSHeadingDegrees(position, previousWaypoint, currentWaypoint): return math.degrees(straight_course - math.atan(Kp * crosstrack_error)) -def measuredWindToGlobalWind(measuredWindSpeed, measuredWindDirectionDegrees, boatSpeed, headingDegrees): +def measuredWindToGlobalWind(measuredWindSpeed, measuredWindDirectionDegrees, boatSpeed, headingDegrees, + trackMadeGoodDegrees): '''Calculate the global wind based on the measured wind and the boat velocity + Args: measuredWindSpeed (float): speed of the wind measured from the boat. All speed values must be in the same units. measuredWindDirectionDegrees (float): angle of the measured with wrt the boat. 0 degrees is wind blowing to the right. 90 degrees is wind blowing forward. - boatSpeed (float): speed of the boat + boatSpeed (float): speed of the boat in the global frame headingDegrees (float): angle of the boat in global frame. 0 degrees is East. 90 degrees is North. + trackMadeGoodDegrees (float): angle of the boat speed in global frame. 0 degrees is East. 90 degrees is North. + Returns: float, float pair representing the globalWindSpeed (same units as input speed), globalWindDirectionDegrees ''' - measuredWindRadians, headingRadians = math.radians(measuredWindDirectionDegrees), math.radians(headingDegrees) + measuredWindRadians = math.radians(measuredWindDirectionDegrees) + headingRadians = math.radians(headingDegrees) + trackMadeGoodRadians = math.radians(trackMadeGoodDegrees) # GF = global frame. BF = boat frame # Calculate wind speed in boat frame. X is right. Y is forward. measuredWindSpeedXBF = measuredWindSpeed * math.cos(measuredWindRadians) measuredWindSpeedYBF = measuredWindSpeed * math.sin(measuredWindRadians) - # Assume boat is moving entirely in heading direction, so all boat speed is in boat frame Y direction - trueWindSpeedXBF = measuredWindSpeedXBF - trueWindSpeedYBF = measuredWindSpeedYBF + boatSpeed - # Calculate wind speed in global frame. X is EAST. Y is NORTH. - trueWindSpeedXGF = trueWindSpeedXBF * math.sin(headingRadians) + trueWindSpeedYBF * math.cos(headingRadians) - trueWindSpeedYGF = trueWindSpeedYBF * math.sin(headingRadians) - trueWindSpeedXBF * math.cos(headingRadians) + trueWindSpeedXGF = measuredWindSpeedXBF * math.sin(headingRadians) + \ + measuredWindSpeedYBF * math.cos(headingRadians) + boatSpeed * math.cos(trackMadeGoodRadians) + trueWindSpeedYGF = measuredWindSpeedYBF * math.sin(headingRadians) - \ + measuredWindSpeedXBF * math.cos(headingRadians) + boatSpeed * math.sin(trackMadeGoodRadians) # Calculate global wind speed and direction globalWindSpeed = (trueWindSpeedXGF**2 + trueWindSpeedYGF**2)**0.5 @@ -250,7 +254,8 @@ def measuredWindToGlobalWind(measuredWindSpeed, measuredWindDirectionDegrees, bo return globalWindSpeed, globalWindDirectionDegrees -def globalWindToMeasuredWind(globalWindSpeed, globalWindDirectionDegrees, boatSpeed, headingDegrees): +def globalWindToMeasuredWind(globalWindSpeed, globalWindDirectionDegrees, boatSpeed, headingDegrees, + trackMadeGoodDegrees): '''Calculate the measured wind based on the global wind and the boat velocity Args: @@ -259,17 +264,20 @@ def globalWindToMeasuredWind(globalWindSpeed, globalWindDirectionDegrees, boatSp 0 degrees is East. 90 degrees is North. boatSpeed (float): speed of the boat headingDegrees (float): angle of the boat in global frame. 0 degrees is East. 90 degrees is North. + trackMadeGoodDegrees (float): angle of the boat speed in global frame. 0 degrees is East. 90 degrees is North. Returns: float, float pair representing the measuredWindSpeed (same units as input speed), measuredWindDirectionDegrees (0 degrees is wind blowing to the right. 90 degrees is wind blowing forward) ''' - globalWindRadians, headingRadians = math.radians(globalWindDirectionDegrees), math.radians(headingDegrees) + globalWindRadians = math.radians(globalWindDirectionDegrees) + headingRadians = math.radians(headingDegrees) + trackMadeGoodRadians = math.radians(trackMadeGoodDegrees) # GF = global frame. BF = boat frame # Calculate the measuredWindSpeed in the global frame - measuredWindXGF = globalWindSpeed * math.cos(globalWindRadians) - boatSpeed * math.cos(headingRadians) - measuredWindYGF = globalWindSpeed * math.sin(globalWindRadians) - boatSpeed * math.sin(headingRadians) + measuredWindXGF = globalWindSpeed * math.cos(globalWindRadians) - boatSpeed * math.cos(trackMadeGoodRadians) + measuredWindYGF = globalWindSpeed * math.sin(globalWindRadians) - boatSpeed * math.sin(trackMadeGoodRadians) # Calculated the measuredWindSpeed in the boat frame measuredWindXBF = measuredWindXGF * math.sin(headingRadians) - measuredWindYGF * math.cos(headingRadians) From a6dc00e85793bdc5732d1bc70eec1361721e8748 Mon Sep 17 00:00:00 2001 From: patrick-5546 Date: Sun, 5 Dec 2021 16:46:01 -0800 Subject: [PATCH 3/5] Update tests --- test/test_utilities.py | 53 +++++++++++++++++++++++++++++++++--------- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/test/test_utilities.py b/test/test_utilities.py index c36f4a5..83bc8eb 100755 --- a/test/test_utilities.py +++ b/test/test_utilities.py @@ -123,7 +123,7 @@ def test_measuredWindToGlobalWind_basic(self): # (Boat moving + no measured wind) => (global wind velocity == boat velocity) globalWindSpeedKmph, globalWindDirectionDegrees = utils.measuredWindToGlobalWind( measuredWindSpeed=0, measuredWindDirectionDegrees=utils.BOAT_RIGHT, boatSpeed=1, - headingDegrees=utils.HEADING_NORTH) + headingDegrees=utils.HEADING_NORTH, trackMadeGoodDegrees=utils.HEADING_NORTH) self.assertAlmostEqual(1, globalWindSpeedKmph, places=3) self.assertAlmostEqual(utils.HEADING_NORTH, globalWindDirectionDegrees, places=3) @@ -133,29 +133,45 @@ def test_measuredWindToGlobalWind_basic(self): measuredDirection = (2 * utils.BOAT_FORWARD + 1 * utils.BOAT_RIGHT) / 3 # 60 degrees globalWindSpeedKmph, globalWindDirectionDegrees = utils.measuredWindToGlobalWind( measuredWindSpeed=1.2, measuredWindDirectionDegrees=measuredDirection, boatSpeed=0, - headingDegrees=utils.HEADING_EAST) + headingDegrees=utils.HEADING_EAST, trackMadeGoodDegrees=utils.HEADING_EAST) self.assertAlmostEqual(1.2, globalWindSpeedKmph, places=3) self.assertAlmostEqual(measuredDirection - 90, globalWindDirectionDegrees, places=3) # (Boat and measured wind along the same axis) => (global wind velocity == boat + measured wind velocity) globalWindSpeedKmph, globalWindDirectionDegrees = utils.measuredWindToGlobalWind( - measuredWindSpeed=5, measuredWindDirectionDegrees=utils.BOAT_FORWARD, boatSpeed=7, headingDegrees=90) + measuredWindSpeed=5, measuredWindDirectionDegrees=utils.BOAT_FORWARD, boatSpeed=7, headingDegrees=90, + trackMadeGoodDegrees=90) self.assertAlmostEqual(12, globalWindSpeedKmph, places=3) self.assertAlmostEqual(90, globalWindDirectionDegrees, places=3) + # (Boat moving backwards, measured wind is opposite of heading) => + # (global wind velocity == boat + measured wind velocity, global wind is same direction as boat movement) + globalWindSpeedKmph, globalWindDirectionDegrees = utils.measuredWindToGlobalWind( + measuredWindSpeed=5, measuredWindDirectionDegrees=utils.BOAT_BACKWARD, boatSpeed=5, + headingDegrees=utils.HEADING_EAST, trackMadeGoodDegrees=utils.HEADING_WEST) + self.assertAlmostEqual(10, globalWindSpeedKmph, places=3) + self.assertAlmostEqual(utils.HEADING_WEST, globalWindDirectionDegrees, places=3) + def test_measuredWindToGlobalWind_advanced(self): # Setup test parameters globalWindSpeedKmph, globalWindDirectionDegrees = utils.measuredWindToGlobalWind( - measuredWindSpeed=2, measuredWindDirectionDegrees=10, boatSpeed=4, headingDegrees=20) + measuredWindSpeed=2, measuredWindDirectionDegrees=10, boatSpeed=4, headingDegrees=20, + trackMadeGoodDegrees=20) self.assertAlmostEqual(4.7726691528, globalWindSpeedKmph, places=3) self.assertAlmostEqual(-4.373700424, globalWindDirectionDegrees, places=3) + globalWindSpeedKmph, globalWindDirectionDegrees = utils.measuredWindToGlobalWind( + measuredWindSpeed=1, measuredWindDirectionDegrees=60, boatSpeed=10, headingDegrees=45, + trackMadeGoodDegrees=40) + self.assertAlmostEqual(10.914492921832556, globalWindSpeedKmph, places=3) + self.assertAlmostEqual(37.78090491698471, globalWindDirectionDegrees, places=3) + def test_globalWindToMeasuredWind_basic(self): # (Boat moving + no global wind) => (measured wind speed == boat speed) # + (measured wind dir == -boat dir == -utils.BOAT_FORWARD) measuredWindSpeedKmph, measuredWindDirectionDegrees = utils.globalWindToMeasuredWind( globalWindSpeed=0, globalWindDirectionDegrees=utils.HEADING_EAST, boatSpeed=2, - headingDegrees=utils.HEADING_EAST) + headingDegrees=utils.HEADING_EAST, trackMadeGoodDegrees=utils.HEADING_EAST) self.assertAlmostEqual(2, measuredWindSpeedKmph, places=3) self.assertAlmostEqual((utils.BOAT_FORWARD + 180) % 360, measuredWindDirectionDegrees % 360, places=3) @@ -165,7 +181,7 @@ def test_globalWindToMeasuredWind_basic(self): globalDirection = (2 * utils.HEADING_NORTH + 1 * utils.HEADING_EAST) / 3 # 60 degrees measuredWindSpeedKmph, measuredWindDirectionDegrees = utils.globalWindToMeasuredWind( globalWindSpeed=10, globalWindDirectionDegrees=globalDirection, boatSpeed=0, - headingDegrees=utils.HEADING_EAST) + headingDegrees=utils.HEADING_EAST, trackMadeGoodDegrees=utils.HEADING_EAST) self.assertAlmostEqual(10, measuredWindSpeedKmph, places=3) self.assertAlmostEqual((globalDirection + 90) % 360, measuredWindDirectionDegrees % 360, places=3) @@ -173,30 +189,45 @@ def test_globalWindToMeasuredWind_basic(self): # (measured wind == global wind - boat speed) + (measured wind dir along boat forward) measuredWindSpeedKmph, measuredWindDirectionDegrees = utils.globalWindToMeasuredWind( globalWindSpeed=10, globalWindDirectionDegrees=utils.HEADING_SOUTH, boatSpeed=3, - headingDegrees=utils.HEADING_SOUTH) + headingDegrees=utils.HEADING_SOUTH, trackMadeGoodDegrees=utils.HEADING_SOUTH) self.assertAlmostEqual(10 - 3, measuredWindSpeedKmph, places=3) self.assertAlmostEqual(utils.BOAT_FORWARD, measuredWindDirectionDegrees % 360, places=3) + # (Boat moving backwards, global wind in same direction) => + # (measured wind velocity = global wind velocity - boat, measured wind direction = opposite of heading) + measuredWindSpeedKmph, measuredWindDirectionDegrees = utils.globalWindToMeasuredWind( + globalWindSpeed=10, globalWindDirectionDegrees=utils.HEADING_EAST, boatSpeed=5, + headingDegrees=utils.HEADING_WEST, trackMadeGoodDegrees=utils.HEADING_EAST) + self.assertAlmostEqual(5, measuredWindSpeedKmph, places=3) + self.assertAlmostEqual(-90, measuredWindDirectionDegrees, places=3) + def test_globalWindToMeasuredWind_advanced(self): # Setup test parameters measuredWindSpeedKmph, measuredWindDirectionDegrees = utils.globalWindToMeasuredWind( globalWindSpeed=11, globalWindDirectionDegrees=12, boatSpeed=14, - headingDegrees=120) - + headingDegrees=120, trackMadeGoodDegrees=120) self.assertAlmostEqual(20.30214851358062, measuredWindSpeedKmph, places=3) self.assertAlmostEqual(-58.98273894650611, measuredWindDirectionDegrees, places=3) + measuredWindSpeedKmph, measuredWindDirectionDegrees = utils.globalWindToMeasuredWind( + globalWindSpeed=5, globalWindDirectionDegrees=12, boatSpeed=14, + headingDegrees=30, trackMadeGoodDegrees=210) + self.assertAlmostEqual(18.818818036245567, measuredWindSpeedKmph, places=3) + self.assertAlmostEqual(85.2905326169815, measuredWindDirectionDegrees, places=3) + def test_globalWindToMeasuredWind_and_measuredWindToGlobalWind(self): # Setup test parameters headingDegrees = 150 + trackMadeGoodDegrees = 100 boatSpeedKmph = 14 globalWindDirectionDegrees = 78 globalWindSpeedKmph = 13 measuredWindSpeedKmph, measuredWindDirectionDegrees = utils.globalWindToMeasuredWind( globalWindSpeed=globalWindSpeedKmph, globalWindDirectionDegrees=globalWindDirectionDegrees, - boatSpeed=boatSpeedKmph, headingDegrees=headingDegrees) + boatSpeed=boatSpeedKmph, headingDegrees=headingDegrees, trackMadeGoodDegrees=trackMadeGoodDegrees) calculatedGlobalWindSpeedKmph, calculatedGlobalWindDirectionDegrees = utils.measuredWindToGlobalWind( - measuredWindSpeedKmph, measuredWindDirectionDegrees, boatSpeedKmph, headingDegrees) + measuredWindSpeedKmph, measuredWindDirectionDegrees, boatSpeedKmph, headingDegrees, + trackMadeGoodDegrees=trackMadeGoodDegrees) # Test that we get back the same global wind as we started with self.assertAlmostEqual(calculatedGlobalWindSpeedKmph, globalWindSpeedKmph, places=3) From 65a71e4082fae18b5d999ef2c71b8de34c92f65d Mon Sep 17 00:00:00 2001 From: patrick-5546 Date: Tue, 7 Dec 2021 12:17:44 -0800 Subject: [PATCH 4/5] Publish trackMadeGoodDegrees field to GPS topic --- python/ros_interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/ros_interface.py b/python/ros_interface.py index 344f0c2..71d42b4 100755 --- a/python/ros_interface.py +++ b/python/ros_interface.py @@ -25,7 +25,7 @@ def __init__(self): rospy.Subscriber("sensors", Sensors, self.sensorsCallback) self.pubMeasuredWind = rospy.Publisher('windSensor', windSensor, queue_size=4) self.pubGlobalWind = rospy.Publisher('global_wind', globalWind, queue_size=4) - self.pubGPS = rospy.Publisher('GPS', GPS, queue_size=4) # TODO: add trackMadeGoodDegrees field to GPS + self.pubGPS = rospy.Publisher('GPS', GPS, queue_size=4) self.initialized = False self.data = None @@ -47,7 +47,7 @@ def pub(self): if self.initialized: self.translate() self.pubGPS.publish(self.gps_lat_decimalDegrees, self.gps_lon_decimalDegrees, - self.gps_headingDegrees, self.gps_speedKmph) + self.gps_headingDegrees, self.gps_trackMadeGoodDegrees, self.gps_speedKmph) self.pubMeasuredWind.publish(self.measured_wind_direction, self.measured_wind_speedKmph) self.pubGlobalWind.publish(self.get_global_wind()) From 6485f87b5175766acdb4ea02c439bb57eb2eb4fc Mon Sep 17 00:00:00 2001 From: Patrick Creighton Date: Tue, 11 Jan 2022 22:06:53 -0800 Subject: [PATCH 5/5] Update more tests --- test/test_sailbot.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/test_sailbot.py b/test/test_sailbot.py index b14ac8a..676755d 100755 --- a/test/test_sailbot.py +++ b/test/test_sailbot.py @@ -34,7 +34,7 @@ def setUp(self): def test_basic(self): # Setup messages - gpsMsg = msg.GPS(10.1, 4.2, 60.3, 12.4) + gpsMsg = msg.GPS(10.1, 4.2, 60.3, 60.3, 12.4) globalWindMsg = msg.globalWind(87.5, 1.89) ships = [msg.AISShip(i, i, i, i, i) for i in range(10)] AISMsg = msg.AISMsg(ships) @@ -61,7 +61,7 @@ def test_basic(self): def test_missing_data(self): # Setup messages - gpsMsg = msg.GPS(10.1, 4.2, 60.3, 12.4) + gpsMsg = msg.GPS(10.1, 4.2, 60.3, 60.3, 12.4) globalWindMsg = msg.globalWind(87.5, 1.89) ships = [msg.AISShip(i, i, i, i, i) for i in range(10)] AISMsg = msg.AISMsg(ships) @@ -99,7 +99,7 @@ def test_missing_data(self): def test_globalPath_detailed(self): # Setup non global path messages - gpsMsg = msg.GPS(10.1, 4.2, 60.3, 12.4) + gpsMsg = msg.GPS(10.1, 4.2, 60.3, 60.3, 12.4) globalWindMsg = msg.globalWind(87.5, 1.89) ships = [msg.AISShip(i, i, i, i, i) for i in range(10)] AISMsg = msg.AISMsg(ships)