From 8f3f65839518b62dc5f28fb9e86c8954857120d1 Mon Sep 17 00:00:00 2001 From: lights0123 Date: Mon, 25 May 2020 17:38:22 -0400 Subject: [PATCH] Add Python 3 support --- CMakeLists.txt | 6 +++--- scripts/nmea_tcp_driver | 6 +++--- src/reach_ros_node/checksum_utils.py | 9 ++++++--- src/reach_ros_node/driver.py | 24 ++++++++++++------------ src/reach_ros_node/parser.py | 18 +++++++++--------- 5 files changed, 33 insertions(+), 30 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ae7f43c..c764e1e 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,9 +6,9 @@ find_package(catkin REQUIRED COMPONENTS roslint) catkin_python_setup() catkin_package() -install(PROGRAMS - scripts/nmea_tcp_driver - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +catkin_install_python(PROGRAMS + scripts/nmea_tcp_driver + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) # Check package for pep8 style, add a test to fail on violations. diff --git a/scripts/nmea_tcp_driver b/scripts/nmea_tcp_driver index cee95c4..c15ff93 100755 --- a/scripts/nmea_tcp_driver +++ b/scripts/nmea_tcp_driver @@ -106,7 +106,7 @@ class ReachSocketHandler: # Also set a timeout so we can make sure we have a valid socket # https://stackoverflow.com/a/15175067 def buffered_readLine(self): - line = "" + line = b"" while not rospy.is_shutdown(): # Try to get data from it try: @@ -118,9 +118,9 @@ class ReachSocketHandler: if not part or len(part) == 0: self.reconnect_to_device() continue - if part != "\n": + if part != b"\n": line += part - elif part == "\n": + elif part == b"\n": break return line diff --git a/src/reach_ros_node/checksum_utils.py b/src/reach_ros_node/checksum_utils.py index 899ae38..5fa9429 100644 --- a/src/reach_ros_node/checksum_utils.py +++ b/src/reach_ros_node/checksum_utils.py @@ -33,7 +33,7 @@ # Check the NMEA sentence checksum. Return True if passes and False if failed def check_nmea_checksum(nmea_sentence): - split_sentence = nmea_sentence.split('*') + split_sentence = nmea_sentence.split(b'*') if len(split_sentence) != 2: #No checksum bytes were found... improperly formatted/incomplete NMEA data? return False @@ -43,6 +43,9 @@ def check_nmea_checksum(nmea_sentence): data_to_checksum = split_sentence[0][1:] checksum = 0 for c in data_to_checksum: - checksum ^= ord(c) + if type(c) is int: + checksum ^= c + else: + checksum ^= ord(c) - return ("%02X" % checksum) == transmitted_checksum.upper() + return (b"%02X" % checksum) == transmitted_checksum.upper() diff --git a/src/reach_ros_node/driver.py b/src/reach_ros_node/driver.py index 7a1d27c..ef9d38b 100644 --- a/src/reach_ros_node/driver.py +++ b/src/reach_ros_node/driver.py @@ -113,13 +113,13 @@ def process_line(self, nmea_string): # Parses the GGA NMEA message type def parse_GGA(self,datag): # Check if we should parse this message - if not 'GGA' in datag: + if not b'GGA' in datag: return # Return if are using RCm if self.use_rmc: return # Else lets set what variables we can - data = datag['GGA'] + data = datag[b'GGA'] # If using ROS time, use the current timestamp if self.use_rostime: self.msg_fix.header.stamp = rospy.get_rostime() @@ -159,13 +159,13 @@ def parse_GGA(self,datag): # Parses the GST NMEA message type def parse_GST(self,datag): # Check if we should parse this message - if not 'GST' in datag: + if not b'GST' in datag: return # Return if are using RCM if self.use_rmc: return # Else lets set what variables we can - data = datag['GST'] + data = datag[b'GST'] # Update the fix message with std self.msg_fix.position_covariance[0] = pow(data['latitude_sigma'],2) self.msg_fix.position_covariance[4] = pow(data['longitude_sigma'],2) @@ -179,13 +179,13 @@ def parse_GST(self,datag): # Parses the VTG NMEA message type def parse_VTG(self,datag): # Check if we should parse this message - if not 'VTG' in datag: + if not b'VTG' in datag: return # Return if are using RCM if self.use_rmc: return # Else lets set what variables we can - data = datag['VTG'] + data = datag[b'VTG'] # Next lets publish the velocity we have # If using ROS time, use the current timestamp # Else this message doesn't provide a time, so just set to zero @@ -206,13 +206,13 @@ def parse_VTG(self,datag): # Parses the RMC NMEA message type def parse_RMC(self,datag): # Check if we should parse this message - if not 'RMC' in datag: + if not b'RMC' in datag: return # Return if not using RCM if not self.use_rmc: return # Else lets set what variables we can - data = datag['RMC'] + data = datag[b'RMC'] # If using ROS time, use the current timestamp if self.use_rostime: self.msg_fix.header.stamp = rospy.get_rostime() @@ -260,10 +260,10 @@ def parse_RMC(self,datag): def parse_time(self,datag): # Get our message data - if not self.use_rmc and 'GGA' in datag: - data = datag['GGA'] - elif self.use_rmc and 'RMC' in datag: - data = datag['RMC'] + if not self.use_rmc and b'GGA' in datag: + data = datag[b'GGA'] + elif self.use_rmc and b'RMC' in datag: + data = datag[b'RMC'] else: return # Return if time is NaN diff --git a/src/reach_ros_node/parser.py b/src/reach_ros_node/parser.py index 1961b4d..7dbcb46 100644 --- a/src/reach_ros_node/parser.py +++ b/src/reach_ros_node/parser.py @@ -118,7 +118,7 @@ def convert_deg_to_rads(degs): https://www.sparkfun.com/datasheets/GPS/NMEA%20Reference%20Manual1.pdf """ parse_maps = { - "GGA": [ + b"GGA": [ ("utc_time", convert_time, 1), ("latitude", convert_latitude, 2), ("latitude_direction", str, 3), @@ -130,7 +130,7 @@ def convert_deg_to_rads(degs): ("altitude", safe_float, 9), ("mean_sea_level", safe_float, 11), ], - "GST": [ + b"GST": [ ("utc_time", convert_time, 1), ("ellipse_sigma_major", safe_float, 3), ("ellipse_sigma_minor", safe_float, 4), @@ -139,7 +139,7 @@ def convert_deg_to_rads(degs): ("longitude_sigma", safe_float, 7), ("altitude_sigma", safe_float, 8), ], - "RMC": [ + b"RMC": [ ("utc_time", convert_time, 1), ("fix_valid", convert_status_flag, 2), ("latitude", convert_latitude, 3), @@ -149,12 +149,12 @@ def convert_deg_to_rads(degs): ("speed", convert_knots_to_mps, 7), ("true_course", convert_deg_to_rads, 8), ], - "VTG": [ + b"VTG": [ ("ori_true", convert_deg_to_rads, 1), ("ori_magnetic", convert_deg_to_rads, 3), ("speed", convert_knots_to_mps, 5), ], - "GSV": [ + b"GSV": [ ("message_total", safe_int, 1), ("message_number", safe_int, 2), ("sat_in_view", safe_int, 3), @@ -175,7 +175,7 @@ def convert_deg_to_rads(degs): ("sat_04_azimuth", convert_deg_to_rads, 18), ("sat_04_snr", safe_float, 19), ], - "GSA": [ + b"GSA": [ ("mode", str, 1), ("fix_type", safe_int, 2), ("sat_id", safe_int, 3), @@ -188,15 +188,15 @@ def convert_deg_to_rads(degs): def parse_nmea_sentence(nmea_sentence): # Check for a valid nmea sentence - if not re.match('(^\$GP|^\$GA|^\$GN|^\$GL).*\*[0-9A-Fa-f]{2}$', nmea_sentence): + if not re.match(b'(^\$GP|^\$GA|^\$GN|^\$GL).*\*[0-9A-Fa-f]{2}$', nmea_sentence): logger.warn("Regex didn't match, sentence not valid NMEA? Sentence was: %s" % repr(nmea_sentence)) return False # Remove the last bit after the asterisk, this is the checksum - nmea_sentence = nmea_sentence.split("*",1)[0] + nmea_sentence = nmea_sentence.split(b"*",1)[0] # Split on the commas - fields = [field.strip(',') for field in nmea_sentence.split(',')] + fields = [field.strip(b',') for field in nmea_sentence.split(b',')] # Ignore the $ and talker ID portions (e.g. GP, GL, GA, GN) sentence_type = fields[0][3:]