How do I Calculate Distance Using 2 GPS Topics

Question

Hello,

So currently I have two GPS modules and they each have their own nodes. The 2 nodes publish their GPS information onto their own topics. So 2 nodes and 2 topics. I'm able to subscribe to both nodes but I'm having trouble using the variables in the callback function to calculate distance.

I would get this error: NameError: global name 'gps_lat' is not defined

Here's my code below

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from nmea_msgs.msg import Sentence
from sensor_msgs.msg import NavSatFix
import math

def callback(data):
    gps_lat = round(data.latitude, 6)
    gps_lon = round(data.longitude, 6)

def callback1(data):
    gps1_lat = round(data.latitude, 6)
    gps1_lon = round(data.longitude, 6)

def listener():

    rospy.init_node('gps_monitor', anonymous=True)
    rospy.Subscriber("/fix", NavSatFix, callback)
    rospy.Subscriber("/fix1", NavSatFix, callback1)
    rospy.spin()

    delta_lat = (gps_lat*(108000) - gps1_lat*(108000))
    delta_lon = (gps_lon*(108000) - gps1_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)



if __name__ == '__main__':
    listener()

---

Originally posted by SupermanPrime01 on ROS Answers with karma: 27 on 2017-04-26

Post score: 0

Answer 1

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.

---

Originally posted by Geoff with karma: 4203 on 2017-04-26

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by SupermanPrime01 on 2017-04-26:
Thank you so much Geoff. This was exactly what I needed. When I tried the global variables method the distances would continuously increase so you're right about that too.