diff --git a/racecar_bringup/install/_udev_/99-tianbot-racecar-core.rules b/racecar_bringup/install/_udev_/99-tianbot-racecar-core.rules new file mode 100644 index 0000000..0acb098 --- /dev/null +++ b/racecar_bringup/install/_udev_/99-tianbot-racecar-core.rules @@ -0,0 +1 @@ +KERNEL=="ttyUSB*", ENV{ID_SERIAL_SHORT}=="3503[0-9]*0001", MODE:="0666", GROUP:="dialout", SYMLINK+="tianbot_racecar_core" diff --git a/racecar_bringup/install/_udev_/99-tianbot-racecar-gps.rules b/racecar_bringup/install/_udev_/99-tianbot-racecar-gps.rules new file mode 100644 index 0000000..58636f8 --- /dev/null +++ b/racecar_bringup/install/_udev_/99-tianbot-racecar-gps.rules @@ -0,0 +1 @@ +KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="tianbot_racecar_gps" diff --git a/racecar_bringup/launch/gps.launch b/racecar_bringup/launch/gps.launch index 15718cf..bbaf5c6 100644 --- a/racecar_bringup/launch/gps.launch +++ b/racecar_bringup/launch/gps.launch @@ -3,8 +3,8 @@ - - + + diff --git a/racecar_description/launch/includes/standard_tf.launch.xml b/racecar_description/launch/includes/standard_rplidar_tf.launch.xml similarity index 100% rename from racecar_description/launch/includes/standard_tf.launch.xml rename to racecar_description/launch/includes/standard_rplidar_tf.launch.xml diff --git a/racecar_description/launch/includes/standard_velodyne_tf.launch.xml b/racecar_description/launch/includes/standard_velodyne_tf.launch.xml new file mode 100644 index 0000000..8c4046d --- /dev/null +++ b/racecar_description/launch/includes/standard_velodyne_tf.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/racecar_description/launch/tianbot_racecar_tf.launch b/racecar_description/launch/tianbot_racecar_tf.launch index bafa8a1..3891660 100644 --- a/racecar_description/launch/tianbot_racecar_tf.launch +++ b/racecar_description/launch/tianbot_racecar_tf.launch @@ -2,8 +2,15 @@ + - + + + + + + + diff --git a/racecar_gps/.gitignore b/racecar_gps/.gitignore new file mode 100644 index 0000000..b788c22 --- /dev/null +++ b/racecar_gps/.gitignore @@ -0,0 +1,4 @@ +build/ +*.pyc +*.pyo +*.swp diff --git a/racecar_gps/CHANGELOG.rst b/racecar_gps/CHANGELOG.rst new file mode 100644 index 0000000..32e37bd --- /dev/null +++ b/racecar_gps/CHANGELOG.rst @@ -0,0 +1,52 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Change log for nmea_navsat_driver package +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.0 (2015-04-23) +------------------ +* Release to Jade. + +0.4.2 (2015-04-23) +------------------ +* Fix remaining parse problem with NovAtel receivers (empty field specified for num_satellite). + +0.4.1 (2014-08-03) +------------------ +* Add debug logging output to the parser (PR #8, Mike Purvis) +* Add queue size arguement to publishers to fix warning on Indigo (PR #9, Mike Purvis) +* Add support for roslint and some related cleanup (PR #10, Mike Purvis) + +0.4.0 (2014-05-04) +------------------- +* Initial release for Indigo +* 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. + +0.3.3 (2013-10-08) +------------------- +* Allow the driver to output velocity information anytime an RMC message is received + +0.3.2 (2013-07-21) +------------------- +* Moved to nmea_navsat_driver package +* Removed .py extensions from new-in-Hydro scripts +* Now uses nmea_msgs/Sentence instead of custom sentence type +* nmea_topic_driver reads the `frame_id` parameter from the sentence, not from the parameter server + +0.3.1 (2013-05-07) +------------------- +* Removed incorrect find_package dependencies + +0.3.0 (2013-05-05) +------------------- +* Initial release for Hydro +* Converted to Catkin +* nmea_gps_driver.py is now deprecated and will be removed in I-Turtle. Replacement node is nmea_serial_driver.py . +* Refactored code into NMEA parser, common ROS driver and separate nodes for reading directly from serial or from topic. +* Bugs fixed: + - nmea_gps_driver crashes when a sentence doesn't have a checksum * character ( http://kforge.ros.org/gpsdrivers/trac/ticket/4 ) + - 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. + +0.2.0 (2012-03-15) +------------------ +* Initial version (released into Fuerte) +* Supports GGA or RMC+GSA sentences to generate sensor_msgs/NavSatFix messages diff --git a/racecar_gps/CMakeLists.txt b/racecar_gps/CMakeLists.txt new file mode 100644 index 0000000..e1f7e40 --- /dev/null +++ b/racecar_gps/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8.3) +project(racecar_gps) + +find_package(catkin REQUIRED) + +catkin_python_setup() +catkin_package() + +install(PROGRAMS + scripts/nmea_serial_driver + scripts/nmea_socket_driver + scripts/nmea_topic_driver + scripts/nmea_topic_serial_reader + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +if (CATKIN_ENABLE_TESTING) + find_package(roslint) + roslint_python() + roslint_add_test() +endif() diff --git a/racecar_gps/README.rst b/racecar_gps/README.rst new file mode 100644 index 0000000..3981556 --- /dev/null +++ b/racecar_gps/README.rst @@ -0,0 +1,11 @@ +nmea_navsat_driver +=============== + +ROS driver to parse NMEA strings and publish standard ROS NavSat message types. Does not require the GPSD daemon to be running. + +API +--- + +This package has no released Code API. + +The ROS API documentation and other information can be found at http://ros.org/wiki/nmea_navsat_driver diff --git a/racecar_gps/package.xml b/racecar_gps/package.xml new file mode 100644 index 0000000..3dc70cf --- /dev/null +++ b/racecar_gps/package.xml @@ -0,0 +1,33 @@ + + + racecar_gps + 0.5.0 + + Package to parse NMEA strings and publish a very simple GPS message. Does not + require or use the GPSD deamon. + + + Ed Venator + + BSD + + http://ros.org/wiki/nmea_navsat_driver + + Eric Perko + Steven Martin + + catkin + roslint + rospy + python-serial + geometry_msgs + nmea_msgs + sensor_msgs + + roslint + + + + + + diff --git a/racecar_gps/scripts/nmea_serial_driver b/racecar_gps/scripts/nmea_serial_driver new file mode 100755 index 0000000..23a69fc --- /dev/null +++ b/racecar_gps/scripts/nmea_serial_driver @@ -0,0 +1,65 @@ +#! /usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Eric Perko +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the names of the authors nor the names of their +# affiliated organizations may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import serial + +import rospy + +import libnmea_navsat_driver.driver + +import sys + +if __name__ == '__main__': + rospy.init_node('nmea_serial_driver') + + serial_port = rospy.get_param('~port','/dev/ttyUSB0') + serial_baud = rospy.get_param('~baud',4800) + frame_id = libnmea_navsat_driver.driver.RosNMEADriver.get_frame_id() + + try: + GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) + GPS.write('\$PMTK220,200*2C\r\n') + try: + driver = libnmea_navsat_driver.driver.RosNMEADriver() + while not rospy.is_shutdown(): + data = GPS.readline().strip() + try: + driver.add_sentence(data, frame_id) + except ValueError as e: + 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) + + except (rospy.ROSInterruptException, serial.serialutil.SerialException): + GPS.close() #Close GPS serial port + except serial.SerialException as ex: + rospy.logfatal("Could not open serial port: I/O error({0}): {1}".format(ex.errno, ex.strerror)) diff --git a/racecar_gps/scripts/nmea_socket_driver b/racecar_gps/scripts/nmea_socket_driver new file mode 100755 index 0000000..9bde855 --- /dev/null +++ b/racecar_gps/scripts/nmea_socket_driver @@ -0,0 +1,96 @@ +#! /usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2016, Rein Appeldoorn +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the names of the authors nor the names of their +# affiliated organizations may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import socket +import sys + +import rospy + +import libnmea_navsat_driver.driver + +if __name__ == '__main__': + rospy.init_node('nmea_socket_driver') + + try: + local_ip = rospy.get_param('~ip', '0.0.0.0') + local_port = rospy.get_param('~port', 10110) + buffer_size = rospy.get_param('~buffer_size', 4096) + timeout = rospy.get_param('~timeout_sec', 2) + except KeyError as e: + rospy.logerr("Parameter %s not found" % e) + sys.exit(1) + + frame_id = libnmea_navsat_driver.driver.RosNMEADriver.get_frame_id() + + driver = libnmea_navsat_driver.driver.RosNMEADriver() + + # Connection-loop: connect and keep receiving. If receiving fails, reconnect + while not rospy.is_shutdown(): + try: + # Create a socket + socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + # Bind the socket to the port + socket_.bind((local_ip, local_port)) + + # Set timeout + socket_.settimeout(timeout) + except socket.error, exc: + rospy.logerr("Caught exception socket.error when setting up socket: %s" % exc) + sys.exit(1) + + # recv-loop: When we're connected, keep receiving stuff until that fails + while not rospy.is_shutdown(): + try: + data, remote_address = socket_.recvfrom(buffer_size) + + # strip the data + data_list = data.strip().split("\n") + + for data in data_list: + + try: + driver.add_sentence(data, frame_id) + except ValueError as e: + 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) + + except socket.error, exc: + rospy.logerr("Caught exception socket.error during recvfrom: %s" % exc) + socket_.close() + # This will break out of the recv-loop so we start another iteration of the connection-loop + break + + socket_.close() # Close socket diff --git a/racecar_gps/scripts/nmea_topic_driver b/racecar_gps/scripts/nmea_topic_driver new file mode 100755 index 0000000..92cc851 --- /dev/null +++ b/racecar_gps/scripts/nmea_topic_driver @@ -0,0 +1,55 @@ +#! /usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Eric Perko +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the names of the authors nor the names of their +# affiliated organizations may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import rospy + +from nmea_msgs.msg import Sentence + +import libnmea_navsat_driver.driver + +def nmea_sentence_callback(nmea_sentence, driver): + try: + driver.add_sentence(nmea_sentence.sentence, frame_id=nmea_sentence.header.frame_id, timestamp=nmea_sentence.header.stamp) + except ValueError as e: + 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) + +if __name__ == '__main__': + rospy.init_node('nmea_topic_driver') + + driver = libnmea_navsat_driver.driver.RosNMEADriver() + + rospy.Subscriber("nmea_sentence", Sentence, nmea_sentence_callback, + driver) + + rospy.spin() diff --git a/racecar_gps/scripts/nmea_topic_serial_reader b/racecar_gps/scripts/nmea_topic_serial_reader new file mode 100755 index 0000000..814fe71 --- /dev/null +++ b/racecar_gps/scripts/nmea_topic_serial_reader @@ -0,0 +1,66 @@ +#! /usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Eric Perko +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the names of the authors nor the names of their +# affiliated organizations may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import serial + +import rospy + +from nmea_msgs.msg import Sentence +from libnmea_navsat_driver.driver import RosNMEADriver + +if __name__ == '__main__': + rospy.init_node('nmea_topic_serial_reader') + + nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) + + serial_port = rospy.get_param('~port','/dev/ttyUSB0') + serial_baud = rospy.get_param('~baud',4800) + + # Get the frame_id + frame_id = RosNMEADriver.get_frame_id() + + try: + GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) + while not rospy.is_shutdown(): + data = GPS.readline().strip() + + sentence = Sentence() + sentence.header.stamp = rospy.get_rostime() + sentence.header.frame_id = frame_id + sentence.sentence = data + + nmea_pub.publish(sentence) + + except rospy.ROSInterruptException: + GPS.close() #Close GPS serial port diff --git a/racecar_gps/setup.py b/racecar_gps/setup.py new file mode 100644 index 0000000..9ca3bf7 --- /dev/null +++ b/racecar_gps/setup.py @@ -0,0 +1,9 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['libnmea_navsat_driver'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/racecar_gps/src/libnmea_navsat_driver/__init__.py b/racecar_gps/src/libnmea_navsat_driver/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/racecar_gps/src/libnmea_navsat_driver/checksum_utils.py b/racecar_gps/src/libnmea_navsat_driver/checksum_utils.py new file mode 100644 index 0000000..899ae38 --- /dev/null +++ b/racecar_gps/src/libnmea_navsat_driver/checksum_utils.py @@ -0,0 +1,48 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Eric Perko +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the names of the authors nor the names of their +# affiliated organizations may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +# Check the NMEA sentence checksum. Return True if passes and False if failed +def check_nmea_checksum(nmea_sentence): + split_sentence = nmea_sentence.split('*') + if len(split_sentence) != 2: + #No checksum bytes were found... improperly formatted/incomplete NMEA data? + return False + transmitted_checksum = split_sentence[1].strip() + + #Remove the $ at the front + data_to_checksum = split_sentence[0][1:] + checksum = 0 + for c in data_to_checksum: + checksum ^= ord(c) + + return ("%02X" % checksum) == transmitted_checksum.upper() diff --git a/racecar_gps/src/libnmea_navsat_driver/driver.py b/racecar_gps/src/libnmea_navsat_driver/driver.py new file mode 100644 index 0000000..fc899b4 --- /dev/null +++ b/racecar_gps/src/libnmea_navsat_driver/driver.py @@ -0,0 +1,272 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Eric Perko +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the names of the authors nor the names of their +# affiliated organizations may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import math + +import rospy + +from sensor_msgs.msg import NavSatFix, NavSatStatus, TimeReference +from geometry_msgs.msg import TwistStamped, QuaternionStamped +from tf.transformations import quaternion_from_euler + +from libnmea_navsat_driver.checksum_utils import check_nmea_checksum +import libnmea_navsat_driver.parser + + +class RosNMEADriver(object): + + def __init__(self): + self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1) + self.vel_pub = rospy.Publisher('vel', TwistStamped, queue_size=1) + self.heading_pub = rospy.Publisher('heading', QuaternionStamped, queue_size=1) + self.time_ref_pub = rospy.Publisher('time_reference', TimeReference, queue_size=1) + + self.time_ref_source = rospy.get_param('~time_ref_source', None) + self.use_RMC = rospy.get_param('~useRMC', False) + + # epe = estimated position error + self.default_epe_quality0 = rospy.get_param('~epe_quality0', 1000000) + self.default_epe_quality1 = rospy.get_param('~epe_quality1', 4.0) + self.default_epe_quality2 = rospy.get_param('~epe_quality2', 0.1) + self.default_epe_quality4 = rospy.get_param('~epe_quality4', 0.02) + self.default_epe_quality5 = rospy.get_param('~epe_quality5', 4.0) + self.default_epe_quality9 = rospy.get_param('~epe_quality9', 3.0) + self.using_receiver_epe = False + + self.lon_std_dev = float("nan") + self.lat_std_dev = float("nan") + self.alt_std_dev = float("nan") + + """Format for this dictionary is the fix type from a GGA message as the key, with + each entry containing a tuple consisting of a default estimated + position error, a NavSatStatus value, and a NavSatFix covariance value.""" + self.gps_qualities = { + # Unknown + -1: [ + self.default_epe_quality0, + NavSatStatus.STATUS_NO_FIX, + NavSatFix.COVARIANCE_TYPE_UNKNOWN + ], + # Invalid + 0: [ + self.default_epe_quality0, + NavSatStatus.STATUS_NO_FIX, + NavSatFix.COVARIANCE_TYPE_UNKNOWN + ], + # SPS + 1: [ + self.default_epe_quality1, + NavSatStatus.STATUS_FIX, + NavSatFix.COVARIANCE_TYPE_APPROXIMATED + ], + # DGPS + 2: [ + self.default_epe_quality2, + NavSatStatus.STATUS_SBAS_FIX, + NavSatFix.COVARIANCE_TYPE_APPROXIMATED + ], + # RTK Fix + 4: [ + self.default_epe_quality4, + NavSatStatus.STATUS_GBAS_FIX, + NavSatFix.COVARIANCE_TYPE_APPROXIMATED + ], + # RTK Float + 5: [ + self.default_epe_quality5, + NavSatStatus.STATUS_GBAS_FIX, + NavSatFix.COVARIANCE_TYPE_APPROXIMATED + ], + # WAAS + 9: [ + self.default_epe_quality9, + NavSatStatus.STATUS_GBAS_FIX, + NavSatFix.COVARIANCE_TYPE_APPROXIMATED + ] + } + + # Returns True if we successfully did something with the passed in + # nmea_string + def add_sentence(self, nmea_string, frame_id, timestamp=None): + if not check_nmea_checksum(nmea_string): + rospy.logwarn("Received a sentence with an invalid checksum. " + + "Sentence was: %s" % repr(nmea_string)) + return False + + parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) + if not parsed_sentence: + rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) + return False + + if timestamp: + current_time = timestamp + else: + current_time = rospy.get_rostime() + current_fix = NavSatFix() + current_fix.header.stamp = current_time + current_fix.header.frame_id = frame_id + current_time_ref = TimeReference() + current_time_ref.header.stamp = current_time + current_time_ref.header.frame_id = frame_id + if self.time_ref_source: + current_time_ref.source = self.time_ref_source + else: + current_time_ref.source = frame_id + + if not self.use_RMC and 'GGA' in parsed_sentence: + current_fix.position_covariance_type = \ + NavSatFix.COVARIANCE_TYPE_APPROXIMATED + + data = parsed_sentence['GGA'] + fix_type = data['fix_type'] + if not (fix_type in self.gps_qualities): + fix_type = -1 + gps_qual = self.gps_qualities[fix_type] + default_epe = gps_qual[0]; + current_fix.status.status = gps_qual[1] + current_fix.position_covariance_type = gps_qual[2]; + current_fix.status.service = NavSatStatus.SERVICE_GPS + + latitude = data['latitude'] + if data['latitude_direction'] == 'S': + latitude = -latitude + current_fix.latitude = latitude + + longitude = data['longitude'] + if data['longitude_direction'] == 'W': + longitude = -longitude + current_fix.longitude = longitude + + # Altitude is above ellipsoid, so adjust for mean-sea-level + altitude = data['altitude'] + data['mean_sea_level'] + current_fix.altitude = altitude + + # use default epe std_dev unless we've received a GST sentence with epes + if not self.using_receiver_epe or math.isnan(self.lon_std_dev): + self.lon_std_dev = default_epe + if not self.using_receiver_epe or math.isnan(self.lat_std_dev): + self.lat_std_dev = default_epe + if not self.using_receiver_epe or math.isnan(self.alt_std_dev): + self.alt_std_dev = default_epe * 2 + + hdop = data['hdop'] + current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 + current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 + current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME + + self.fix_pub.publish(current_fix) + + if not math.isnan(data['utc_time']): + current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) + self.time_ref_pub.publish(current_time_ref) + + elif 'RMC' in parsed_sentence: + data = parsed_sentence['RMC'] + + # Only publish a fix from RMC if the use_RMC flag is set. + if self.use_RMC: + if data['fix_valid']: + current_fix.status.status = NavSatStatus.STATUS_FIX + else: + current_fix.status.status = NavSatStatus.STATUS_NO_FIX + + current_fix.status.service = NavSatStatus.SERVICE_GPS + + latitude = data['latitude'] + if data['latitude_direction'] == 'S': + latitude = -latitude + current_fix.latitude = latitude + + longitude = data['longitude'] + if data['longitude_direction'] == 'W': + longitude = -longitude + current_fix.longitude = longitude + + current_fix.altitude = float('NaN') + current_fix.position_covariance_type = \ + NavSatFix.COVARIANCE_TYPE_UNKNOWN + + self.fix_pub.publish(current_fix) + + if not math.isnan(data['utc_time']): + current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) + self.time_ref_pub.publish(current_time_ref) + + # Publish velocity from RMC regardless, since GGA doesn't provide it. + if data['fix_valid']: + current_vel = TwistStamped() + current_vel.header.stamp = current_time + current_vel.header.frame_id = frame_id + current_vel.twist.linear.x = data['speed'] * \ + math.sin(data['true_course']) + current_vel.twist.linear.y = data['speed'] * \ + math.cos(data['true_course']) + self.vel_pub.publish(current_vel) + elif 'GST' in parsed_sentence: + data = parsed_sentence['GST'] + + # Use receiver-provided error estimate if available + self.using_receiver_epe = True + self.lon_std_dev = data['lon_std_dev'] + self.lat_std_dev = data['lat_std_dev'] + self.alt_std_dev = data['alt_std_dev'] + elif 'HDT' in parsed_sentence: + data = parsed_sentence['HDT'] + if data['heading']: + current_heading = QuaternionStamped() + current_heading.header.stamp = current_time + current_heading.header.frame_id = frame_id + q = quaternion_from_euler(0, 0, math.radians(data['heading'])) + current_heading.quaternion.x = q[0] + current_heading.quaternion.y = q[1] + current_heading.quaternion.z = q[2] + current_heading.quaternion.w = q[3] + self.heading_pub.publish(current_heading) + else: + return False + + """Helper method for getting the frame_id with the correct TF prefix""" + + @staticmethod + def get_frame_id(): + frame_id = rospy.get_param('~frame_id', 'gps') + if frame_id[0] != "/": + """Add the TF prefix""" + prefix = "" + prefix_param = rospy.search_param('tf_prefix') + if prefix_param: + prefix = rospy.get_param(prefix_param) + if prefix[0] != "/": + prefix = "/%s" % prefix + return "%s/%s" % (prefix, frame_id) + else: + return frame_id diff --git a/racecar_gps/src/libnmea_navsat_driver/parser.py b/racecar_gps/src/libnmea_navsat_driver/parser.py new file mode 100644 index 0000000..f468c1f --- /dev/null +++ b/racecar_gps/src/libnmea_navsat_driver/parser.py @@ -0,0 +1,163 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Eric Perko +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the names of the authors nor the names of their +# affiliated organizations may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import re +import time +import calendar +import math +import logging +logger = logging.getLogger('rosout') + + +def safe_float(field): + try: + return float(field) + except ValueError: + return float('NaN') + + +def safe_int(field): + try: + return int(field) + except ValueError: + return 0 + + +def convert_latitude(field): + return safe_float(field[0:2]) + safe_float(field[2:]) / 60.0 + + +def convert_longitude(field): + return safe_float(field[0:3]) + safe_float(field[3:]) / 60.0 + + +def convert_time(nmea_utc): + # Get current time in UTC for date information + utc_struct = time.gmtime() # immutable, so cannot modify this one + utc_list = list(utc_struct) + # If one of the time fields is empty, return NaN seconds + if not nmea_utc[0:2] or not nmea_utc[2:4] or not nmea_utc[4:6]: + return float('NaN') + else: + hours = int(nmea_utc[0:2]) + minutes = int(nmea_utc[2:4]) + seconds = int(nmea_utc[4:6]) + utc_list[3] = hours + utc_list[4] = minutes + utc_list[5] = seconds + unix_time = calendar.timegm(tuple(utc_list)) + return unix_time + + +def convert_status_flag(status_flag): + if status_flag == "A": + return True + elif status_flag == "V": + return False + else: + return False + + +def convert_knots_to_mps(knots): + return safe_float(knots) * 0.514444444444 + + +# Need this wrapper because math.radians doesn't auto convert inputs +def convert_deg_to_rads(degs): + return math.radians(safe_float(degs)) + +"""Format for this dictionary is a sentence identifier (e.g. "GGA") as the key, with a +list of tuples where each tuple is a field name, conversion function and index +into the split sentence""" +parse_maps = { + "GGA": [ + ("fix_type", int, 6), + ("latitude", convert_latitude, 2), + ("latitude_direction", str, 3), + ("longitude", convert_longitude, 4), + ("longitude_direction", str, 5), + ("altitude", safe_float, 9), + ("mean_sea_level", safe_float, 11), + ("hdop", safe_float, 8), + ("num_satellites", safe_int, 7), + ("utc_time", convert_time, 1), + ], + "RMC": [ + ("utc_time", convert_time, 1), + ("fix_valid", convert_status_flag, 2), + ("latitude", convert_latitude, 3), + ("latitude_direction", str, 4), + ("longitude", convert_longitude, 5), + ("longitude_direction", str, 6), + ("speed", convert_knots_to_mps, 7), + ("true_course", convert_deg_to_rads, 8), + ], + "GST": [ + ("utc_time", convert_time, 1), + ("ranges_std_dev", safe_float, 2), + ("semi_major_ellipse_std_dev", safe_float, 3), + ("semi_minor_ellipse_std_dev", safe_float, 4), + ("semi_major_orientation", safe_float, 5), + ("lat_std_dev", safe_float, 6), + ("lon_std_dev", safe_float, 7), + ("alt_std_dev", safe_float, 8), + ], + "HDT": [ + ("heading", safe_float, 1), + ] + } + + +def parse_nmea_sentence(nmea_sentence): + # Check for a valid nmea sentence + + if not re.match('(^\$GP|^\$GN|^\$GL).*\*[0-9A-Fa-f]{2}$', nmea_sentence): + logger.debug("Regex didn't match, sentence not valid NMEA? Sentence was: %s" + % repr(nmea_sentence)) + return False + fields = [field.strip(',') for field in nmea_sentence.split(',')] + + # Ignore the $ and talker ID portions (e.g. GP) + sentence_type = fields[0][3:] + + if not sentence_type in parse_maps: + logger.debug("Sentence type %s not in parse map, ignoring." + % repr(sentence_type)) + return False + + parse_map = parse_maps[sentence_type] + + parsed_sentence = {} + for entry in parse_map: + parsed_sentence[entry[0]] = entry[1](fields[entry[2]]) + + return {sentence_type: parsed_sentence}