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