How do I Calculate Distance Using 2 GPS Topics

edit

You can't use variables local to one function (your callback functions, in this case) in another function (the listener() function).

For working with data from multiple topics, there are several ways you can structure your program.

The first is to use global variables. Make gps_lat, gps_lon, gps1_lat and gps1_lon global variables that are updated in the callbacks and used in listener(). However this approach does not synchronise the data so you might get inaccurate values for the calculated distance if it uses a mix of new and old data.

The second is to make your node a class, with the gps data stored in member variables and the callbacks methods of the class. This would essentially be the same as the above, but without the use of global variables.

The third is to use message filters to synchronise the data received from the two sensors and calculate the distance when data is received from both. With a message filter, you provide it a set of inputs (your two topics, /fix and /fix1), and register a callback. The callback is called when the message filter has data meeting its condition, such as a message on each topic with similar timestamps. If your GPS sensors are not producing data in sync, then you might need to use the approximate time policy filter. You can use it like this:

#!/usr/bin/env python

import rospy
import message_filters
from sensor_msgs.msg import NavSatFix


def calc_distance(gps1, gps2):
    gps1_lat = round(gps1.latitude, 6)
    gps1_lon = round(gps1.longitude, 6)
    gps2_lat = round(gps2.latitude, 6)
    gps2_lon = round(gps2.longitude, 6)
    delta_lat = (gps1_lat*(108000) - gps2_lat*(108000))
    delta_lon = (gps1_lon*(108000) - gps2_lon*(108000))
    hyp_m = (delta_lat**2 + delta_lon**2)**0.5
    hyp_ft = (hyp_m*3.2800839)
    rospy.loginfo("Distance is %s in ft.", hyp_ft)


def listener():
    rospy.init_node('gps_monitor', anonymous=True)
    gps1 = message_filters.Subscriber('/fix', NavSatFix)
    gps2 = message_filters.Subscriber('/fix1', NavSatFix)
    ts = message_filters.ApproximateTimeSynchronizer([gps1, gps2], 10, 5)
    ts.registerCallback(calc_distance)
    rospy.spin()


if __name__ == '__main__':
    listener()

This will print a new distance calculation every time it gets a message on each topic that are within 5 seconds of each other.