ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Reading data from Topic

asked 2021-10-21 20:18:10 -0500

Jord gravatar image

I am using a GPS plugin on my model. I know it works as I have tested by using

rostopic echo gps_out

and it continually outputs data in the NavSatFix form.

I am trying to write a script that will ultimately give my robot velocity commands through reading a target and current GPS location.

The python script I'm currently writing is to read in the data and then assign the longitude and latitude to global variables which can be displayed to confirm they are correct. This is the python script currently.

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import NavSatFix
import tf

global longitude
global latitude

latitude='Not re-assigned'
longitude='Not re-assigned'

def callback(data):
    latitude = data.latitude
    longitude = data.longitude

rospy.init_node('topic_reader')

rospy.Subscriber('gps_out', NavSatFix, callback)

print("Longitude:", longitude)
print("Latitude:", latitude)

As can be read from the code, the only output is

Longitude: Not re-assigned
Latitude: Not re-assigned

What am I doing wrong?

From my understanding I am setting longitude and latitude variables to be global, and assigning them the initial conditions. Then assigned the nodes name, then subscribing to the topic and running callback which should assign the values to the variables which will allow them to be printed.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-10-21 21:03:15 -0500

janindu gravatar image

You might want to look at the subscriber documentation. Essentially you need rospy.spin() so that the callbacks will be called, and your print functions should be called within the callback. I've fixed it here

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import NavSatFix
import tf

global longitude
global latitude

latitude='Not re-assigned'
longitude='Not re-assigned'

def callback(data):
    # Assign the data to variables
    latitude = data.latitude
    longitude = data.longitude

    # Print
    print("Longitude:", longitude)
    print("Latitude:", latitude)

rospy.init_node('topic_reader')

rospy.Subscriber('gps_out', NavSatFix, callback)

# Until ctrl + c is pressed, lines after the call to rospy.spin will not be executed. 
rospy.spin()
edit flag offensive delete link more

Comments

@Jord in addition to the rospy::spin(), the global keywords need to be inside the callback function. As it is now, you are creating local variables and assigning to them. Also, make sure that "python" means python3.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-10-22 17:38:26 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2021-10-21 20:18:10 -0500

Seen: 598 times

Last updated: Oct 21 '21