nmea_socket_driver.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2016, Rein Appeldoorn
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the names of the authors nor the names of their
17 # affiliated organizations may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 """Defines the main method for the nmea_socket_driver executable."""
34 
35 
36 import select
37 import sys
38 import traceback
39 
40 try:
41  import socketserver
42 except ImportError:
43  import SocketServer as socketserver # Python 2.7
44 
45 import rospy
46 
47 from libnmea_navsat_driver.driver import RosNMEADriver
48 
49 
50 class NMEAMessageHandler(socketserver.DatagramRequestHandler):
51  def handle(self):
52  for line in self.rfile:
53  line = line.strip()
54  if not line:
55  continue
56 
57  try:
58  self.server.driver.add_sentence(line, self.server.frame_id)
59  except ValueError:
60  rospy.logwarn(
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"
65  "```\n" +
66  repr(line) + "\n\n" +
67  traceback.format_exc() +
68  "```")
69 
70 
71 def main():
72  """Create and run the nmea_socket_driver ROS node.
73 
74  Creates a ROS NMEA Driver and feeds it NMEA sentence strings from a UDP socket.
75 
76  ROS parameters:
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.
80  """
81  rospy.init_node('nmea_socket_driver')
82 
83  try:
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)
87  except KeyError as e:
88  rospy.logerr("Parameter %s not found" % e)
89  sys.exit(1)
90 
91  # Create a socket
92  server = socketserver.UDPServer((local_ip, local_port), NMEAMessageHandler,
93  bind_and_activate=False)
94  server.frame_id = RosNMEADriver.get_frame_id()
95  server.driver = RosNMEADriver()
96 
97  # Start listening for connections
98  server.server_bind()
99  server.server_activate()
100 
101  # Handle incoming connections until ROS shuts down
102  try:
103  while not rospy.is_shutdown():
104  rlist, _, _ = select.select([server], [], [], timeout)
105  if server in rlist:
106  server.handle_request()
107  except Exception:
108  rospy.logerr(traceback.format_exc())
109  finally:
110  server.server_close()


nmea_navsat_driver
Author(s): Eric Perko , Steven Martin
autogenerated on Mon Feb 28 2022 22:57:09