├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── package.xml ├── scripts ├── ifconfig.sh ├── nmea_tcp_driver ├── nmea_topic_driver └── nmea_topic_tcp_reader ├── setup.py └── src └── libnmea_navsat_driver ├── __init__.py ├── checksum_utils.py ├── driver.py └── parser.py /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *.pyc 3 | *.pyo 4 | *.swp 5 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Change log for nmea_navsat_driver package 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.0 (2015-04-23) 6 | ------------------ 7 | * Release to Jade. 8 | 9 | 0.4.2 (2015-04-23) 10 | ------------------ 11 | * Fix remaining parse problem with NovAtel receivers (empty field specified for num_satellite). 12 | 13 | 0.4.1 (2014-08-03) 14 | ------------------ 15 | * Add debug logging output to the parser (PR #8, Mike Purvis) 16 | * Add queue size arguement to publishers to fix warning on Indigo (PR #9, Mike Purvis) 17 | * Add support for roslint and some related cleanup (PR #10, Mike Purvis) 18 | 19 | 0.4.0 (2014-05-04) 20 | ------------------- 21 | * Initial release for Indigo 22 | * Fix #5: Empty fields spam rosout with warnings. Driver now outputs sensor_msgs/NavSatFix messages that may contain NaNs in position and covariance when receiving invalid fixes from the device. 23 | 24 | 0.3.3 (2013-10-08) 25 | ------------------- 26 | * Allow the driver to output velocity information anytime an RMC message is received 27 | 28 | 0.3.2 (2013-07-21) 29 | ------------------- 30 | * Moved to nmea_navsat_driver package 31 | * Removed .py extensions from new-in-Hydro scripts 32 | * Now uses nmea_msgs/Sentence instead of custom sentence type 33 | * nmea_topic_driver reads the `frame_id` parameter from the sentence, not from the parameter server 34 | 35 | 0.3.1 (2013-05-07) 36 | ------------------- 37 | * Removed incorrect find_package dependencies 38 | 39 | 0.3.0 (2013-05-05) 40 | ------------------- 41 | * Initial release for Hydro 42 | * Converted to Catkin 43 | * nmea_gps_driver.py is now deprecated and will be removed in I-Turtle. Replacement node is nmea_serial_driver.py . 44 | * Refactored code into NMEA parser, common ROS driver and separate nodes for reading directly from serial or from topic. 45 | * Bugs fixed: 46 | - nmea_gps_driver crashes when a sentence doesn't have a checksum * character ( http://kforge.ros.org/gpsdrivers/trac/ticket/4 ) 47 | - Add ability for nmea_gps_driver to support reading from string topic ( https://github.com/ros-drivers/nmea_gps_driver/issues/1 ). Use the nmea_topic_driver.py node to get this support. 48 | 49 | 0.2.0 (2012-03-15) 50 | ------------------ 51 | * Initial version (released into Fuerte) 52 | * Supports GGA or RMC+GSA sentences to generate sensor_msgs/NavSatFix messages 53 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nmea_tcp_driver) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roslint) 5 | 6 | catkin_python_setup() 7 | catkin_package() 8 | 9 | install(PROGRAMS 10 | scripts/nmea_tcp_driver 11 | scripts/nmea_topic_driver 12 | scripts/nmea_topic_tcp_reader 13 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 14 | ) 15 | 16 | # Check package for pep8 style, add a test to fail on violations. 17 | roslint_python() 18 | roslint_add_test() 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # nmea_tcp_driver 2 | 3 | nmea_tcp_driver is a ROS driver to parse NMEA strings and publish standard ROS NavSat message types from TCP input. it does not require the GPSD daemon to be running. This is a modification of the nmea_navsat_driver project (http://ros.org/wiki/nmea_navsat_driver 4 | ) to work with GPS systems that transmit solutions through TCP, like the Reach RTK GPS (https://emlid.com/reach/). This project does not take ownership of the nmea_navsat_driver project as detailed in the license, and it does not take ownership of the Reach RTK GPS. 5 | 6 | ### Version 7 | 1.0 8 | 9 | ### Tested On 10 | 11 | - Linux Ubuntu 14.04.1 LTS 12 | - Kernel 4.2.0 13 | - ROS Indigo 14 | - Reach RTK GPS 15 | 16 | ### Installation 17 | 18 | To install nmea_tcp_driver, clone the github repo into a catkin workspace and catkin_make as so: 19 | 20 | cd ~/catkin_ws/src/ 21 | git clone https://github.com/CearLab/nmea_tcp_driver.git 22 | cd ~/catkin_ws 23 | source devel/setup.bash 24 | catkin_make 25 | 26 | ### Usage 27 | 28 | First, make sure your device is publishing nmea messages through TCP to a certain port in your network. Output solutions to a tcpsvr with a certain port number (as so: https://docs.emlid.com/reach/quickstart/#setting-up-rover). The default port number for the nmea_tcp_driver is 12346. This port output number can be configured in the Reachview App config tab of the rover. The Reach output solution format must be set to nmea. 29 | 30 | To output TCP solutions through the USB connection on the Reach RTK, follow this guide: https://docs.emlid.com/reach/software-development/#using-ethernet-over-usb 31 | 32 | After the GPS correctly outputs the nmea data to a TCP server, begin the ros topic. To create the ros topic that publishes the processed nmea messages, issue the following command: 33 | 34 | rosrun nmea_tcp_driver nmea_tcp_driver 35 | 36 | Then run rostopic list, and the following topics should be present (provided a proper ros master is running): 37 | 38 | /tcpfix 39 | /tcpvel 40 | /tcptime_reference 41 | 42 | ### ROS Parameters 43 | 44 | ~host (string, default: 'reach.local') 45 | The host (GPS) to which the code will try to connect and receive TCP messaged from 46 | 47 | ~port (int, default: 12346) 48 | The port number for the TCP connection 49 | 50 | 51 | ### Notes 52 | 53 | - The ifconfig.sh file in the /scripts folder is used to fix a network manager issue some computers might have when mainting USB TCP connection with the Reach RTK. We witnessed this issue in only one of three computers, and it was not a large issue but is worth fixing. The ifconfig.sh script simply infinitely calls the "sudo ifconfig usb0 192.168.2.2" command so that any disconnections can be quickly reconnected. To run it, open a new terminal, move to the scripts folder, and run ./ifconfig.sh . 54 | - Some computers have issues when attempting to connect to the reach.local host (default), so if connection issues are encountered, try running nmea_tcp_driver with the host number passed as a parameter, EX: "rosrun nmea_tcp_driver nmea_tcp_driver host=192.168.2.15" 55 | - The status numbers have the following correlations: "0 = NO_FIX, 1 = FIX, 2 = SBAS_FIX, 4/5 = GBAS_FIX" 56 | 57 | API 58 | --- 59 | 60 | This package has no released Code API. 61 | 62 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nmea_tcp_driver 4 | 0.5.0 5 | 6 | Package to parse NMEA strings from TCP input and publish a very simple GPS message. Does not require the GPSD deamon. 7 | 8 | 9 | 10 | Gabriel Arpino 11 | 12 | 13 | BSD 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | catkin 25 | roslint 26 | rospy 27 | python-serial 28 | geometry_msgs 29 | nmea_msgs 30 | sensor_msgs 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /scripts/ifconfig.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | a=0 4 | 5 | while [ $a -lt 10 ] 6 | do 7 | echo "turtlebot" | sudo ifconfig usb0 192.168.2.2 8 | done 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /scripts/nmea_tcp_driver: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2013, Eric Perko 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the names of the authors nor the names of their 19 | # affiliated organizations may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import serial 36 | import socket # Import socket module 37 | import rospy 38 | import os #import libraries necessary for running the ifconfigg.sh file 39 | import sys 40 | import libnmea_navsat_driver.driver 41 | 42 | if __name__ == '__main__': 43 | rospy.init_node('nmea_tcp_driver') 44 | 45 | s = socket.socket() # Create a socket object 46 | host = rospy.get_param('~host', 'reach.local') 47 | port = rospy.get_param('~port', 12346) 48 | 49 | s.connect((host, port)) 50 | print "Device Connected" 51 | frame_id = libnmea_navsat_driver.driver.RosNMEADriver.get_frame_id() 52 | 53 | try: 54 | GPS = s.recv(1024) 55 | driver = libnmea_navsat_driver.driver.RosNMEADriver() 56 | while not rospy.is_shutdown(): 57 | GPS = s.recv(1024) 58 | data = GPS.strip() 59 | 60 | split = data.split() 61 | data = split[0] 62 | for el in split: 63 | if 'GPGGA' in el: 64 | data = el 65 | break 66 | elif 'RMC' in el: 67 | data = el 68 | 69 | 70 | try: 71 | driver.add_sentence(data, frame_id) 72 | except ValueError as e: 73 | rospy.logwarn("Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA sentences that caused it." % e) 74 | 75 | except rospy.ROSInterruptException: 76 | GPS.close #Close GPS socket when done 77 | -------------------------------------------------------------------------------- /scripts/nmea_topic_driver: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2013, Eric Perko 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the names of the authors nor the names of their 19 | # affiliated organizations may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import rospy 36 | 37 | 38 | from nmea_msgs.msg import Sentence 39 | 40 | import libnmea_navsat_driver.driver 41 | 42 | def nmea_sentence_callback(nmea_sentence, driver): 43 | try: 44 | driver.add_sentence(nmea_sentence.sentence, frame_id=nmea_sentence.header.frame_id, timestamp=nmea_sentence.header.stamp) 45 | except ValueError as e: 46 | rospy.logwarn("Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA sentences that caused it." % e) 47 | 48 | if __name__ == '__main__': 49 | rospy.init_node('nmea_topic_driver') 50 | 51 | driver = libnmea_navsat_driver.driver.RosNMEADriver() 52 | 53 | rospy.Subscriber("nmea_sentence", Sentence, nmea_sentence_callback, 54 | driver) 55 | 56 | rospy.spin() 57 | -------------------------------------------------------------------------------- /scripts/nmea_topic_tcp_reader: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2013, Eric Perko 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the names of the authors nor the names of their 19 | # affiliated organizations may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import serial 36 | import socket # Import socket module 37 | import rospy 38 | import os # Import libraries necessary for running the ifconfigg.sh file 39 | import sys 40 | from nmea_msgs.msg import Sentence 41 | from libnmea_navsat_driver.driver import RosNMEADriver 42 | 43 | if __name__ == '__main__': 44 | rospy.init_node('nmea_topic_tcp_reader') 45 | 46 | nmea_pub = rospy.Publisher("nmea_sentence", Sentence) 47 | 48 | s = socket.socket() # Create a socket object 49 | host = rospy.get_param('~host', 'reach.local') 50 | port = rospy.get_param('~port', 12346) 51 | 52 | s.connect((host, port)) 53 | print "Device Connected" 54 | 55 | # Get the frame_id 56 | frame_id = RosNMEADriver.get_frame_id() 57 | 58 | try: 59 | GPS = s.recv(1024) 60 | while not rospy.is_shutdown(): 61 | GPS = s.recv(1024) 62 | data = GPS.strip() 63 | 64 | split = data.split() 65 | data = split[0] 66 | for el in split: 67 | if 'GPGGA' in el: 68 | data = el 69 | break 70 | elif 'RMC' in el: 71 | data = el 72 | 73 | sentence = Sentence() 74 | sentence.header.stamp = rospy.get_rostime() 75 | sentence.header.frame_id = frame_id 76 | sentence.sentence = data 77 | 78 | nmea_pub.publish(sentence) 79 | 80 | except rospy.ROSInterruptException: 81 | GPS.close #Close GPS socket when done 82 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['libnmea_navsat_driver'], 6 | package_dir={'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /src/libnmea_navsat_driver/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CearLab/nmea_tcp_driver/473cc8326478ec42b0b4d2ba04044e8ebbd6c1c2/src/libnmea_navsat_driver/__init__.py -------------------------------------------------------------------------------- /src/libnmea_navsat_driver/checksum_utils.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2013, Eric Perko 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the names of the authors nor the names of their 17 | # affiliated organizations may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | 34 | # Check the NMEA sentence checksum. Return True if passes and False if failed 35 | def check_nmea_checksum(nmea_sentence): 36 | split_sentence = nmea_sentence.split('*') 37 | if len(split_sentence) != 2: 38 | #No checksum bytes were found... improperly formatted/incomplete NMEA data? 39 | return False 40 | transmitted_checksum = split_sentence[1].strip() 41 | 42 | #Remove the $ at the front 43 | data_to_checksum = split_sentence[0][1:] 44 | checksum = 0 45 | for c in data_to_checksum: 46 | checksum ^= ord(c) 47 | 48 | return ("%02X" % checksum) == transmitted_checksum.upper() 49 | -------------------------------------------------------------------------------- /src/libnmea_navsat_driver/driver.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2013, Eric Perko 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the names of the authors nor the names of their 17 | # affiliated organizations may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import math 34 | 35 | import rospy 36 | 37 | from sensor_msgs.msg import NavSatFix, NavSatStatus, TimeReference 38 | from geometry_msgs.msg import TwistStamped 39 | 40 | from libnmea_navsat_driver.checksum_utils import check_nmea_checksum 41 | import libnmea_navsat_driver.parser 42 | 43 | 44 | class RosNMEADriver(object): 45 | def __init__(self): 46 | self.fix_pub = rospy.Publisher('tcpfix', NavSatFix, queue_size=1) 47 | self.vel_pub = rospy.Publisher('tcpvel', TwistStamped, queue_size=1) 48 | self.time_ref_pub = rospy.Publisher('tcptime_reference', TimeReference, queue_size=1) 49 | 50 | self.time_ref_source = rospy.get_param('~time_ref_source', None) 51 | self.use_RMC = rospy.get_param('~useRMC', False) 52 | 53 | # Returns True if we successfully did something with the passed in 54 | # nmea_string 55 | def add_sentence(self, nmea_string, frame_id, timestamp=None): 56 | if not check_nmea_checksum(nmea_string): 57 | rospy.logwarn("Received a sentence with an invalid checksum. " + 58 | "Sentence was: %s" % repr(nmea_string)) 59 | return False 60 | 61 | parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) 62 | if not parsed_sentence: 63 | rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) 64 | return False 65 | 66 | if timestamp: 67 | current_time = timestamp 68 | else: 69 | current_time = rospy.get_rostime() 70 | current_fix = NavSatFix() 71 | current_fix.header.stamp = current_time 72 | current_fix.header.frame_id = frame_id 73 | current_time_ref = TimeReference() 74 | current_time_ref.header.stamp = current_time 75 | current_time_ref.header.frame_id = frame_id 76 | if self.time_ref_source: 77 | current_time_ref.source = self.time_ref_source 78 | else: 79 | current_time_ref.source = frame_id 80 | 81 | if not self.use_RMC and 'GGA' in parsed_sentence: 82 | data = parsed_sentence['GGA'] 83 | gps_qual = data['fix_type'] 84 | if gps_qual == 0: 85 | current_fix.status.status = NavSatStatus.STATUS_NO_FIX 86 | elif gps_qual == 1: 87 | current_fix.status.status = NavSatStatus.STATUS_FIX 88 | elif gps_qual == 2: 89 | current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX 90 | elif gps_qual in (4, 5): 91 | current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX 92 | else: 93 | current_fix.status.status = NavSatStatus.STATUS_NO_FIX 94 | 95 | current_fix.status.service = NavSatStatus.SERVICE_GPS 96 | 97 | current_fix.header.stamp = current_time 98 | 99 | latitude = data['latitude'] 100 | if data['latitude_direction'] == 'S': 101 | latitude = -latitude 102 | current_fix.latitude = latitude 103 | 104 | longitude = data['longitude'] 105 | if data['longitude_direction'] == 'W': 106 | longitude = -longitude 107 | current_fix.longitude = longitude 108 | 109 | hdop = data['hdop'] 110 | current_fix.position_covariance[0] = hdop ** 2 111 | current_fix.position_covariance[4] = hdop ** 2 112 | current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME 113 | current_fix.position_covariance_type = \ 114 | NavSatFix.COVARIANCE_TYPE_APPROXIMATED 115 | 116 | # Altitude is above ellipsoid, so adjust for mean-sea-level 117 | altitude = data['altitude'] + data['mean_sea_level'] 118 | current_fix.altitude = altitude 119 | 120 | self.fix_pub.publish(current_fix) 121 | 122 | if not math.isnan(data['utc_time']): 123 | current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) 124 | self.time_ref_pub.publish(current_time_ref) 125 | 126 | elif 'RMC' in parsed_sentence: 127 | data = parsed_sentence['RMC'] 128 | 129 | # Only publish a fix from RMC if the use_RMC flag is set. 130 | if self.use_RMC: 131 | if data['fix_valid']: 132 | current_fix.status.status = NavSatStatus.STATUS_FIX 133 | else: 134 | current_fix.status.status = NavSatStatus.STATUS_NO_FIX 135 | 136 | current_fix.status.service = NavSatStatus.SERVICE_GPS 137 | 138 | latitude = data['latitude'] 139 | if data['latitude_direction'] == 'S': 140 | latitude = -latitude 141 | current_fix.latitude = latitude 142 | 143 | longitude = data['longitude'] 144 | if data['longitude_direction'] == 'W': 145 | longitude = -longitude 146 | current_fix.longitude = longitude 147 | 148 | current_fix.altitude = float('NaN') 149 | current_fix.position_covariance_type = \ 150 | NavSatFix.COVARIANCE_TYPE_UNKNOWN 151 | 152 | self.fix_pub.publish(current_fix) 153 | 154 | if not math.isnan(data['utc_time']): 155 | current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) 156 | self.time_ref_pub.publish(current_time_ref) 157 | 158 | # Publish velocity from RMC regardless, since GGA doesn't provide it. 159 | if data['fix_valid']: 160 | current_vel = TwistStamped() 161 | current_vel.header.stamp = current_time 162 | current_vel.header.frame_id = frame_id 163 | current_vel.twist.linear.x = data['speed'] * \ 164 | math.sin(data['true_course']) 165 | current_vel.twist.linear.y = data['speed'] * \ 166 | math.cos(data['true_course']) 167 | self.vel_pub.publish(current_vel) 168 | else: 169 | return False 170 | 171 | """Helper method for getting the frame_id with the correct TF prefix""" 172 | 173 | @staticmethod 174 | def get_frame_id(): 175 | frame_id = rospy.get_param('~frame_id', 'gps') 176 | if frame_id[0] != "/": 177 | """Add the TF prefix""" 178 | prefix = "" 179 | prefix_param = rospy.search_param('tf_prefix') 180 | if prefix_param: 181 | prefix = rospy.get_param(prefix_param) 182 | if prefix[0] != "/": 183 | prefix = "/%s" % prefix 184 | return "%s/%s" % (prefix, frame_id) 185 | else: 186 | return frame_id 187 | -------------------------------------------------------------------------------- /src/libnmea_navsat_driver/parser.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2013, Eric Perko 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the names of the authors nor the names of their 17 | # affiliated organizations may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import re 34 | import time 35 | import calendar 36 | import math 37 | import logging 38 | logger = logging.getLogger('rosout') 39 | 40 | 41 | def safe_float(field): 42 | try: 43 | return float(field) 44 | except ValueError: 45 | return float('NaN') 46 | 47 | 48 | def safe_int(field): 49 | try: 50 | return int(field) 51 | except ValueError: 52 | return 0 53 | 54 | 55 | def convert_latitude(field): 56 | return safe_float(field[0:2]) + safe_float(field[2:]) / 60.0 57 | 58 | 59 | def convert_longitude(field): 60 | return safe_float(field[0:3]) + safe_float(field[3:]) / 60.0 61 | 62 | 63 | def convert_time(nmea_utc): 64 | # Get current time in UTC for date information 65 | utc_struct = time.gmtime() # immutable, so cannot modify this one 66 | utc_list = list(utc_struct) 67 | # If one of the time fields is empty, return NaN seconds 68 | if not nmea_utc[0:2] or not nmea_utc[2:4] or not nmea_utc[4:6]: 69 | return float('NaN') 70 | else: 71 | hours = int(nmea_utc[0:2]) 72 | minutes = int(nmea_utc[2:4]) 73 | seconds = int(nmea_utc[4:6]) 74 | utc_list[3] = hours 75 | utc_list[4] = minutes 76 | utc_list[5] = seconds 77 | unix_time = calendar.timegm(tuple(utc_list)) 78 | return unix_time 79 | 80 | 81 | def convert_status_flag(status_flag): 82 | if status_flag == "A": 83 | return True 84 | elif status_flag == "V": 85 | return False 86 | else: 87 | return False 88 | 89 | 90 | def convert_knots_to_mps(knots): 91 | return safe_float(knots) * 0.514444444444 92 | 93 | 94 | # Need this wrapper because math.radians doesn't auto convert inputs 95 | def convert_deg_to_rads(degs): 96 | return math.radians(safe_float(degs)) 97 | 98 | """Format for this is a sentence identifier (e.g. "GGA") as the key, with a 99 | tuple of tuples where each tuple is a field name, conversion function and index 100 | into the split sentence""" 101 | parse_maps = { 102 | "GGA": [ 103 | ("fix_type", int, 6), 104 | ("latitude", convert_latitude, 2), 105 | ("latitude_direction", str, 3), 106 | ("longitude", convert_longitude, 4), 107 | ("longitude_direction", str, 5), 108 | ("altitude", safe_float, 9), 109 | ("mean_sea_level", safe_float, 11), 110 | ("hdop", safe_float, 8), 111 | ("num_satellites", safe_int, 7), 112 | ("utc_time", convert_time, 1), 113 | ], 114 | "RMC": [ 115 | ("utc_time", convert_time, 1), 116 | ("fix_valid", convert_status_flag, 2), 117 | ("latitude", convert_latitude, 3), 118 | ("latitude_direction", str, 4), 119 | ("longitude", convert_longitude, 5), 120 | ("longitude_direction", str, 6), 121 | ("speed", convert_knots_to_mps, 7), 122 | ("true_course", convert_deg_to_rads, 8), 123 | ] 124 | } 125 | 126 | 127 | def parse_nmea_sentence(nmea_sentence): 128 | # Check for a valid nmea sentence 129 | if not re.match('^\$GP.*\*[0-9A-Fa-f]{2}$', nmea_sentence): 130 | logger.debug("Regex didn't match, sentence not valid NMEA? Sentence was: %s" 131 | % repr(nmea_sentence)) 132 | return False 133 | fields = [field.strip(',') for field in nmea_sentence.split(',')] 134 | 135 | # Ignore the $ and talker ID portions (e.g. GP) 136 | sentence_type = fields[0][3:] 137 | 138 | if not sentence_type in parse_maps: 139 | logger.debug("Sentence type %s not in parse map, ignoring." 140 | % repr(sentence_type)) 141 | return False 142 | 143 | parse_map = parse_maps[sentence_type] 144 | 145 | parsed_sentence = {} 146 | for entry in parse_map: 147 | parsed_sentence[entry[0]] = entry[1](fields[entry[2]]) 148 | 149 | return {sentence_type: parsed_sentence} 150 | --------------------------------------------------------------------------------