samples.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # License: BSD
4 # https://raw.github.com/robotics-in-concert/rocon_multimaster/license/LICENSE
5 #
6 ##############################################################################
7 # Imports
8 ##############################################################################
9 
10 import rospy
11 import gateway_msgs.msg as gateway_msgs
12 import gateway_msgs.srv as gateway_srvs
13 import rocon_gateway_utils
14 import rocon_python_comms
15 
16 from .exceptions import GatewaySampleRuntimeError
17 from .utils import connection_types
18 
19 ##############################################################################
20 # Constants
21 ##############################################################################
22 
23 _gateway_namespace = '/gateway'
24 
25 ##############################################################################
26 # Utility functions
27 ##############################################################################
28 
29 
30 def wait_for_gateway(ns=_gateway_namespace, timeout=rospy.Duration(5.0)):
31  '''
32  Slowly loop (and block) until the gateway is connected to a hub.
33  '''
34  gateway_info_service = rocon_python_comms.SubscriberProxy(ns + '/gateway_info', gateway_msgs.GatewayInfo)
35  start_time = rospy.Time.now()
36  while not rospy.is_shutdown():
37  gateway_info = gateway_info_service()
38  if gateway_info.connected:
39  break
40  if rospy.Time.now() - start_time > timeout:
41  raise GatewaySampleRuntimeError("timed out waiting for the gateway to connect to a hub")
42  rospy.sleep(0.5)
43 
44 
45 def wait_for_remote_gateway(remote_gateway_name, ns=_gateway_namespace, timeout=rospy.Duration(5.0)):
46  '''
47  Slowly loop (and block) until remote the gateway is visible on our hub.
48  '''
49  rospy.wait_for_service(ns + '/remote_gateway_info')
50  remote_gateway_info_service = rospy.ServiceProxy(ns + '/remote_gateway_info', gateway_srvs.RemoteGatewayInfo)
51  req = gateway_srvs.RemoteGatewayInfoRequest()
52  start_time = rospy.Time.now()
53  while not rospy.is_shutdown():
54  req = gateway_srvs.RemoteGatewayInfoRequest()
55  req.gateways = []
56  resp = remote_gateway_info_service(req)
57  matched = False
58  if rospy.Time.now() - start_time > timeout:
59  raise GatewaySampleRuntimeError("timed out waiting for a remote gateway to appear")
60  for gateway in resp.gateways:
61  if remote_gateway_name == gateway.name:
62  matched = True
63  break
64  remote_gateway_basename = rocon_gateway_utils.gateway_basename(gateway.name)
65  print("Samples: gateway comparison [%s][%s]" % (remote_gateway_basename, remote_gateway_name))
66  if remote_gateway_name == remote_gateway_basename:
67  matched = True
68  break
69  if matched:
70  break
71 
72 
73 def find_first_remote_gateway(ns=_gateway_namespace, timeout=rospy.Duration(15.0)):
74  '''
75  Parses the remote gateway list to find a gateway to use for testing.
76 
77  It's a dumb hack to make testing quite convenient.
78 
79  @return gateway string name
80  @rtype string
81  '''
82  rospy.wait_for_service(ns + '/remote_gateway_info')
83  remote_gateway_info = rospy.ServiceProxy(ns + '/remote_gateway_info', gateway_srvs.RemoteGatewayInfo)
84  start_time = rospy.Time.now()
85  while not rospy.is_shutdown():
86  req = gateway_srvs.RemoteGatewayInfoRequest()
87  req.gateways = []
88  resp = remote_gateway_info(req)
89  if rospy.Time.now() - start_time > timeout:
90  raise GatewaySampleRuntimeError("timed out waiting for a remote gateway to appear")
91  if len(resp.gateways) > 0:
92  break
93  rospy.sleep(0.5)
94  return resp.gateways[0].name
95 
96 
97 def create_tutorial_dictionaries(use_regex_patterns=False):
98  names = {}
99  nodes = {}
100  if use_regex_patterns:
101  names = {
102  gateway_msgs.ConnectionType.PUBLISHER: '.*ter',
103  gateway_msgs.ConnectionType.SUBSCRIBER: '.*ter',
104  gateway_msgs.ConnectionType.SERVICE: '/add_.*_ints',
105  gateway_msgs.ConnectionType.ACTION_CLIENT: '/fibonacci/cli.*',
106  gateway_msgs.ConnectionType.ACTION_SERVER: '/fibonacci/ser.*'
107  }
108  nodes = {
109  gateway_msgs.ConnectionType.PUBLISHER: '/t.*er',
110  gateway_msgs.ConnectionType.SUBSCRIBER: '',
111  gateway_msgs.ConnectionType.SERVICE: '',
112  gateway_msgs.ConnectionType.ACTION_CLIENT: '',
113  gateway_msgs.ConnectionType.ACTION_SERVER: ''
114  }
115  else:
116  names = {
117  gateway_msgs.ConnectionType.PUBLISHER: '/chatter',
118  gateway_msgs.ConnectionType.SUBSCRIBER: '/chatter',
119  gateway_msgs.ConnectionType.SERVICE: '/add_two_ints',
120  gateway_msgs.ConnectionType.ACTION_CLIENT: '/fibonacci/client',
121  gateway_msgs.ConnectionType.ACTION_SERVER: '/fibonacci/server'
122  }
123  nodes = {
124  gateway_msgs.ConnectionType.PUBLISHER: '',
125  gateway_msgs.ConnectionType.SUBSCRIBER: '',
126  gateway_msgs.ConnectionType.SERVICE: '',
127  gateway_msgs.ConnectionType.ACTION_CLIENT: '',
128  gateway_msgs.ConnectionType.ACTION_SERVER: ''
129  }
130  return names, nodes
131 
132 ##############################################################################
133 # Methods
134 ##############################################################################
135 #
136 # Notes about these methods:
137 # Ros is already running (i.e. rospy.init_node has been called
138 # Use the gateway namespace above (could probably make it smarter by hunting)
139 #
140 ##############################################################################
141 
142 
143 def _action_text(cancel=False, msg='acting'):
144  text = "cancelling" if cancel else msg
145  return text
146 
147 
148 def advertise_all(cancel=False, ns=_gateway_namespace):
149  '''
150  Sends a rule for advertising everything except the default blacklist.
151  '''
152  rospy.wait_for_service(ns + '/advertise_all')
153  advertise_all = rospy.ServiceProxy(ns + '/advertise_all', gateway_srvs.AdvertiseAll)
154  req = gateway_srvs.AdvertiseAllRequest()
155  req.cancel = cancel
156  req.blacklist = []
157  rospy.loginfo("Advertise All : %s all." % _action_text(cancel, 'advertising'))
158  resp = advertise_all(req)
159  if resp.result == gateway_msgs.ErrorCodes.ADVERTISEMENT_EXISTS: # already advertising all error is ignored. this call only has no effect.
160  pass
161  elif resp.result != 0:
162  raise GatewaySampleRuntimeError("failed to advertise all (todo: no error message yet)")
163 
164 
165 def advertise_tutorials(cancel=False, regex_patterns=False, ns=_gateway_namespace):
166  rospy.wait_for_service(ns + '/advertise')
167  advertise = rospy.ServiceProxy(ns + '/advertise', gateway_srvs.Advertise)
168  req = gateway_srvs.AdvertiseRequest()
169  req.cancel = cancel
170  rule = gateway_msgs.Rule()
171  if regex_patterns:
172  names, nodes = create_tutorial_dictionaries(use_regex_patterns=True)
173  else:
174  names, nodes = create_tutorial_dictionaries(use_regex_patterns=False)
175  for connection_type in connection_types:
176  req.rules = []
177  rule.name = names[connection_type]
178  rule.type = connection_type
179  rule.node = nodes[connection_type]
180  rospy.loginfo("Advertise : %s [%s,%s,%s]." %
181  (_action_text(cancel, 'advertising'), rule.type, rule.name, rule.node or 'None'))
182  req.rules.append(rule)
183  resp = advertise(req)
184  if resp.result == gateway_msgs.ErrorCodes.ADVERTISEMENT_EXISTS: # already advertising all error is ignored. this call only has no effect.
185  pass
186  elif resp.result != 0:
187  raise GatewaySampleRuntimeError("failed to advertise %s [%s]" % (rule.name, resp.error_message))
188 
189 
190 def pull_all(remote_gateway_name=None, cancel=False, ns=_gateway_namespace):
191  '''
192  Sends a rule for pulling everything from the specified remote gateway.
193  '''
194  rospy.wait_for_service(ns + '/pull_all')
195  pull_all = rospy.ServiceProxy(ns + '/pull_all', gateway_srvs.RemoteAll)
196  if not remote_gateway_name:
197  remote_gateway_name = find_first_remote_gateway()
198  req = gateway_srvs.RemoteAllRequest()
199  req.gateway = remote_gateway_name
200  req.cancel = cancel
201  req.blacklist = []
202  rospy.loginfo("Pull All : %s." % _action_text(cancel, 'sending pull rule for all to the gateway'))
203  resp = pull_all(req)
204  if resp.result == gateway_msgs.ErrorCodes.PULL_RULE_ALREADY_EXISTS: # already pulled all error is ignored. this call only has no effect.
205  pass
206  elif resp.result != 0:
207  raise GatewaySampleRuntimeError("failed to pull all from %s [%s]" % (remote_gateway_name, resp.error_message))
208 
209 
210 def pull_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace):
211  rospy.wait_for_service(ns + '/pull')
212  pull = rospy.ServiceProxy(ns + '/pull', gateway_srvs.Remote)
213  if not remote_gateway_name:
214  remote_gateway_name = find_first_remote_gateway()
215  req = gateway_srvs.RemoteRequest()
216  req.cancel = cancel
217  if regex_patterns:
218  names, nodes = create_tutorial_dictionaries(use_regex_patterns=True)
219  else:
220  names, nodes = create_tutorial_dictionaries(use_regex_patterns=False)
221  req.remotes = []
222  for connection_type in connection_types:
223  rule = gateway_msgs.Rule()
224  rule.name = names[connection_type]
225  rule.type = connection_type
226  rule.node = nodes[connection_type]
227  rospy.loginfo(
228  "Pull : %s [%s,%s,%s][%s]." %
229  (_action_text(
230  cancel,
231  'sending pull rule to the gateway'),
232  rule.type,
233  rule.name,
234  rule.node or 'None',
235  remote_gateway_name))
236  req.remotes.append(gateway_msgs.RemoteRule(remote_gateway_name, rule))
237  resp = pull(req)
238  if resp.result == gateway_msgs.ErrorCodes.PULL_RULE_ALREADY_EXISTS: # already pulled all error is ignored. this call only has no effect.
239  pass
240  elif resp.result != 0:
241  raise GatewaySampleRuntimeError("failed to pull %s [%s]" % (rule.name, resp.error_message))
242 
243 
244 def flip_all(remote_gateway_name=None, cancel=False, ns=_gateway_namespace):
245  '''
246  Sends a rule for flipping everything to the specified remote gateway.
247  '''
248  rospy.wait_for_service(ns + '/flip_all')
249  flip_all = rospy.ServiceProxy(ns + '/flip_all', gateway_srvs.RemoteAll)
250  if not remote_gateway_name:
251  remote_gateway_name = find_first_remote_gateway()
252  req = gateway_srvs.RemoteAllRequest()
253  req.gateway = remote_gateway_name
254  req.cancel = cancel
255  req.blacklist = []
256  rospy.loginfo("Flip All : %s [%s]." %
257  (_action_text(cancel, 'sending flip rule for all to the gateway'), remote_gateway_name))
258  resp = flip_all(req)
259  if resp.result == gateway_msgs.ErrorCodes.FLIP_RULE_ALREADY_EXISTS: # already flipped all error is ignored. this call only has no effect.
260  pass
261  elif resp.result != 0:
262  raise GatewaySampleRuntimeError("failed to flip all to %s [%s]" % (remote_gateway_name, resp.error_message))
263 
264 
265 def flip_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace):
266  rospy.wait_for_service(ns + '/flip')
267  flip = rospy.ServiceProxy(ns + '/flip', gateway_srvs.Remote)
268  if not remote_gateway_name:
269  remote_gateway_name = find_first_remote_gateway()
270  req = gateway_srvs.RemoteRequest()
271  req.cancel = cancel
272  if regex_patterns:
273  names, nodes = create_tutorial_dictionaries(use_regex_patterns=True)
274  else:
275  names, nodes = create_tutorial_dictionaries(use_regex_patterns=False)
276  req.remotes = []
277  for connection_type in connection_types:
278  rule = gateway_msgs.Rule()
279  rule.name = names[connection_type]
280  rule.type = connection_type
281  rule.node = nodes[connection_type]
282  rospy.loginfo(
283  "Flip : %s [%s,%s,%s][%s]." %
284  (_action_text(
285  cancel,
286  'requesting flip to gateway'),
287  rule.type,
288  rule.name,
289  rule.node or 'None',
290  remote_gateway_name))
291  req.remotes.append(gateway_msgs.RemoteRule(remote_gateway_name, rule))
292  resp = flip(req)
293  if resp.result == gateway_msgs.ErrorCodes.FLIP_RULE_ALREADY_EXISTS: # already flipped all error is ignored. this call only has no effect.
294  pass
295  elif resp.result != 0:
296  raise GatewaySampleRuntimeError("failed to flip %s [%s]" % (rule.name, resp.error_message))
297 
298 
299 def connect_hub_by_service(ns=_gateway_namespace, raise_exception=True):
300  service_name = ns + "/connect_hub"
301  rospy.wait_for_service(service_name, 1.0) # should catch ROSException for when timeout is exceeded
302  connect = rospy.ServiceProxy(service_name, gateway_srvs.ConnectHub)
303  # Form a request message
304  req = gateway_srvs.ConnectHubRequest()
305  req.uri = "http://localhost:6380"
306  rospy.loginfo("")
307  rospy.loginfo("Request:\n\n%s\n" % req)
308  rospy.loginfo("")
309  resp = connect(req)
310  rospy.loginfo("")
311  rospy.loginfo("Response:\n\n%s\n" % resp)
312  if raise_exception:
313  if resp.result != gateway_msgs.ErrorCodes.SUCCESS:
314  raise GatewaySampleRuntimeError("failed to connect to hub [%s][%s]" % (req.uri, resp.error_message))
315  return resp.result, resp.error_message
rocon_gateway.samples.connect_hub_by_service
def connect_hub_by_service(ns=_gateway_namespace, raise_exception=True)
Definition: samples.py:299
rocon_gateway.samples.wait_for_remote_gateway
def wait_for_remote_gateway(remote_gateway_name, ns=_gateway_namespace, timeout=rospy.Duration(5.0))
Definition: samples.py:45
rocon_gateway.samples._action_text
def _action_text(cancel=False, msg='acting')
Methods.
Definition: samples.py:143
rocon_gateway.samples.flip_all
def flip_all(remote_gateway_name=None, cancel=False, ns=_gateway_namespace)
Definition: samples.py:244
rocon_gateway.samples.pull_tutorials
def pull_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace)
Definition: samples.py:210
rocon_gateway.samples.advertise_tutorials
def advertise_tutorials(cancel=False, regex_patterns=False, ns=_gateway_namespace)
Definition: samples.py:165
rocon_gateway.samples.wait_for_gateway
def wait_for_gateway(ns=_gateway_namespace, timeout=rospy.Duration(5.0))
Utility functions.
Definition: samples.py:30
rocon_gateway.samples.pull_all
def pull_all(remote_gateway_name=None, cancel=False, ns=_gateway_namespace)
Definition: samples.py:190
rocon_gateway.samples.flip_tutorials
def flip_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace)
Definition: samples.py:265
rocon_gateway.samples.create_tutorial_dictionaries
def create_tutorial_dictionaries(use_regex_patterns=False)
Definition: samples.py:97
rocon_gateway.samples.find_first_remote_gateway
def find_first_remote_gateway(ns=_gateway_namespace, timeout=rospy.Duration(15.0))
Definition: samples.py:73
rocon_gateway.samples.advertise_all
def advertise_all(cancel=False, ns=_gateway_namespace)
Definition: samples.py:148

rocon_gateway
Author(s): Daniel Stonier , Jihoon Lee , Piyush Khandelwal
autogenerated on Mon Jun 10 2019 14:40:10