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))
