diff --git a/CMakeLists.txt b/CMakeLists.txt index ae7f43c..e63603e 100755 --- a/CMakeLists.txt +++ b/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 diff --git a/doc/rtklib_manual_2.4.2.pdf b/doc/rtklib_manual_2.4.2.pdf new file mode 100644 index 0000000..e192fa6 Binary files /dev/null and b/doc/rtklib_manual_2.4.2.pdf differ diff --git a/launch/reach_ros_node.launch b/launch/reach_ros_node.launch new file mode 100644 index 0000000..8e78adb --- /dev/null +++ b/launch/reach_ros_node.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/msg/LLH.msg b/msg/LLH.msg new file mode 100644 index 0000000..e423363 --- /dev/null +++ b/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 diff --git a/package.xml b/package.xml index ae4b68b..5b7f93a 100755 --- a/package.xml +++ b/package.xml @@ -16,12 +16,16 @@ catkin + std_msgs + message_generation roslint rospy python-serial geometry_msgs + message_runtime nmea_msgs sensor_msgs + std_msgs diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..34ba2bd --- /dev/null +++ b/requirements.txt @@ -0,0 +1,2 @@ +py +pytest diff --git a/scripts/nmea_tcp_driver b/scripts/nmea_tcp_driver index cee95c4..8a35578 100755 --- a/scripts/nmea_tcp_driver +++ b/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.") diff --git a/src/reach_ros_node/__init__.py b/src/reach_ros_node/__init__.py index e69de29..6e77564 100644 --- a/src/reach_ros_node/__init__.py +++ b/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 diff --git a/src/reach_ros_node/checksum_utils.py b/src/reach_ros_node/checksum_utils.py index 899ae38..02bec04 100644 --- a/src/reach_ros_node/checksum_utils.py +++ b/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: diff --git a/src/reach_ros_node/driver.py b/src/reach_ros_node/driver.py index 7a1d27c..c120343 100644 --- a/src/reach_ros_node/driver.py +++ b/src/reach_ros_node/driver.py @@ -39,6 +39,7 @@ from geometry_msgs.msg import TwistStamped from reach_ros_node.checksum_utils import check_nmea_checksum +from reach_ros_node.msg import LLH import reach_ros_node.parser @@ -63,21 +64,21 @@ def __init__(self): self.msg_vel = TwistStamped() self.msg_timeref = TimeReference() - - - # Will process the nmea_string, and try to update our current state - # Should try to publish as many messages as possible with the given data + # Will process the nmea_string, and try to update our current state. + # Should try to publish as many messages as possible with the given data. def process_line(self, nmea_string): # Check if valid message if not check_nmea_checksum(nmea_string): - rospy.logwarn("Received a sentence with an invalid checksum. Sentence was: %s" % repr(nmea_string)) + rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return # Else we are good, lets try to process this message parsed_sentence = reach_ros_node.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: - rospy.logwarn("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) + rospy.logwarn("Failed to parse NMEA sentence. Sentence was: %s" % + nmea_string) return # We have a good message!! @@ -107,20 +108,17 @@ def process_line(self, nmea_string): self.msg_timeref = TimeReference() self.has_timeref = False - - - - # Parses the GGA NMEA message type - def parse_GGA(self,datag): - # Check if we should parse this message + # Parses the GGA NMEA message type. + def parse_GGA(self, datag): + # Check if we should parse this message. if not 'GGA' in datag: return - # Return if are using RCm + # Return if are using RCm. if self.use_rmc: return - # Else lets set what variables we can + # Else lets set what variables we can. data = datag['GGA'] - # If using ROS time, use the current timestamp + # If using ROS time, use the current timestamp. if self.use_rostime: self.msg_fix.header.stamp = rospy.get_rostime() else: @@ -140,7 +138,7 @@ def parse_GGA(self,datag): else: self.msg_fix.status.status = NavSatStatus.STATUS_NO_FIX self.msg_fix.status.service = NavSatStatus.SERVICE_GPS - # Set our lat lon position + # Set our lat lon position. latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude @@ -153,67 +151,58 @@ def parse_GGA(self,datag): self.msg_fix.altitude = data['altitude'] + data['mean_sea_level'] self.has_fix = True - - - - # Parses the GST NMEA message type - def parse_GST(self,datag): - # Check if we should parse this message + # Parses the GST NMEA message type. + def parse_GST(self, datag): + # Check if we should parse this message. if not 'GST' in datag: return - # Return if are using RCM + # Return if are using RCM. if self.use_rmc: return - # Else lets set what variables we can + # Else lets set what variables we can. data = datag['GST'] - # Update the fix message with std + # 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) self.msg_fix.position_covariance[8] = pow(data['altitude_sigma'],2) self.msg_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED self.has_std = True - - - - # Parses the VTG NMEA message type - def parse_VTG(self,datag): - # Check if we should parse this message + # Parses the VTG NMEA message type. + def parse_VTG(self, datag): + # Check if we should parse this message. if not 'VTG' in datag: return - # Return if are using RCM + # Return if are using RCM. if self.use_rmc: return - # Else lets set what variables we can + # Else lets set what variables we can. data = datag['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 + # 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. if self.use_rostime: self.msg_vel.header.stamp = rospy.get_rostime() else: self.msg_vel.header.stamp = rospy.Time.from_sec(0) - # Set the frame ID + # Set the frame ID. self.msg_vel.header.frame_id = self.frame_gps - # Calculate the change in orientatoin + # Calculate the change in orientation. self.msg_vel.twist.linear.x = data['speed'] * math.sin(data['ori_true']) self.msg_vel.twist.linear.y = data['speed'] * math.cos(data['ori_true']) self.has_vel = True - - - - # Parses the RMC NMEA message type - def parse_RMC(self,datag): - # Check if we should parse this message + # Parses the RMC NMEA message type. + def parse_RMC(self, datag): + # Check if we should parse this message. if not 'RMC' in datag: return - # Return if not using RCM + # Return if not using RCM. if not self.use_rmc: return - # Else lets set what variables we can + # Else lets set what variables we can. data = datag['RMC'] - # If using ROS time, use the current timestamp + # If using ROS time, use the current timestamp. if self.use_rostime: self.msg_fix.header.stamp = rospy.get_rostime() else: @@ -241,46 +230,74 @@ def parse_RMC(self,datag): self.has_fix = True self.has_std = True - # Next lets publish the velocity we have - # If using ROS time, use the current timestamp + # Next lets publish the velocity we have. + # If using ROS time, use the current timestamp. if self.use_rostime: self.msg_vel.header.stamp = rospy.get_rostime() else: self.msg_vel.header.stamp = rospy.Time.from_sec(data['utc_time']) - # Set the frame ID + # Set the frame ID. self.msg_vel.header.frame_id = self.frame_gps - # Calculate the change in orientatoin + # Calculate the change in orientation. self.msg_vel.twist.linear.x = data['speed'] * math.sin(data['true_course']) self.msg_vel.twist.linear.y = data['speed'] * math.cos(data['true_course']) self.has_vel = True + # Parses the NMEA messages and just grab the time reference. + def parse_time(self, datag): - - # Parses the NMEA messages and just grab the time reference - def parse_time(self,datag): - - # Get our message data + # 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'] else: return - # Return if time is NaN + # Return if time is NaN. if math.isnan(data['utc_time']): return - # If using ROS time, use the current timestamp + # If using ROS time, use the current timestamp. if self.use_rostime: self.msg_timeref.header.stamp = rospy.get_rostime() else: self.msg_timeref.header.stamp = rospy.Time.from_sec(data['utc_time']) - # Set the frame ID + # Set the frame ID. self.msg_timeref.header.frame_id = self.frame_timeref - # Set the actuall time reference + # Set the actual time reference. self.msg_timeref.time_ref = rospy.Time.from_sec(data['utc_time']) self.msg_timeref.source = self.frame_timeref self.has_timeref = True - +class RosLLHDriver(object): + def __init__(self): + self.frame_gps = rospy.get_param('~frame_gps', 'gps') + self.seq = 0 + self.llh_pub = rospy.Publisher('llh', LLH, queue_size=1) + + def process_line(self, llh_string): + parsed_sentence = reach_ros_node.parser.parse_llh_sentence(llh_string) + if not parsed_sentence: + rospy.logwarn("Failed to parse LLH sentence. Sentence was: %s" % + llh_string) + return + # Populate an LLH message, and publish it. + self.seq += 1 + llh_msg = LLH() + llh_msg.header.stamp = rospy.Time.now() # Message publish time. + llh_msg.header.frame_id = self.frame_gps + llh_msg.header.seq = self.seq + llh_msg.date = parsed_sentence['date'] + llh_msg.utc_time = parsed_sentence['utc_time'] + llh_msg.latitude = parsed_sentence['latitude'] + llh_msg.longitude = parsed_sentence['longitude'] + llh_msg.altitude = parsed_sentence['altitude'] + llh_msg.quality_flag = parsed_sentence['quality_flag'] + llh_msg.num_sats = parsed_sentence['num_sats'] + llh_msg.position_covariance = parsed_sentence['position_covariance'] + llh_msg.position_covariance_type = parsed_sentence[ + 'position_covariance_type'] + llh_msg.diff_age = parsed_sentence['diff_age'] + llh_msg.ratio = parsed_sentence['ratio'] + self.llh_pub.publish(llh_msg) diff --git a/src/reach_ros_node/parser.py b/src/reach_ros_node/parser.py index ee89637..0625bcc 100644 --- a/src/reach_ros_node/parser.py +++ b/src/reach_ros_node/parser.py @@ -31,6 +31,9 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +# Number of fields in the LLH output from the Reach. +LLH_FIELD_COUNT = 15 + import re import time import calendar @@ -39,6 +42,7 @@ import logging logger = logging.getLogger('rosout') +from reach_ros_node.msg import LLH def safe_float(field): try: @@ -62,13 +66,13 @@ def convert_longitude(field): return safe_float(field[0:3]) + safe_float(field[3:]) / 60.0 -# convert the time into standard unix time in seconds +# convert the time into standard unix time in seconds: # https://stackoverflow.com/a/11111177 def convert_time(nmea_utc): - # Get current time in UTC for date information - utc_struct = time.gmtime() # immutable, so cannot modify this one + # Get current time in UTC for date information. + utc_struct = time.gmtime() # Immutable, so cannot modify this one. utc_list = list(utc_struct) - # If one of the time fields is empty, return NaN seconds + # If one of the time fields is empty, return NaN seconds. if not nmea_utc[0:2] or not nmea_utc[2:4] or not nmea_utc[4:6] or not nmea_utc[7:]: return float('NaN') else: @@ -110,6 +114,25 @@ def convert_deg_to_rads(degs): return math.radians(safe_float(degs)) +def covariance_matrix(llh): + """ + Return a full 9-element (symmetric) covariance matrix, + given the six known stdev values in the LLH sentence. + Args: + llh (str): LLH sentence, as received from an Emlid Reach. + See page 101 in doc/rtklib_manaual_2.4.2.pdf. + Returns: + cm (list(float)): full 9-element covariance matrix. + """ + cm = [llh['sdn'], llh['sdne'], llh['sdun'], + llh['sdne'], llh['sde'], llh['sdeu'], + llh['sdun'], llh['sdeu'], llh['sdu']] + + # Cov == sigma ^ 2. + cm = map(lambda sigma: sigma ** 2, cm) + return cm + + """ Format for this is a sentence identifier (e.g. "GGA") as the key, with a tuple of tuples where each tuple is a field name, conversion function and index @@ -182,6 +205,23 @@ def convert_deg_to_rads(degs): ("pdop", safe_float, 4), ("hdop", safe_float, 5), ("vdop", safe_float, 6) + ], + "LLH": [ + ("date", str, 0), + ("utc_time", str, 1), + ("latitude", safe_float, 2), + ("longitude", safe_float, 3), + ("altitude", safe_float, 4), + ("quality_flag", safe_int, 5), + ("num_sats", safe_int, 6), + ("sdn", safe_float, 7), + ("sde", safe_float, 8), + ("sdu", safe_float, 9), + ("sdne", safe_float, 10), + ("sdeu", safe_float, 11), + ("sdun", safe_float, 12), + ("diff_age", safe_float, 13), + ("ratio", safe_float, 14) ] } @@ -189,11 +229,12 @@ 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): - logger.warn("Regex didn't match, sentence not valid NMEA? Sentence was: %s" % repr(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("*", 1)[0] # Split on the commas fields = [field.strip(',') for field in nmea_sentence.split(',')] @@ -203,7 +244,8 @@ def parse_nmea_sentence(nmea_sentence): # Check to see if we have a maping for this message type if not sentence_type in parse_maps: - logger.warn("Sentence type %s not in parse map, ignoring." % repr(sentence_type)) + logger.warn("Sentence type %s not in parse map, ignoring." % + repr(sentence_type)) return False # Parse the message, and return it @@ -212,3 +254,24 @@ def parse_nmea_sentence(nmea_sentence): for entry in parse_map: parsed_sentence[entry[0]] = entry[1](fields[entry[2]]) return {sentence_type: parsed_sentence} + + +def parse_llh_sentence(llh_sentence): + fields = llh_sentence.split() + + # If the LLH feed should restart (e.g. when we change settings in Reachview), + # the first line is a header that looks like this: + # % (lat/lon/height=WGS84/ellipsoidal,Q=1:fix,2:float,3:sbas,4:dgps,5:single,6:ppp,ns=# of satellites), + # We skip this one. + if len(fields) != LLH_FIELD_COUNT: + # Malformed sentence. + return None + + parsed_sentence = {} + for entry in parse_maps['LLH']: + parsed_sentence[entry[0]] = entry[1](fields[entry[2]]) + + # Build a full covariance matrix from the given values. + parsed_sentence['position_covariance'] = covariance_matrix(parsed_sentence) + parsed_sentence['position_covariance_type'] = LLH().COVARIANCE_TYPE_KNOWN + return parsed_sentence diff --git a/test/conftest.py b/test/conftest.py new file mode 100644 index 0000000..decaaef --- /dev/null +++ b/test/conftest.py @@ -0,0 +1,7 @@ +import py +import pytest +@pytest.fixture(scope="session") +def test_data_dir(): + this_dir = py.path.local(__file__).dirpath() + tdd = this_dir.join('test_data').ensure(dir=True) + return tdd diff --git a/test/test_data/llh.txt b/test/test_data/llh.txt new file mode 100644 index 0000000..31be3ab --- /dev/null +++ b/test/test_data/llh.txt @@ -0,0 +1,2 @@ +# date time latitude(deg) longitude(deg) height(m) Q ns sdn(m) sde(m) sdu(m) sdne(m) sdeu(m) sdun(m) age(s) ratio +2019/10/22 18:37:14.200 25.958982413 -80.264451550 -22.7068 1 8 0.0117 0.0072 0.0242 0.0043 0.0029 0.0130 0.59 295.9 diff --git a/test/test_parser.py b/test/test_parser.py new file mode 100644 index 0000000..7db6ecf --- /dev/null +++ b/test/test_parser.py @@ -0,0 +1,20 @@ +from reach_ros_node import parser + + +def test_parse_llh_sentence(test_data_dir): + # Load test data. + llh_sentence = '' + with open(str(test_data_dir.join('llh.txt')), 'r') as f: + llh_sentence = f.readlines()[1] + + # Call. + parsed = parser.parse_llh_sentence(llh_sentence) + + assert type(parsed) == dict + + # Check that the covariance matrix matches the test data. + stdev = [0.0117, 0.0043, 0.0130, + 0.0043, 0.0072, 0.0029, + 0.0130, 0.0029, 0.0242] + cov = map(lambda sigma: sigma ** 2, stdev) + assert parsed['position_covariance'] == cov