Llh support

CMakeLists.txt

@@ -1,10 +1,25 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(reach_ros_node)
 
-find_package(catkin REQUIRED COMPONENTS roslint)
+find_package(catkin REQUIRED COMPONENTS
+  std_msgs
+  roslint
+  message_generation
+)
 
 catkin_python_setup()
-catkin_package()
+
+add_message_files(
+  FILES
+  LLH.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs)
 
 install(PROGRAMS
    scripts/nmea_tcp_driver

launch/reach_ros_node.launch

@@ -0,0 +1,11 @@
+<launch>
+    <node name="gps" pkg="reach_ros_node" type="nmea_tcp_driver" respawn="true">
+        <param name="~frame_gps" value="base_link"/>
+        <param name="~host" value="192.168.2.15" />
+        <param name="~port" value="12346" />
+
+        <!-- Optional LLH output. The Reach must be configured to
+        output LLH. Delete this entry if not needed. -->
+        <param name="~llh_port" value="12347" />
+    </node>
+</launch>

msg/LLH.msg

@@ -0,0 +1,29 @@
+# Positioning Solution Status. See page 101 in doc/rtklib_manaual_2.4.2.pdf.
+Header header
+string date
+string utc_time
+float64 latitude
+float64 longitude
+float64 altitude
+
+# Quality of our fix.
+uint8 QUALITY_FLAG_FIX = 1
+uint8 QUALITY_FLAG_FLOAT = 2
+uint8 QUALITY_FLAG_RESERVED = 3
+uint8 QUALITY_FLAG_DGPS = 4
+uint8 QUALITY_FLAG_SINGLE = 5
+uint8 quality_flag
+
+# Number of valid satellites.
+uint8 num_sats
+
+float64[9] position_covariance
+uint8 COVARIANCE_TYPE_UNKNOWN = 0
+uint8 COVARIANCE_TYPE_APPROXIMATED = 1
+uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
+uint8 COVARIANCE_TYPE_KNOWN = 3
+uint8 position_covariance_type
+
+# RTKLIB-specific stats.
+float64 diff_age
+float64 ratio

package.xml

@@ -16,12 +16,16 @@
 
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>roslint</build_depend>
   <run_depend>rospy</run_depend>
   <run_depend>python-serial</run_depend>
   <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
   <run_depend>nmea_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <!-- This is a pure Python package, so mark it architecture independent -->

requirements.txt

@@ -0,0 +1,2 @@
+py
+pytest

scripts/nmea_tcp_driver

@@ -32,46 +32,45 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import serial
 import socket
+import threading
+
 import rospy
-import os
-import sys
-import reach_ros_node.driver
+
+from reach_ros_node.driver import RosLLHDriver, RosNMEADriver
 
 
-class ReachSocketHandler:
+class ReachSocketHandler(threading.Thread):
 
     # Set our parameters and the default socket to open
-    def __init__(self,host,port):
-        self.host = host
-        self.port = port
-
-
-    # Should open the connection and connect to the device
-    # This will then also start publishing the information
-    def start(self):
-        # Try to connect to the device
-        rospy.loginfo('Connecting to Reach RTK %s on port %s' % (str(self.host),str(self.port)))
+    def __init__(self, hostname, tcp_port, format_nmea_driver):
+        self.host = hostname
+        self.port = tcp_port
+        self.nmea_driver = format_nmea_driver
+        self.soc = None
+        threading.Thread.__init__(self)
+
+    # Should open the connection and connect to the device.
+    # This will then also start publishing the information.
+    def run(self):
+        # Try to connect to the device.
+        rospy.loginfo('Connecting to Reach RTK %s on port %s' %
+                      (str(self.host), str(self.port)))
         self.connect_to_device()
-        try:
-            # Create the driver
-            driver = reach_ros_node.driver.RosNMEADriver()
-            while not rospy.is_shutdown():
-                #GPS = soc.recv(1024)
-                data = self.buffered_readLine().strip()
-                # Debug print message line
-                #print(data)
-                # Try to parse this data!
-                try:
-                    driver.process_line(data)
-                except ValueError as e:
-                    rospy.logerr("Value error, likely due to missing fields in the NMEA message. Error was: %s." % e)
-        except rospy.ROSInterruptException:
-            # Close GPS socket when done
-            self.soc.close()
-
+        while not rospy.is_shutdown():
+            data = self.buffered_readLine().strip()
+            rospy.logdebug(data)
 
+            # Try to parse this data!
+            try:
+                self.nmea_driver.process_line(data)
+            except ValueError as e:
+                rospy.logerr("Value error, likely due to missing fields in "
+                             "the message. Error was: %s." % e)
+        # Close GPS socket when done.
+        self.soc.close()
+        rospy.logdebug("ReachSocketHandler.run() (port %d) exiting." %
+                       self.port)
 
     # Try to connect to the device, allows for reconnection
     # Will loop till we get a connection, note we have a long timeout
@@ -80,30 +79,28 @@ class ReachSocketHandler:
             try:
                 self.soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                 self.soc.settimeout(5.0)
-                self.soc.connect((self.host,self.port))
-                rospy.loginfo('Successfully connected to device, starting publishing!')
+                self.soc.connect((self.host, self.port))
+                rospy.loginfo('Successfully connected to device, '
+                              'starting publishing!')
                 return
             except socket.timeout:
-                rospy.logwarn_throttle(30,'Socket connection timeout. Retrying...')
+                rospy.logwarn_throttle(30, 'Socket connection timeout. '
+                                           'Retrying...')
                 continue
             except Exception as e:
                 rospy.logerr("Socket connection error. Error was: %s." % e)
-                exit()
 
-
-    # Try to connect to the device, assuming it just was disconnected
-    # Will loop till we get a connection
+    # Try to connect to the device, assuming it just was disconnected.
+    # Will loop till we get a connection.
     def reconnect_to_device(self):
         rospy.logwarn('Device disconnected. Reconnecting...')
         self.soc.close()
         self.connect_to_device()
 
-
-
-    # Read one line from the socket
-    # We want to read a single line as this is a single nmea message
+    # Read one line from the socket.
+    # We want to read a single line as this as a single message:
     # https://stackoverflow.com/a/41333900
-    # Also set a timeout so we can make sure we have a valid socket
+    # Also set a timeout so we can make sure we have a valid socket:
     # https://stackoverflow.com/a/15175067
     def buffered_readLine(self):
         line = ""
@@ -125,20 +122,33 @@ class ReachSocketHandler:
         return line
 
 
-
 if __name__ == '__main__':
 
     # Initialize our ros node
     rospy.init_node('reach_ros_node')
 
     # Read in ROS parameters
-    host = rospy.get_param('~host', 'reach.local')
-    port = rospy.get_param('~port', 12346)
-
-    # Open the socket to our device, and start streaming the data
-    device = ReachSocketHandler(host,port)
-    device.start()
-
-
-
-
+    host = rospy.get_param('~host', '127.0.0.1')
+    nmea_port = rospy.get_param('~port', 12346)
+
+    # Open the socket to our nmea_sock_handler, and start streaming the data.
+    nmea_driver = RosNMEADriver()
+    nmea_sock_handler = ReachSocketHandler(host, nmea_port, nmea_driver)
+    nmea_sock_handler.start()
+
+    # If specified, create and run the (optional) LLH driver
+    llh_port = rospy.get_param('~llh_port', 12347)
+    llh_sock_handler = None
+    if llh_port:
+        llh_driver = RosLLHDriver()
+        llh_sock_handler = ReachSocketHandler(host, llh_port, llh_driver)
+        llh_sock_handler.start()
+
+    # Run until shutdown.
+    rospy.spin()
+
+    # Wait for SocketHandler to finish, then exit.
+    nmea_sock_handler.join()
+    if llh_sock_handler:
+        llh_sock_handler.join()
+    rospy.logdebug("__main()__ exiting.")

src/reach_ros_node/__init__.py

@@ -0,0 +1,6 @@
+# See doc/rtklib_manual_2.4.2.pdf.
+QUALITY_FLAG_FIX = 1
+QUALITY_FLAG_FLOAT = 2
+QUALITY_FLAG_RESERVED = 3
+QUALITY_FLAG_DGPS = 4
+QUALITY_FLAG_SINGLE = 5

src/reach_ros_node/checksum_utils.py

@@ -35,11 +35,12 @@
 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?
+        # No checksum bytes were found...
+        # improperly formatted/incomplete NMEA data?
         return False
     transmitted_checksum = split_sentence[1].strip()
 
-    #Remove the $ at the front
+    # Remove the $ at the front
     data_to_checksum = split_sentence[0][1:]
     checksum = 0
     for c in data_to_checksum: