rosbridge_udp.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2012, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 from __future__ import print_function
35 import rospy
36 import sys
37 
38 from socket import error
39 from twisted.internet import reactor
40 from rosbridge_server import RosbridgeUdpSocket,RosbridgeUdpFactory
41 
42 from rosbridge_library.capabilities.advertise import Advertise
44 from rosbridge_library.capabilities.subscribe import Subscribe
45 from rosbridge_library.capabilities.advertise_service import AdvertiseService
46 from rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
47 from rosbridge_library.capabilities.call_service import CallService
48 
49 from std_msgs.msg import Int32
50 
52  reactor.stop()
53 
54 if __name__ == "__main__":
55  rospy.init_node("rosbridge_websocket")
56  rospy.on_shutdown(shutdown_hook) # register shutdown hook to stop the server
57 
58  ##################################################
59  # Parameter handling #
60  ##################################################
61  # get RosbridgeProtocol parameters
62  RosbridgeUdpSocket.fragment_timeout = rospy.get_param('~fragment_timeout',
63  RosbridgeUdpSocket.fragment_timeout)
64  RosbridgeUdpSocket.delay_between_messages = rospy.get_param('~delay_between_messages',
65  RosbridgeUdpSocket.delay_between_messages)
66  RosbridgeUdpSocket.max_message_size = rospy.get_param('~max_message_size',
67  RosbridgeUdpSocket.max_message_size)
68  RosbridgeUdpSocket.unregister_timeout = rospy.get_param('~unregister_timeout',
69  RosbridgeUdpSocket.unregister_timeout)
70  if RosbridgeUdpSocket.max_message_size == "None":
71  RosbridgeUdpSocket.max_message_size = None
72 
73  # Get the glob strings and parse them as arrays.
74  RosbridgeUdpSocket.topics_glob = [
75  element.strip().strip("'")
76  for element in rospy.get_param('~topics_glob', '')[1:-1].split(',')
77  if len(element.strip().strip("'")) > 0]
78  RosbridgeUdpSocket.services_glob = [
79  element.strip().strip("'")
80  for element in rospy.get_param('~services_glob', '')[1:-1].split(',')
81  if len(element.strip().strip("'")) > 0]
82  RosbridgeUdpSocket.params_glob = [
83  element.strip().strip("'")
84  for element in rospy.get_param('~params_glob', '')[1:-1].split(',')
85  if len(element.strip().strip("'")) > 0]
86 
87  # if authentication should be used
88  RosbridgeUdpSocket.authenticate = rospy.get_param('~authenticate', False)
89  port = rospy.get_param('~port', 9090)
90  interface = rospy.get_param('~interface', "")
91  # Publisher for number of connected clients
92  RosbridgeUdpSocket.client_count_pub = rospy.Publisher('client_count', Int32, queue_size=10, latch=True)
93  RosbridgeUdpSocket.client_count_pub.publish(0)
94 
95  if "--port" in sys.argv:
96  idx = sys.argv.index("--port")+1
97  if idx < len(sys.argv):
98  port = int(sys.argv[idx])
99  else:
100  print("--port argument provided without a value.")
101  sys.exit(-1)
102 
103  if "--interface" in sys.argv:
104  idx = sys.argv.index("--interface")+1
105  if idx < len(sys.argv):
106  interface = int(sys.argv[idx])
107  else:
108  print("--interface argument provided without a value.")
109  sys.exit(-1)
110 
111  if "--fragment_timeout" in sys.argv:
112  idx = sys.argv.index("--fragment_timeout") + 1
113  if idx < len(sys.argv):
114  RosbridgeUdpSocket.fragment_timeout = int(sys.argv[idx])
115  else:
116  print("--fragment_timeout argument provided without a value.")
117  sys.exit(-1)
118 
119  if "--delay_between_messages" in sys.argv:
120  idx = sys.argv.index("--delay_between_messages") + 1
121  if idx < len(sys.argv):
122  RosbridgeUdpSocket.delay_between_messages = float(sys.argv[idx])
123  else:
124  print("--delay_between_messages argument provided without a value.")
125  sys.exit(-1)
126 
127  if "--max_message_size" in sys.argv:
128  idx = sys.argv.index("--max_message_size") + 1
129  if idx < len(sys.argv):
130  value = sys.argv[idx]
131  if value == "None":
132  RosbridgeUdpSocket.max_message_size = None
133  else:
134  RosbridgeUdpSocket.max_message_size = int(value)
135  else:
136  print("--max_message_size argument provided without a value. (can be None or <Integer>)")
137  sys.exit(-1)
138 
139  if "--unregister_timeout" in sys.argv:
140  idx = sys.argv.index("--unregister_timeout") + 1
141  if idx < len(sys.argv):
142  unregister_timeout = float(sys.argv[idx])
143  else:
144  print("--unregister_timeout argument provided without a value.")
145  sys.exit(-1)
146 
147  if "--topics_glob" in sys.argv:
148  idx = sys.argv.index("--topics_glob") + 1
149  if idx < len(sys.argv):
150  value = sys.argv[idx]
151  if value == "None":
152  RosbridgeUdpSocket.topics_glob = []
153  else:
154  RosbridgeUdpSocket.topics_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
155  else:
156  print("--topics_glob argument provided without a value. (can be None or a list)")
157  sys.exit(-1)
158 
159  if "--services_glob" in sys.argv:
160  idx = sys.argv.index("--services_glob") + 1
161  if idx < len(sys.argv):
162  value = sys.argv[idx]
163  if value == "None":
164  RosbridgeUdpSocket.services_glob = []
165  else:
166  RosbridgeUdpSocket.services_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
167  else:
168  print("--services_glob argument provided without a value. (can be None or a list)")
169  sys.exit(-1)
170 
171  if "--params_glob" in sys.argv:
172  idx = sys.argv.index("--params_glob") + 1
173  if idx < len(sys.argv):
174  value = sys.argv[idx]
175  if value == "None":
176  RosbridgeUdpSocket.params_glob = []
177  else:
178  RosbridgeUdpSocket.params_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
179  else:
180  print("--params_glob argument provided without a value. (can be None or a list)")
181  sys.exit(-1)
182 
183  # To be able to access the list of topics and services, you must be able to access the rosapi services.
184  if RosbridgeUdpSocket.services_glob:
185  RosbridgeUdpSocket.services_glob.append("/rosapi/*")
186 
187  Subscribe.topics_glob = RosbridgeUdpSocket.topics_glob
188  Advertise.topics_glob = RosbridgeUdpSocket.topics_glob
189  Publish.topics_glob = RosbridgeUdpSocket.topics_glob
190  AdvertiseService.services_glob = RosbridgeUdpSocket.services_glob
191  UnadvertiseService.services_glob = RosbridgeUdpSocket.services_glob
192  CallService.services_glob = RosbridgeUdpSocket.services_glob
193 
194  ##################################################
195  # Done with parameter handling #
196  ##################################################
197 
198  rospy.loginfo("Rosbridge UDP server started on port %d", port)
199  reactor.listenUDP(port, RosbridgeUdpFactory(), interface=interface)
200  reactor.run()
def shutdown_hook()


rosbridge_server
Author(s): Jonathan Mace
autogenerated on Fri May 10 2019 02:17:05