Merge pull request #17 from jkretzmer/tcp_support

adding tcp connection support

piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml

@@ -1,21 +1,60 @@
-# General Settings.
-serial_port: '/dev/ttyUSB0'
-baud_rate: 230400
+# General Settings
+
+# interface: specifies the interface over which to connect.
+# supported values are 'serial' and 'tcp'
+# if this value is not valid, the driver will default to serial communication
+interface: 'serial'
+
+# set this to true if the Swift device is acting as RTK Base Station.
 base_station_mode: false
-debug_mode: false # if true, output every topic read by this driver
+
+# if true, output every topic read by this driver
+debug_mode: false
+
+# if true the driver is verbose.
 driver_verbose: true
+
+# broadcast_addr: ip address to use for broadcasting corrections
 # whenever possible, use network broadcast, not global ip broadcast
 # use ifconfig to show network broadcast (or calculate it yourself....).
 broadcast_addr: 192.168.0.255
+
+# broadcast_port: port to use for broadcasting corrections
 broadcast_port: 26078
-base_station_ip_for_latency_estimation: 192.168.0.1 # ip base station if you want to estimate latency.
+
+# base_station_ip_for_latency_estimation: IP of base station for
+# latency estimation.
+base_station_ip_for_latency_estimation: 192.168.0.1
+
 # Coordinate frame of the GPS receiver with respect to the base link.
 navsatfix_frame_id: 'base_link'
 
+# Covariance Settings
 # Variance to be published in the NavSatFix message (in diagonal part).
 # Single Point Positioning (SPP).
 var_spp: [25.0, 25.0, 64.0]
 # RTK float mode.
 var_rtk_float: [25.0, 25.0, 64.0]
 # RTK fix mode.
 var_rtk_fix: [0.0049, 0.0049, 0.01]
+
+# Serial Settings
+
+# serial_port: defines which serial device to use for communication
+# ex '/dev/ttyUSB0'
+serial_port: '/dev/ttyUSB0'
+
+# baud_rate: defines the rate for the serial connection. This driver uses
+# 230400 as the default. Swift Navigation receivers ship with a
+# default baud rate of 115200.
+baud_rate: 230400
+
+# TCP Settings
+
+# tcp_addr: the IP address of the receiver. By default, Swift Navigation
+# receivers have an IP address of 192.168.0.222.
+tcp_addr: 192.168.0.222
+
+# tcp_port: the port number of the TCP server. By default, Swift Navigation
+# receivers publish SBP data on TCP port 55555.
+tcp_port: 55555

piksi_multi_rtk_ros/src/piksi_multi.py

@@ -19,8 +19,10 @@
     Transform
 # Import Piksi SBP library
 from sbp.client.drivers.pyserial_driver import PySerialDriver
+from sbp.client.drivers.network_drivers import TCPDriver
 from sbp.client import Handler, Framer
 from sbp.navigation import *
+from sbp.orientation import *
 from sbp.logging import *
 from sbp.system import *
 from sbp.tracking import *  # WARNING: tracking is part of the draft messages, could be removed in future releases of libsbp.
@@ -75,14 +77,24 @@ def __init__(self):
                               installed_sbp_version, PiksiMulti.LIB_SBP_VERSION_MULTI))
 
         # Open a connection to Piksi.
-        serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
-        baud_rate = rospy.get_param('~baud_rate', 115200)
-
-        try:
-            self.driver = PySerialDriver(serial_port, baud=baud_rate)
-        except SystemExit:
-            rospy.logerr("Piksi not found on serial port '%s'", serial_port)
-            raise
+        interface = rospy.get_param('~interface', 'serial')
+
+        if interface == 'tcp':
+            tcp_addr = rospy.get_param('~tcp_addr', '192.168.0.222')
+            tcp_port = rospy.get_param('~tcp_port', 55555)
+            try:
+                self.driver = TCPDriver(tcp_addr, tcp_port)
+            except SystemExit:
+                rospy.logerr("Unable to open TCP connection %s:%s", (tcp_addr, tcp_port))
+                raise
+        else:
+            serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
+            baud_rate = rospy.get_param('~baud_rate', 230400)
+            try:
+                self.driver = PySerialDriver(serial_port, baud=baud_rate)
+            except SystemExit:
+                rospy.logerr("Swift receiver not found on serial port '%s'", serial_port)
+                raise
 
         # Create a handler to connect Piksi driver to callbacks.
         self.driver_verbose = rospy.get_param('~driver_verbose', True)
@@ -91,13 +103,13 @@ def __init__(self):
 
         self.debug_mode = rospy.get_param('~debug_mode', False)
         if self.debug_mode:
-            rospy.loginfo("Piksi driver started in debug mode, every available topic will be published.")
+            rospy.loginfo("Swift driver started in debug mode, every available topic will be published.")
             # Debugging parameters.
             debug_delayed_corrections_stack_size = rospy.get_param('~debug_delayed_corrections_stack_size', 10)
             self.received_corrections_fifo_stack = collections.deque([], debug_delayed_corrections_stack_size)
             rospy.loginfo("Debug mode: delayed corrections stack size: %d." % debug_delayed_corrections_stack_size)
         else:
-            rospy.loginfo("Piksi driver started in normal mode.")
+            rospy.loginfo("Swift driver started in normal mode.")
 
         # Corrections over WiFi settings.
         self.base_station_mode = rospy.get_param('~base_station_mode', False)
@@ -905,17 +917,17 @@ def reset_piksi_service_callback(self, request):
             reset_msg = reset_sbp.pack()
             self.driver.write(reset_msg)
 
-            rospy.logwarn("Piksi hard reset via rosservice call.")
+            rospy.logwarn("Swift receiver hard reset via rosservice call.")
 
             # Init messages with "memory".
             self.receiver_state_msg = self.init_receiver_state_msg()
             self.num_wifi_corrections = self.init_num_corrections_msg()
 
             response.success = True
-            response.message = "Piksi reset command sent."
+            response.message = "Swift receiver reset command sent."
         else:
             response.success = False
-            response.message = "Piksi reset command not sent."
+            response.message = "Swift receiver reset command not sent."
 
         return response
 
@@ -965,7 +977,7 @@ def settings_save_callback(self, request):
         if request.data:
             self.settings_save()
             response.success = True
-            response.message = "Piksi settings have been saved to flash."
+            response.message = "Swift receiver settings have been saved to flash."
         else:
             response.success = False
             response.message = "Please pass 'true' to this service call to explicitly save to flash the local settings."
@@ -993,7 +1005,7 @@ def get_installed_sbp_version(self):
 
     def settings_write(self, section_setting, setting, value):
         """
-        Write the defined configuration to Piksi.
+        Write the defined configuration to Swift receiver.
         """
         setting_string = '%s\0%s\0%s\0' % (section_setting, setting, value)
         write_msg = MsgSettingsWrite(setting=setting_string)
@@ -1009,7 +1021,7 @@ def settings_save(self):
 
     def settings_read_req(self, section_setting, setting):
         """
-        Request a configuration value to Piksi.
+        Request a configuration value to Swift receiver.
         """
         setting_string = '%s\0%s\0' % (section_setting, setting)
         read_req_msg = MsgSettingsReadReq(setting=setting_string)
@@ -1027,7 +1039,7 @@ def settings_read_resp(self, msg_raw, **metadata):
 
     def settings_read_by_index_req(self, index):
         """
-        Request a configuration value to Piksi by parameter index number.
+        Request a configuration value to Swift receiver by parameter index number.
         """
         read_req_by_index_msg = MsgSettingsReadByIndexReq(index=index)
         self.framer(read_req_by_index_msg)