gps nmea sentence subscriber format for python
Q: What is the correct from ... import ...
statement for python for the /nmea_sentence/sentence
topic as well as the rospy.Subscriber("/nmea_sentence/sentence", Sentence, callback)
statement?
My launch file works fine
<launch>
<node name="gps" pkg="nmea_navsat_driver" type="nmea_topic_serial_reader">
<param name="port" value="/dev/gps" />
<param name="baud" value="115200" />
</node>
<node name="nmea" pkg="nmea_navsat_driver" type="nmea_topic_driver">
<param name="useRMC" value="True" />
</node>
</launch>
And I can use rostopic echo /nmea_sentence/sentence
To get output on the screen of my nmea sentence/sentence topic.
…
"$GPRMC,171450.40,A,4020.7237762,N,08007.7303416,W,000.003,206.1,220320,0.0,W,D*3A"
--- "$GPGGA,171450.60,4020.7237758,N,08007.7303411,W,4,12,0.9,319.3637,M,-34.191,M,00,0004*6D"
--- "$GNGSA,M,3,04,16,09,26,27,08,,,,,,,1.8,0.9,1.6*24"
--- "$GNGSA,M,3,48,46,57,47,58,38,,,,,,,1.8,0.9,1.6*2E"
--- "$GPRMC,171450.60,A,4020.7237758,N,08007.7303411,W,000.004,177.6,220320,0.0,W,D*33"
--- "$GPGGA,171450.80,4020.7237760,N,08007.7303404,W,4,12,0.9,319.3683,M,-34.191,M,00,0004*63"
--- "$GNGSA,M,3,04,16,09,26,27,08,,,,,,,1.8,0.9,1.6*24"
--- "$GNGSA,M,3,48,46,57,47,58,38,,,,,,,1.8,0.9,1.6*2E"
--- "$GPRMC,171450.80,A,4020.7237760,N,08007.7303404,W,000.003,139.0,220320,0.0,W,D*39"
What I can’t seem to do is figure out the syntax of the python code to subscribe to the /nmea_sentence/sentence topic
. Can I get some help?
You can see my commented out failed attempts.
#!/usr/bin/env python
import rospy
#from std_msgs.msg import Header
#from nmea_msgs.msg import Sentence
#from std_msgs.msg import String
# from nmea_msgs.msg import String
#from nmea_msgs.msg import Header
from nmea_msgs.msg import Sentence
rospy.init_node('read_nmea', anonymous = True)
def callback(message):
#response = message.sentence
response = message
#rospy.loginfo("I heard %s",data.header.sentence) # with from std_msgs.msg import Header and rospy.Subscriber("/nmea_sentence", Header, callback)
#rospy.loginfo("I heard %s",header.sentence) # with from std_msgs.msg import Header and rospy.Subscriber("/nmea_sentence", Header, callback)
#rospy.loginfo("I heard %s",data.sentence) # with from std_msgs.msg import Header and rospy.Subscriber("/nmea_sentence", Header, callback)
#rospy.loginfo("I heard %s",sentence) # with from std_msgs.msg import Header and rospy.Subscriber("/nmea_sentence", Header, callback)
#rospy.loginfo("I heard %s",data.sentence) # with from nmea_msgs.msg import Sentence and rospy.Subscriber("/nmea_sentence/sentence", Sentence, callback)
#rospy.loginfo("I heard %s",data) # with from nmea_msgs.msg import Sentence and rospy.Subscriber("/nmea_sentence/sentence", Sentence, callback)
#rospy.loginfo("I heard %s",sentence) # with from nmea_msgs.msg import Sentence and rospy.Subscriber("/nmea_sentence/sentence", Sentence, callback)
#rospy.loginfo("I heard %s",message.sentence) # with from nmea_msgs.msg import Sentence and ('nmea_sentence/sentence', Sentence, callback)
rospy.loginfo("I heard %s",response) # with Sentence and Header; using message.Sentence; using "", '' and / on topic
def listener():
rospy.loginfo("start listening")
#rospy.init_node('read_nmea', anonymous = True)
#rospy.Subscriber("/nmea_sentence", Header, callback)
#rospy.Subscriber('nmea_sentence/sentence', Header, callback)
#rospy.Subscriber("nmea_sentence/sentence", Header, callback)
#rospy.Subscriber('nmea_sentence/sentence', Sentence, callback)
#rospy.Subscriber("/nmea_sentence/sentence", Header, callback)
#rospy.Subscriber("/nmea_sentence/sentence", String, callback)
#rospy.Subscriber("/nmea_sentence/sentence", Header, callback)
rospy.Subscriber("/nmea_sentence/sentence", Sentence, callback)
rospy.spin()
if __name__ == '__main__':
rospy.loginfo("Initiating read_nmea.py.(h)..")
listener()
Ultimately I want to parse the heading, speed, number of satellites and signal quality of the gps signal within ROS.
Ref: http://docs.ros.org/api/nmea_msgs/html/msg/Sentence.html
$ rostopic type /nmea_sentence | rosmsg show
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string sentence
$ rostopic type /nmea_sentence/sentence | rosmsg show
uint32 seq
time stamp
string frame_id
Asked by Al53 on 2020-03-22 13:32:43 UTC
Comments
You don't actually tell us about any error/problem.
Asked by gvdhoorn on 2020-03-22 13:51:52 UTC
Sorry for that. I have a window open echoing /nmea_sentence/sentence so I know sentences are being produced. I start the subscriber and the loginfo statements are printed, but no further output is provided. $ rosrun beginner_tutorials read_nmea.py --debug [INFO] [1584903378.444488]: Initiating read_nmea.py.(h).. [INFO] [1584903378.445123]: start listening
Asked by Al53 on 2020-03-22 13:59:47 UTC
Figured it out. Here is the solution.
Asked by Al53 on 2020-03-22 18:37:28 UTC
What is/was the solution?
Please try to clearly describe both the problem and whatever you did to solve it.
It's quite unclear what you did that fixed the problem you apparently had.
Asked by gvdhoorn on 2020-03-23 04:20:24 UTC
The problem was no loginfo output in the callback function even though I knew sentences were being produced. The solution was to use the correct syntax for the key statements:
from nmea_msgs.msg import Sentence
response = message.sentence
rospy.Subscriber("/nmea_sentence", Sentence, callback)
Asked by Al53 on 2020-03-23 15:22:55 UTC