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(): rospy.init_node('nmea_topic_serial_reader') nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) # Get the frame_id frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) while not rospy.is_shutdown(): data = GPS.readline().strip() sentence = Sentence() sentence.header.stamp = rospy.get_rostime() sentence.header.frame_id = frame_id sentence.sentence = data nmea_pub.publish(sentence) except rospy.ROSInterruptException: GPS.close() # Close GPS serial port
def main(): rospy.init_node('nmea_topic_serial_reader') nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) # Get the frame_id frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) while not rospy.is_shutdown(): data = GPS.readline().strip() sentence = Sentence() sentence.header.stamp = rospy.get_rostime() sentence.header.frame_id = frame_id sentence.sentence = data # log the raw nmea (hope we will see some GGA) rospy.loginfo_throttle(1, sentence) nmea_pub.publish(sentence) except rospy.ROSInterruptException: GPS.close() # Close GPS serial port
def main(): rospy.init_node('nmea_topic_driver') driver = RosNMEADriver() rospy.Subscriber("nmea_sentence", Sentence, nmea_sentence_callback, driver) rospy.spin()
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_socket_driver ROS node. Creates a ROS NMEA Driver and feeds it NMEA sentence strings from a UDP socket. :ROS Parameters: - ~ip (str) IPV4 address of the socket to open. - ~port (int) Local port of the socket to open. - ~timeout (float) The time out period for the socket, in seconds. """ rospy.init_node('nmea_socket_driver') try: local_ip = rospy.get_param('~ip', '0.0.0.0') local_port = rospy.get_param('~port', 10110) timeout = rospy.get_param('~timeout_sec', 2) except KeyError as e: rospy.logerr("Parameter %s not found" % e) sys.exit(1) # Create a socket server = socketserver.UDPServer((local_ip, local_port), NMEAMessageHandler, bind_and_activate=False) server.frame_id = RosNMEADriver.get_frame_id() server.driver = RosNMEADriver() # Start listening for connections server.server_bind() server.server_activate() # Handle incoming connections until ROS shuts down try: while not rospy.is_shutdown(): rlist, _, _ = select.select([server], [], [], timeout) if server in rlist: server.handle_request() except Exception: rospy.logerr(traceback.format_exc()) finally: server.server_close()
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 __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 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(): """Create and run the nmea_topic_driver ROS node. Creates a NMEA Driver and feeds it NMEA sentence strings from a ROS subscriber. ROS subscribers: mea_sentence (nmea_msgs.msg.Sentence): NMEA sentence messages to feed to the driver. """ rospy.init_node('nmea_topic_driver') driver = RosNMEADriver() rospy.Subscriber("nmea_sentence", Sentence, nmea_sentence_callback, driver) rospy.spin()
def main(): """Create and run the nmea_topic_serial_reader ROS node. Opens a serial device and publishes data from the device as nmea_msgs.msg.Sentence messages. :ROS Parameters: - ~port (str) Path of the serial device to open. - ~baud (int) Baud rate to configure the serial device. :ROS Publishers: - nmea_sentence (nmea_msgs.msg.Sentence) Publishes each line from the openserial device as a new message. The header's stamp is set to the rostime when the data is read from the serial device. """ rospy.init_node('nmea_topic_serial_reader') nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) # Get the frame_id frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) while not rospy.is_shutdown(): data = GPS.readline().strip() sentence = Sentence() sentence.header.stamp = rospy.get_rostime() sentence.header.frame_id = frame_id try: sentence.sentence = data.decode('ascii') 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)) else: nmea_pub.publish(sentence) except rospy.ROSInterruptException: GPS.close() # Close GPS serial port
def main(): """Create and run the nmea_topic_serial_reader ROS node. Opens a serial device and publishes data from the device as nmea_msgs.msg.Sentence messages. ROS parameters: ~port (str): Path of the serial device to open. ~baud (int): Baud rate to configure the serial device. ROS publishers: nmea_sentence (nmea_msgs.msg.Sentence): Publishes each line from the open serial device as a new message. The header's stamp is set to the rostime when the data is read from the serial device. """ rospy.init_node('nmea_topic_serial_reader') #初始化ros节点名 nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) #定义发布者 serial_port = rospy.get_param('~port', '/dev/ttyUSB0') #设置串口设备文件名 serial_baud = rospy.get_param('~baud', 4800) #设置波特率 # Get the frame_id frame_id = RosNMEADriver.get_frame_id() #设置frame_id try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) #调用python串口模块Serial while not rospy.is_shutdown(): data = GPS.readline().strip() #从串口读入数据 sentence = Sentence() #创建nmea_msgs/Sentence结构变量sentence sentence.header.stamp = rospy.get_rostime() #设置sentence的时间戳,为当前时间 sentence.header.frame_id = frame_id #设置sentence的frame_id sentence.sentence = data #设置sentence的sentence变量。 nmea_pub.publish(sentence) #发布sentence except rospy.ROSInterruptException: #异常处理 GPS.close() # Close GPS serial port
def main(): """Create and run the nmea_topic_serial_reader ROS node. Opens a serial device and publishes data from the device as nmea_msgs.msg.Sentence messages. ROS parameters: ~port (str): Path of the serial device to open. ~baud (int): Baud rate to configure the serial device. ROS publishers: nmea_sentence (nmea_msgs.msg.Sentence): Publishes each line from the open serial device as a new message. The header's stamp is set to the rostime when the data is read from the serial device. """ rospy.init_node('nmea_topic_serial_reader') nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) # Get the frame_id frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) while not rospy.is_shutdown(): data = GPS.readline().strip() sentence = Sentence() sentence.header.stamp = rospy.get_rostime() sentence.header.frame_id = frame_id sentence.sentence = data nmea_pub.publish(sentence) except rospy.ROSInterruptException: GPS.close() # Close GPS serial port
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
import rospy from nmea_msgs.msg import Sentence from libnmea_navsat_driver.driver import RosNMEADriver if __name__ == '__main__': rospy.init_node('nmea_topic_serial_reader') nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) serial_port = rospy.get_param('~port','/dev/ttyUSB0') serial_baud = rospy.get_param('~baud',4800) # Get the frame_id frame_id = RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) while not rospy.is_shutdown(): data = GPS.readline().strip() sentence = Sentence() sentence.header.stamp = rospy.get_rostime() sentence.header.frame_id = frame_id sentence.sentence = data nmea_pub.publish(sentence) except rospy.ROSInterruptException: GPS.close() #Close GPS serial port
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()
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. - ~logging (bool) Activates logging of raw data. - ~logging_path (str) Logging path. - ~playback (bool) Activates playback of raw data instead of reading from device. - ~playback_path (str) Playback path. - ~chunk_size (int) Maximum size in bytes to be read at once, reads that much or until buffer is empty. - ~driver_rate (int) Rate of the driver im Hz. Low rates reduce system load. Too low rates might lead to a growing queue of unprocessed data. """ 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() logging = rospy.get_param('~logging', False) logging_path = rospy.get_param('~logging_path', path.expanduser("~")) playback = rospy.get_param('~playback', False) playback_path = rospy.get_param('~playback_path', '') chunk_size = rospy.get_param('~chunk_size', 1024) driver_rate = rospy.get_param('~driver_rate', 100) # get binary message definitions ubx_message_definition_file = ( rospkg.RosPack().get_path("nmea_navsat_driver") + "/src/libnmea_navsat_driver/ubx_messages.yaml") with open(ubx_message_definition_file, 'r') as stream: ubx_message_definitions = yaml.safe_load(stream) if logging: try: log = open( path.join( logging_path, 'gnss_log_' + frame_id + '_' + str(strftime('%Y-%m-%d_%H-%M-%S')) + '.log'), 'ab') except IOError: rospy.logwarn("Failed to access logging path.") logging = False try: if not playback: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=0, rtscts=1) else: GPS = open(playback_path, "r") try: driver = RosNMEADriver() cache = b'' startup_delay_expired = False while not rospy.is_shutdown(): data = GPS.read(chunk_size) timestamp = rospy.get_rostime() if playback: # Publishing data before subscribers have a chance to connect leads to non-reproducible behavior. # Therefore wait for a second after creating the publishers. if startup_delay_expired is False: rospy.sleep(1) startup_delay_expired = True if data == b'': rospy.loginfo( "End of file reached, shutting down nmea_navsat_driver." ) rospy.signal_shutdown("End of file reached") try: cache = extract_sentences(cache + data, ubx_message_definitions, driver, frame_id, timestamp=timestamp) rospy.sleep(1. / driver_rate) 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) if logging: log.write(data) 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))