Merge pull request #73 from rsx-utoronto/feature/gps_to_pose

takes two gps navSatFIx messages and publishes pose data based off it

rover/autonomy/scripts/gps/functions.py

@@ -0,0 +1,76 @@
+#!/usr/bin/python3
+"""
+This script contains the following functions:
+    getHeadingBetweenGPSCoordinates()
+    getDistanceBetweenGPSCoordinates()
+    getAddedAngles()
+    eulerToQuaternion()
+"""
+from math import cos, sin, atan2, pi, degrees, radians
+from geopy import distance
+from typing import Tuple
+
+
+def getHeadingBetweenGPSCoordinates(lat1: float, long1: float, lat2: float, long2: float) -> float:
+    """
+    This function takes as input the latitude and longitude of two coordinates (1 and 2) and returns
+    the heading (in radians range -pi to pi, with North as 0.0) from the first coordinate to the second. Assumes lat and
+    long values are in decimal degrees.
+    """
+    # converting our degree inputs to radians for calculation
+    la1 = radians(lat1)
+    la2 = radians(lat2)
+    lo1 = radians(long1)
+    lo2 = radians(long2)
+    # calculate heading
+    deltaLo = lo2 - lo1
+    y = cos(la2)*sin(deltaLo)
+    x = cos(la1)*sin(la2) - sin(la1)*cos(la2)*cos(deltaLo)
+    heading = atan2(y,x)
+    return heading
+
+
+def getDistanceBetweenGPSCoordinates(latLong1: Tuple[float, float], latLong2: Tuple[float, float]) -> float:
+    """
+    This function takes as input the latitude and longitude of two coordinates and returns
+    the absolute distance (in meters) from the first coordinate to the second. Assumes lat and
+    long values are in decimal degrees.
+    """
+    # we use the geopy library to get the distance in km
+    dist = distance.distance(latLong1, latLong2).km
+    # we convert to meters
+    dist = dist*1000
+    return dist
+
+
+def getAddedAngles(theta, phi):
+    """
+    Given 2 angles (in range -pi to pi) we add them and return the new angle in range (-pi, pi].
+    """
+    # first add then add pi since they were originally in the range (-pi,pi] and we need (o, 2pi] 
+    comb = theta + phi + pi
+    # next we get the result modulo 2pi and subtract pi to get the angle in the (-pi, pi] range
+    comb = (comb % (2*pi)) - pi
+    # note that because of how modulo works angles that should be pi will be -pi so we make a catch for that
+    if comb == -pi:
+        comb = pi
+    return comb
+
+
+def eulerToQuaternion(roll: float, pitch: float, yaw: float) -> Tuple[float,float,float,float]:
+    """
+    Given the roll, pitch, and yaw angles, we calculate and return the quaternion for it 
+    """
+    # we first calculate the parts for the conversion to quaternion to make it easier to read
+    sinRoll = sin(roll/2)
+    cosRoll = cos(roll/2)
+    sinPitch = sin(pitch/2)
+    cosPitch = cos(pitch/2)
+    sinYaw = sin(yaw/2)
+    cosYaw = cos(yaw/2)
+    # now we calculate the parts of the quaternion
+    qx = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw
+    qy = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw
+    qz = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw
+    qw = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw
+    return (qx,qy,qz,qw)

rover/autonomy/scripts/gps/gps_to_pose.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python3
+import rospy
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import PoseStamped
+from math import atan2, pi, sin, cos
+import functions
+import message_filters
+from calian_gnss_ros2_msg.msg import GnssSignalStatus
+
+
+# important constants, make sure these are accurate to the current state of the rover (read below description)
+# doens't matter which antenna is 1 or 2, the positive y axis is the direction of the front of the rover and
+# the positive x axis is to the right, give the units don't matter as long as you are consitent for all
+X_POS_ANTENNA_1 = -1.0
+Y_POS_ANTENNA_1 = 0.0
+X_POS_ANTENNA_2 = 0.0
+Y_POS_ANTENNA_2 = 0.0
+# the NavSatFix publishers should correspond to the same numbered antenna from the position above
+GPS_ANTENNA_1 = "calian_gnss/gps_extended"
+GPS_ANTENNA_2 = "calian_gnss/base_gps_extended"
+
+
+# our heading is calculated as the direction from antenna 1 to 2, so we 
+# need to find the correction angle that places the heading as the direction the rover is moving by
+# using the positions of the antennas
+# we first get the heading from antenna 1 to 2, note atan2 returns the direction from 1 to 2 in (-pi, pi]
+# scale so if antenna 1 is in the middle then 2 being directly up returns pi/2 or directly right returns 0
+# for example
+ORIG_HEADING = atan2(Y_POS_ANTENNA_2-Y_POS_ANTENNA_1, X_POS_ANTENNA_2-X_POS_ANTENNA_1)
+# we know (since we mandated it above) that the front of the rover would be in direction x=0,y=1 which
+# using the same scale as above puts the heading at pi/2, if the calculated heading between antenna is not
+# this then we need to correct any given angle by the difference (note pi is added first to simiplify there
+# being positive and negative values for ORIG_HEADING)
+ANGLE_CORRECTION = pi/2 - ORIG_HEADING
+# the above angle correct means if we calculate the heading from antenna 1 to 2, then add the correction
+# we get the heading of the direction of the front of the rover
+
+
+class GPSToPose: 
+
+    def __init__(self):
+        self.origin_coordinates = None
+        self.gps1 = message_filters.Subscriber(GPS_ANTENNA_1, GnssSignalStatus)
+        self.gps2 = message_filters.Subscriber(GPS_ANTENNA_2, GnssSignalStatus)
+        # here 5 is the size of the queue and 0.2 is the time allowed between messages
+        self.ts = message_filters.ApproximateTimeSynchronizer([self.gps1, self.gps2], 5, 0.2)
+        self.ts.registerCallback(self.callback)
+        self.pose_pub = rospy.Publisher('pose', PoseStamped, queue_size=1)
+        self.msg = PoseStamped()
+
+    def callback(self, gps1, gps2):
+        # first we get the most recent longitudes and latitudes
+        lat1 = gps1.latitude
+        long1 = gps1.longitude
+        lat2 = gps2.latitude
+        long2 = gps2.longitude
+
+        # let's first calculate our heading
+        heading = functions.getHeadingBetweenGPSCoordinates(lat1, long1, lat2, long2)
+        # now we apply the angle correction so its the heading towards the front of the rover
+        heading = functions.getAddedAngles(heading, ANGLE_CORRECTION)
+        # now we convert our angle to a quaternion for our pose data
+        qx,qy,qz,qw = functions.eulerToQuaternion(0.0, 0.0, heading)
+
+        # now let's get our coordinate
+        # if the value for origin_coordinates is None then this is the first callback and we set the current
+        # current gps location as the origin
+        # note that we consider north (or 0.0 as a calculated gps heading on the -pi to pi scale) to be the
+        # positive y direction for our coordinate system 
+        if self.origin_coordinates is None:
+            # note that since the gps antenna locations are fixed distances from the origin, to not have to
+            # we use the coordinates for antenna one, then apply a fix based of where antenna 1 is from the
+            # center of the rover if needed afterwards
+            self.origin_coordinates = (lat1, long1)
+            x = 0
+            y = 0
+        else:
+            # it is difficult to calculate from decimal degrees the x and y distance between two points
+            # this is because a degree of longitude at te equator is a different distance that at 23 degree
+            # North, for example
+            # thus, I first get the distance and angle between the origin and our current rover position,
+            # then use these value to convert to our coordinate system (where North is 0.0 degrees)
+            orig_lat, orig_long = self.origin_coordinates
+            distance = functions.getDistanceBetweenGPSCoordinates((orig_lat, orig_long), (lat1, long1))
+            theta = functions.getHeadingBetweenGPSCoordinates(orig_lat, orig_long, lat1, long1)
+            # since we measure from north y is r*cos(theta) and x is -r*sin(theta)
+            x = -distance * sin(theta)
+            y = distance * cos(theta)
+
+        # now that we have the coordinate and angle quaternion information we can return the pose message
+        msg = self.msg
+        msg.header.stamp = rospy.Time.now()
+        msg.header.frame_id = "map"
+        msg.pose.position.x = x
+        msg.pose.position.y = y
+        msg.pose.position.z = 0.0
+
+        msg.pose.orientation.x = qx
+        msg.pose.orientation.y = qy
+        msg.pose.orientation.z = qz
+        msg.pose.orientation.w = qw
+        self.pose_pub.publish(msg)
+
+
+def main():
+    rospy.init_node("gps_to_pose")
+    gps_converter = GPSToPose()
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()