Merge pull request #5 from tianbot/dev

Dev

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"

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"

racecar_bringup/launch/gps.launch

@@ -3,8 +3,8 @@
 <launch>
   <arg name="gps" default="$(optenv TIANBOT_RACECAR_GPS none)" />
 
-  <node pkg="nmea_navat_driver" type="nmea_serial_driver" name="tianbot_racecar_gps" if="$(eval gps=='nmea0183')" >
-    <param name="port" value="/dev/ttyUSB2" />
+  <node pkg="racecar_gps" type="nmea_serial_driver" name="tianbot_racecar_gps" if="$(eval gps=='nmea0183')" >
+    <param name="port" value="/dev/tianbot_racecar_gps" />
     <param name="baud" value="38400" />
   </node>
 </launch>

racecar_description/launch/includes/standard_velodyne_tf.launch.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+
+<launch>
+    <node pkg="tf" type="static_transform_publisher" name="base_footprint2base_link" args="0 0 0.055 0 0 0 base_footprint base_link 10"/>
+    <node pkg="tf" type="static_transform_publisher" name="base_link2laser" args="0.22 0 0.1 0 0 0 base_link laser 10"/>
+    <node pkg="tf" type="static_transform_publisher" name="base_link2velodyne" args="0.22 0 0.1 0 0 0 base_link velodyne 10"/>
+    <node pkg="tf" type="static_transform_publisher" name="base_link2gps" args="0.20 0.05 0.1 0 0 0 base_link gps 10"/>
+    <node pkg="tf" type="static_transform_publisher" name="base_link2imu" args="0.22 0 0.1 0 0 0 base_link IMU_link 10"/>
+    <node pkg="tf" type="static_transform_publisher" name="base_link2camera" args="0.36 0 0.055 0 0 0 base_link camera_link 10"/>
+</launch>
+

racecar_description/launch/tianbot_racecar_tf.launch

@@ -2,8 +2,15 @@
 
 <launch>
   <arg name="base" default="$(optenv TIANBOT_RACECAR_BASE compact)" />
+  <arg name="lidar" default="$(optenv TIANBOT_RACECAR_LIDAR rplidar_a2)" />
 
   <include if="$(eval base == 'compact')" file="$(find racecar_description)/launch/includes/compact_tf.launch.xml" />
-  <include if="$(eval base == 'standard')" file="$(find racecar_description)/launch/includes/standard_tf.launch.xml" />
+
+  <group if="$(eval base == 'standard')">
+    <include if="$(eval 'rplidar' in lidar)" file="$(find racecar_description)/launch/includes/standard_rplidar_tf.launch.xml" />
+    <include if="$(eval 'velodyne' in lidar)" file="$(find racecar_description)/launch/includes/standard_velodyne_tf.launch.xml" />
+  </group>
+
+
   <include if="$(eval base == 'fullsize')" file="$(find racecar_description)/launch/includes/fullsize_tf.launch.xml" />
 </launch>

racecar_gps/.gitignore

@@ -0,0 +1,4 @@
+build/
+*.pyc
+*.pyo
+*.swp

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

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()

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

racecar_gps/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>racecar_gps</name>
+  <version>0.5.0</version>
+  <description>
+    Package to parse NMEA strings and publish a very simple GPS message. Does not 
+    require or use the GPSD deamon.
+  </description>
+
+  <maintainer email="evenator@gmail.com">Ed Venator</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/nmea_navsat_driver</url>
+
+  <author email="eric@ericperko.com">Eric Perko</author>
+  <author>Steven Martin</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslint</build_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>python-serial</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nmea_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+  <test_depend>roslint</test_depend>
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- This is a pure Python package, so mark it architecture independent -->
+    <architecture_independent/>
+  </export>
+</package>

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))

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

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()