From da8081dbee4435b9bb3aba8b94369ac5a115b2ab Mon Sep 17 00:00:00 2001 From: Josh Kretzmer Date: Mon, 26 Mar 2018 09:09:58 -0700 Subject: [PATCH 1/3] adding tcp connection support --- .../cfg/piksi_multi_driver_settings.yaml | 46 +++++++++++++++-- .../install/install_piksi_multi.sh | 2 +- piksi_multi_rtk_ros/src/piksi_multi.py | 50 ++++++++++++------- 3 files changed, 73 insertions(+), 25 deletions(-) diff --git a/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml b/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml index 407221f8..a2ac0534 100644 --- a/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml +++ b/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml @@ -1,16 +1,31 @@ -# 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' + base_station_mode: false -debug_mode: false # if true, output every topic read by this driver + +# debug_mode: if true, output every topic read by this driver # whenever possible, use network broadcast, not global ip broadcast # use ifconfig to show network broadcast (or calculate it yourself....). +debug_mode: false + +# broadcast_addr: ip address to use for broadcasting corrections 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] @@ -18,3 +33,24 @@ var_spp: [25.0, 25.0, 64.0] 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 +# 234000 as the default. Swift Navigation receivers ship with a +# default baud rate of 115200. +baud_rate: 234000 + +# 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 diff --git a/piksi_multi_rtk_ros/install/install_piksi_multi.sh b/piksi_multi_rtk_ros/install/install_piksi_multi.sh index 0eb31820..a2dff6aa 100755 --- a/piksi_multi_rtk_ros/install/install_piksi_multi.sh +++ b/piksi_multi_rtk_ros/install/install_piksi_multi.sh @@ -1,7 +1,7 @@ #!/bin/bash GIT_REPO_LIBSBP=https://github.com/swift-nav/libsbp.git -REPO_TAG=v2.2.15 #version you want to checkout before installing +REPO_TAG=v2.3.11 #version you want to checkout before installing # Sort of reg express used to see if there is another verision on SBP library already installed # Adapted from https://stackoverflow.com/questions/6363441/check-if-a-file-exists-with-wildcard-in-shell-script diff --git a/piksi_multi_rtk_ros/src/piksi_multi.py b/piksi_multi_rtk_ros/src/piksi_multi.py index bd98bdcc..66283d60 100755 --- a/piksi_multi_rtk_ros/src/piksi_multi.py +++ b/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. @@ -45,7 +47,7 @@ class PiksiMulti: - LIB_SBP_VERSION_MULTI = '2.2.15' # SBP version used for Piksi Multi. + LIB_SBP_VERSION_MULTI = '2.3.11' # SBP version used for Piksi Multi. # Geodetic Constants. kSemimajorAxis = 6378137 @@ -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', 23400) + 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.framer = Framer(self.driver.read, self.driver.write, verbose=True) @@ -90,13 +102,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) @@ -427,7 +439,7 @@ def callback(msg, **metadata): for attr in attrs: if attr == 'flags': # Least significat three bits of flags indicate status. - if (msg.flags & 0x07) == 0: + if (sbp_message.flags & 0x07) == 0: return # Invalid message, do not publish it. setattr(ros_message, attr, getattr(sbp_message, attr)) @@ -904,17 +916,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 @@ -964,7 +976,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." @@ -992,7 +1004,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) @@ -1008,7 +1020,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) @@ -1026,7 +1038,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) From ff1dd5f4949e2aa0ccdf7bdf8dfd741ec786ef4b Mon Sep 17 00:00:00 2001 From: Josh Kretzmer Date: Mon, 26 Mar 2018 10:01:55 -0700 Subject: [PATCH 2/3] update to PR based on feedback from ETHZ --- piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml | 8 ++++---- piksi_multi_rtk_ros/src/piksi_multi.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml b/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml index a2ac0534..fb7fa76d 100644 --- a/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml +++ b/piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml @@ -8,11 +8,11 @@ interface: 'serial' base_station_mode: false # debug_mode: if true, output every topic read by this driver -# whenever possible, use network broadcast, not global ip broadcast -# use ifconfig to show network broadcast (or calculate it yourself....). debug_mode: false # 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 @@ -41,9 +41,9 @@ var_rtk_fix: [0.0049, 0.0049, 0.01] serial_port: '/dev/ttyUSB0' # baud_rate: defines the rate for the serial connection. This driver uses -# 234000 as the default. Swift Navigation receivers ship with a +# 230400 as the default. Swift Navigation receivers ship with a # default baud rate of 115200. -baud_rate: 234000 +baud_rate: 230400 # TCP Settings diff --git a/piksi_multi_rtk_ros/src/piksi_multi.py b/piksi_multi_rtk_ros/src/piksi_multi.py index 66283d60..7f1ec14b 100755 --- a/piksi_multi_rtk_ros/src/piksi_multi.py +++ b/piksi_multi_rtk_ros/src/piksi_multi.py @@ -89,7 +89,7 @@ def __init__(self): raise else: serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0') - baud_rate = rospy.get_param('~baud_rate', 23400) + baud_rate = rospy.get_param('~baud_rate', 230400) try: self.driver = PySerialDriver(serial_port, baud=baud_rate) except SystemExit: From bf9fc5df04d8d9e04f159fcaac61d0a91cc4c669 Mon Sep 17 00:00:00 2001 From: Josh Kretzmer Date: Mon, 26 Mar 2018 10:04:00 -0700 Subject: [PATCH 3/3] reverting SBP to 2.2.15 for PR --- piksi_multi_rtk_ros/install/install_piksi_multi.sh | 2 +- piksi_multi_rtk_ros/src/piksi_multi.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/piksi_multi_rtk_ros/install/install_piksi_multi.sh b/piksi_multi_rtk_ros/install/install_piksi_multi.sh index a2dff6aa..0eb31820 100755 --- a/piksi_multi_rtk_ros/install/install_piksi_multi.sh +++ b/piksi_multi_rtk_ros/install/install_piksi_multi.sh @@ -1,7 +1,7 @@ #!/bin/bash GIT_REPO_LIBSBP=https://github.com/swift-nav/libsbp.git -REPO_TAG=v2.3.11 #version you want to checkout before installing +REPO_TAG=v2.2.15 #version you want to checkout before installing # Sort of reg express used to see if there is another verision on SBP library already installed # Adapted from https://stackoverflow.com/questions/6363441/check-if-a-file-exists-with-wildcard-in-shell-script diff --git a/piksi_multi_rtk_ros/src/piksi_multi.py b/piksi_multi_rtk_ros/src/piksi_multi.py index 7f1ec14b..1c02a92b 100755 --- a/piksi_multi_rtk_ros/src/piksi_multi.py +++ b/piksi_multi_rtk_ros/src/piksi_multi.py @@ -47,7 +47,7 @@ class PiksiMulti: - LIB_SBP_VERSION_MULTI = '2.3.11' # SBP version used for Piksi Multi. + LIB_SBP_VERSION_MULTI = '2.2.15' # SBP version used for Piksi Multi. # Geodetic Constants. kSemimajorAxis = 6378137