33 """Defines the main method for the nmea_socket_driver executable.""" 43 import SocketServer
as socketserver
52 for line
in self.rfile:
58 self.server.driver.add_sentence(line, self.server.frame_id)
61 "ValueError, likely due to missing fields in the NMEA " 62 "message. Please report this issue at " 63 "https://github.com/ros-drivers/nmea_navsat_driver" 64 ", including the following:\n\n" 67 traceback.format_exc() +
72 """Create and run the nmea_socket_driver ROS node. 74 Creates a ROS NMEA Driver and feeds it NMEA sentence strings from a UDP socket. 77 ~ip (str): IPV4 address of the socket to open. 78 ~port (int): Local port of the socket to open. 79 ~timeout (float): The time out period for the socket, in seconds. 81 rospy.init_node(
'nmea_socket_driver')
84 local_ip = rospy.get_param(
'~ip',
'0.0.0.0')
85 local_port = rospy.get_param(
'~port', 10110)
86 timeout = rospy.get_param(
'~timeout_sec', 2)
88 rospy.logerr(
"Parameter %s not found" % e)
92 server = socketserver.UDPServer((local_ip, local_port), NMEAMessageHandler,
93 bind_and_activate=
False)
94 server.frame_id = RosNMEADriver.get_frame_id()
99 server.server_activate()
103 while not rospy.is_shutdown():
104 rlist, _, _ = select.select([server], [], [], timeout)
106 server.handle_request()
108 rospy.logerr(traceback.format_exc())
110 server.server_close()