def main(): rospy.init_node('nmea_serial_driver') serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) try: driver = RosNMEADriver() while not rospy.is_shutdown(): data = GPS.readline().strip() try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. " "Error was: %s. Please report this issue at " "github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA " "sentences that caused it." % e) except (rospy.ROSInterruptException, serial.serialutil.SerialException): GPS.close() # Close GPS serial port except serial.SerialException as ex: rospy.logfatal( "Could not open serial port: I/O error({0}): {1}".format( ex.errno, ex.strerror))
def main(): """Create and run the nmea_serial_driver ROS node. Creates a ROS NMEA Driver and feeds it NMEA sentence strings from a serial device. ROS parameters: ~port (str): Path of the serial device to open. ~baud (int): Baud rate to configure the serial device. """ rospy.init_node('nmea_serial_driver') #serial_port = rospy.get_param('~port', '/dev/ttyUSB0') #serial_baud = rospy.get_param('~baud', 4800) serial_port = rospy.get_param('~port', '/dev/ttyACM0') serial_port = '/dev/ttyACM0' # can I do this from the roslaunch command by adding a ~port parameter? serial_baud = rospy.get_param('~baud', 9600) frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) try: driver = RosNMEADriver() while not rospy.is_shutdown(): data = GPS.readline().strip() # https://github.com/ros-drivers/nmea_navsat_driver/pull/55 # remove all stuff before the first $. try: # some strings contained no $ and were causing a fatal error. data = data[data.index('$'):] except: print("no $ to index data - data is ", data) try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. " "Error was: %s. Please report this issue at " "github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA " "sentences that caused it." % e) except (rospy.ROSInterruptException, serial.serialutil.SerialException): GPS.close() # Close GPS serial port except serial.SerialException as ex: rospy.logfatal( "Could not open serial port: I/O error({0}): {1}".format( ex.errno, ex.strerror))
def main(): """Create and run the nmea_serial_driver ROS node. Creates a ROS NMEA Driver and feeds it NMEA sentence strings from a serial device. :ROS Parameters: - ~port (str) Path of the serial device to open. - ~baud (int) Baud rate to configure the serial device. """ rospy.init_node('nmea_serial_driver') serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) try: driver = RosNMEADriver() while not rospy.is_shutdown(): data = GPS.readline().strip() try: nmea_str = data.decode('ascii') driver.add_sentence(nmea_str, frame_id) except UnicodeError as e: rospy.logwarn( "Skipped reading a line from the serial device because it could not be " "decoded as an ASCII string. The bytes were {0}". format(data)) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. " "Error was: %s. Please report this issue at " "github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA " "sentences that caused it." % e) except (rospy.ROSInterruptException, serial.serialutil.SerialException): GPS.close() # Close GPS serial port except serial.SerialException as ex: rospy.logfatal( "Could not open serial port: I/O error({0}): {1}".format( ex.errno, ex.strerror))
def main(): """Create and run the nmea_serial_driver ROS node. Creates a ROS NMEA Driver and feeds it NMEA sentence strings from a serial device. ROS parameters: ~port (str): Path of the serial device to open. ~baud (int): Baud rate to configure the serial device. """ rospy.init_node('nmea_serial_driver') serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) frame_id = RosNMEADriver.get_frame_id() gps_port_opened = False while not gps_port_opened: try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) gps_port_opened = True try: driver = RosNMEADriver() while not rospy.is_shutdown(): data = GPS.readline().strip() try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. " "Error was: %s. Please report this issue at " "github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA " "sentences that caused it." % e) except (rospy.ROSInterruptException, serial.serialutil.SerialException): GPS.close() # Close GPS serial port except serial.SerialException as ex: rospy.logfatal( "Could not open serial port: I/O error({0}): {1} Trying again in 10 secs" .format(ex.errno, ex.strerror)) rospy.sleep(10.)
def main(): """Create and run the nmea_serial_driver ROS node. Creates a ROS NMEA Driver and feeds it NMEA sentence strings from a serial device. ROS parameters: ~port (str): Path of the serial device to open. ~baud (int): Baud rate to configure the serial device. """ rospy.init_node('nmea_serial_driver') serial_port = rospy.get_param('~port', '/dev/artivGPS') serial_baud = rospy.get_param('~baud', 115200) frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) try: driver = RosNMEADriver() while not rospy.is_shutdown(): data = GPS.readline().strip() try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. " "Error was: %s. Please report this issue at " "github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA " "sentences that caused it." % e) except (rospy.ROSInterruptException, serial.serialutil.SerialException): GPS.close() # Close GPS serial port rospy.logfatal("RTK-GNSS connection is FAILED!") except serial.SerialException as ex: rospy.logfatal( "RTK-GNSS is NOT connected!")
def main(): rospy.init_node('nmea_serial_driver') context = pyudev.Context() serial_port = "/dev/ttyUSB0" for device in context.list_devices(subsystem='tty'): if 'ID_VENDOR' in device and device[ 'ID_VENDOR'] == 'Prolific_Technology_Inc.': serial_port = device['DEVNAME'] serial_baud = rospy.get_param('~baud', 4800) frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) try: driver = RosNMEADriver() while not rospy.is_shutdown(): data = GPS.readline().strip() try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. " "Error was: %s. Please report this issue at " "github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA " "sentences that caused it." % e) except (rospy.ROSInterruptException, serial.serialutil.SerialException): GPS.close() # Close GPS serial port except serial.SerialException as ex: rospy.logfatal( "Could not open serial port: I/O error({0}): {1}".format( ex.errno, ex.strerror))
def main(): rospy.init_node('nmea_socket_driver') try: local_ip = rospy.get_param('~ip', '0.0.0.0') local_port = rospy.get_param('~port', 10110) buffer_size = rospy.get_param('~buffer_size', 4096) timeout = rospy.get_param('~timeout_sec', 2) except KeyError as e: rospy.logerr("Parameter %s not found" % e) sys.exit(1) frame_id = RosNMEADriver.get_frame_id() driver = RosNMEADriver() # Connection-loop: connect and keep receiving. If receiving fails, # reconnect while not rospy.is_shutdown(): try: # Create a socket socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Bind the socket to the port socket_.bind((local_ip, local_port)) # Set timeout socket_.settimeout(timeout) except socket.error as exc: rospy.logerr( "Caught exception socket.error when setting up socket: %s" % exc) sys.exit(1) # recv-loop: When we're connected, keep receiving stuff until that # fails while not rospy.is_shutdown(): try: data, remote_address = socket_.recvfrom(buffer_size) # strip the data data_list = data.strip().split("\n") for data in data_list: try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. " "Error was: %s. Please report this issue at " "github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA " "sentences that caused it." % e) except socket.error as exc: rospy.logerr( "Caught exception socket.error during recvfrom: %s" % exc) socket_.close() # This will break out of the recv-loop so we start another # iteration of the connection-loop break socket_.close() # Close socket
class NmeaTcpDriver(object): """The nmea_tcp_driver ROS node. ROS parameters: ~host_ip (str): IP of the TCP device to open. Can be overridden by host_name. ~host_name (str): Hostname of the TCP device, optional. ~host_port (int): Port on the TCP host to read from. ~chunk_size (int): Maximum size in Bytes to be read at once, reads that much or until buffer is empty. """ def __init__(self): """Get and process parameters.""" rospy.init_node('nmea_tcp_driver') self.host_ip = rospy.get_param('~ip', '') self.host_name = rospy.get_param('~hostname', '') self.host_port = rospy.get_param('~port', '') self.chunk_size = rospy.get_param('~chunk_size', 1024) self.frame_id = RosNMEADriver.get_frame_id() self.driver = RosNMEADriver() self.logging = rospy.get_param('~logging', False) self.logging_path = rospy.get_param( '~logging_path', '/tmp/gnss_log_' + str(strftime('%Y-%m-%d_%H-%M-%S')) + '.log') # try to resolve hostname, otherwise use IP as fallback try: # in case the hostname is left empty the given IP shall be used. the # empty hostname would otherwise be resolved to 0.0.0.0 if self.host_name != "": self.host_ip = socket.gethostbyname(self.host_name) rospy.loginfo( "GNSS IP address successfully resolved to \"%s\"", self.host_ip) except Exception: rospy.logwarn( "Unable to resolve GNSS hostname. Fallback to alternative IP address \"%s\"", self.host_ip) if self.host_ip == "": rospy.logerr("Invalid GNSS IP address") sys.exit(1) if self.logging: try: self.log = open(self.logging_path, 'ab') except IOError: rospy.logwarn("Failed to access logging path.") self.logging = False def __exit__(self): """Clean disconnect on exit.""" self.disconnect() if self.logging: self.log.close() def connect(self): """Connect to device.""" while not rospy.is_shutdown() and not self.connect_tcp(): rospy.sleep(5.) def connect_tcp(self): """Open TCP device.""" try: self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.s.connect((self.host_ip, self.host_port)) self.s.setblocking(0) self.s.settimeout(1.0) return True except socket.error as e: rospy.logwarn("GNSS socket error: %s, trying to reconnect", e) self.disconnect() return False def disconnect(self): """Try to disconnect cleanly.""" try: self.s.close() except AttributeError: pass def receive(self): """Receive and process data.""" # properly handle too small chunk sizes, rest of previous datablock rest = "" while not rospy.is_shutdown(): try: chunk = self.s.recv(self.chunk_size).decode('utf-8') for data in chunk.replace("\r", "").split("\n"): # if the datablock doesn't end with a checksum it probably # isn't complete, wait for the rest if len(data) < 3 or data[-3] != "*": rest = rest + data continue if data[-3] == "*": try: self.driver.add_sentence(rest + data, self.frame_id) rest = "" except (ValueError, TypeError): rospy.loginfo("Invalid NMEA message: %s", data) if self.logging: self.log.write(data + '\n') except socket.timeout as e: # try to reconnect on timeout rospy.logwarn("GNSS socket error: %s, trying to reconnect", e) self.disconnect() rospy.loginfo("Trying to reconnect") self.connect() except socket.error as e: self.disconnect()