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