Separate track made good and heading

python/MOCK_sensors.py

@@ -31,11 +31,14 @@ 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(
-            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
@@ -47,25 +50,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
 
@@ -74,7 +68,23 @@ 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
+        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.rad2deg(np.arctan2(speedYGlobalFrame, speedXGlobalFrame))
 
     def publish(self):
         # Populate sensors, leaving commented out the ones we don't know for now
@@ -92,17 +102,17 @@ 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
 
         # 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

python/ros_interface.py

@@ -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
@@ -46,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())
 
@@ -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)
 
 

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)

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)