rocon_gateway.samples

1 #!/usr/bin/env python 2 # 3 # License: BSD 4 # https://raw.github.com/robotics-in-concert/rocon_multimaster/hydro_devel/rocon_gateway/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_utilities 14 15 # local imports 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_utilities.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_utilities.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_two_.*', 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 != 0: 160 raise GatewaySampleRuntimeError("failed to advertise all (todo: no error message yet)")

161 162

163 -def advertise_tutorials(cancel=False, regex_patterns=False, ns=_gateway_namespace):

164 rospy.wait_for_service(ns + '/advertise') 165 advertise = rospy.ServiceProxy(ns + '/advertise', gateway_srvs.Advertise) 166 req = gateway_srvs.AdvertiseRequest() 167 req.cancel = cancel 168 rule = gateway_msgs.Rule() 169 if regex_patterns: 170 names, nodes = create_tutorial_dictionaries(use_regex_patterns=True) 171 else: 172 names, nodes = create_tutorial_dictionaries(use_regex_patterns=False) 173 for connection_type in connection_types: 174 req.rules = [] 175 rule.name = names[connection_type] 176 rule.type = connection_type 177 rule.node = nodes[connection_type] 178 rospy.loginfo("Advertise : %s [%s,%s,%s]." % (_action_text(cancel, 'advertising'), rule.type, rule.name, rule.node or 'None')) 179 req.rules.append(rule) 180 resp = advertise(req) 181 if resp.result != 0: 182 raise GatewaySampleRuntimeError("failed to advertise %s [%s]" % (rule.name, resp.error_message))

183 184

185 -def pull_all(remote_gateway_name=None, cancel=False, ns=_gateway_namespace):

186 ''' 187 Sends a rule for pulling everything from the specified remote gateway. 188 ''' 189 rospy.wait_for_service(ns + '/pull_all') 190 pull_all = rospy.ServiceProxy(ns + '/pull_all', gateway_srvs.RemoteAll) 191 if not remote_gateway_name: 192 remote_gateway_name = find_first_remote_gateway() 193 req = gateway_srvs.RemoteAllRequest() 194 req.gateway = remote_gateway_name 195 req.cancel = cancel 196 req.blacklist = [] 197 rospy.loginfo("Pull All : %s." % _action_text(cancel, 'sending pull rule for all to the gateway')) 198 resp = pull_all(req) 199 if resp.result != 0: 200 raise GatewaySampleRuntimeError("failed to pull all from %s [%s]" % (remote_gateway_name, resp.error_message))

201 202

203 -def pull_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace):

204 rospy.wait_for_service(ns + '/pull') 205 pull = rospy.ServiceProxy(ns + '/pull', gateway_srvs.Remote) 206 if not remote_gateway_name: 207 remote_gateway_name = find_first_remote_gateway() 208 req = gateway_srvs.RemoteRequest() 209 req.cancel = cancel 210 if regex_patterns: 211 names, nodes = create_tutorial_dictionaries(use_regex_patterns=True) 212 else: 213 names, nodes = create_tutorial_dictionaries(use_regex_patterns=False) 214 req.remotes = [] 215 for connection_type in connection_types: 216 rule = gateway_msgs.Rule() 217 rule.name = names[connection_type] 218 rule.type = connection_type 219 rule.node = nodes[connection_type] 220 rospy.loginfo("Pull : %s [%s,%s,%s][%s]." % (_action_text(cancel, 'sending pull rule to the gateway'), rule.type, rule.name, rule.node or 'None', remote_gateway_name)) 221 req.remotes.append(gateway_msgs.RemoteRule(remote_gateway_name, rule)) 222 resp = pull(req) 223 if resp.result != 0: 224 raise GatewaySampleRuntimeError("failed to pull %s [%s]" % (rule.name, resp.error_message))

225 226

227 -def flip_all(remote_gateway_name=None, cancel=False, ns=_gateway_namespace):

228 ''' 229 Sends a rule for flipping everything to the specified remote gateway. 230 ''' 231 rospy.wait_for_service(ns + '/flip_all') 232 flip_all = rospy.ServiceProxy(ns + '/flip_all', gateway_srvs.RemoteAll) 233 if not remote_gateway_name: 234 remote_gateway_name = find_first_remote_gateway() 235 req = gateway_srvs.RemoteAllRequest() 236 req.gateway = remote_gateway_name 237 req.cancel = cancel 238 req.blacklist = [] 239 rospy.loginfo("Flip All : %s [%s]." % (_action_text(cancel, 'sending flip rule for all to the gateway'), remote_gateway_name)) 240 resp = flip_all(req) 241 if resp.result != 0: 242 raise GatewaySampleRuntimeError("failed to flip all to %s [%s]" % (remote_gateway_name, resp.error_message))

243 244

245 -def flip_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace):

246 rospy.wait_for_service(ns + '/flip') 247 flip = rospy.ServiceProxy(ns + '/flip', gateway_srvs.Remote) 248 if not remote_gateway_name: 249 remote_gateway_name = find_first_remote_gateway() 250 req = gateway_srvs.RemoteRequest() 251 req.cancel = cancel 252 if regex_patterns: 253 names, nodes = create_tutorial_dictionaries(use_regex_patterns=True) 254 else: 255 names, nodes = create_tutorial_dictionaries(use_regex_patterns=False) 256 req.remotes = [] 257 for connection_type in connection_types: 258 rule = gateway_msgs.Rule() 259 rule.name = names[connection_type] 260 rule.type = connection_type 261 rule.node = nodes[connection_type] 262 rospy.loginfo("Flip : %s [%s,%s,%s][%s]." % (_action_text(cancel, 'requesting flip to gateway'), rule.type, rule.name, rule.node or 'None', remote_gateway_name)) 263 req.remotes.append(gateway_msgs.RemoteRule(remote_gateway_name, rule)) 264 resp = flip(req) 265 if resp.result != 0: 266 raise GatewaySampleRuntimeError("failed to flip %s [%s]" % (rule.name, resp.error_message))

267 268

269 -def connect_hub_by_service(ns=_gateway_namespace, raise_exception=True):

270 connect = rospy.ServiceProxy(ns + '/connect_hub', gateway_srvs.ConnectHub) 271 # Form a request message 272 req = gateway_srvs.ConnectHubRequest() 273 req.uri = "http://localhost:6380" 274 rospy.loginfo("") 275 rospy.loginfo("Request:\n\n%s\n" % req) 276 rospy.loginfo("") 277 resp = connect(req) 278 rospy.loginfo("") 279 rospy.loginfo("Response:\n\n%s\n" % resp) 280 if raise_exception: 281 if resp.result != gateway_msgs.ErrorCodes.SUCCESS: 282 raise GatewaySampleRuntimeError("failed to connect to hub [%s][%s]" % (req.uri, resp.error_message)) 283 return resp.result, resp.error_message

284

Source Code for Module rocon_gateway.samples