node_manager_fkie.start

54 ''' 55 This class contains the methods to run the nodes on local and remote machines 56 in a screen terminal. 57 '''

58 - def __init__(self):

59 pass

60 61 @classmethod

62 - def runNode(cls, node, launch_config, force2host=None, masteruri=None, auto_pw_request=False, user=None, pw=None):

63 ''' 64 Start the node with given name from the given configuration. 65 @param node: the name of the node (with name space) 66 @type node: C{str} 67 @param launch_config: the configuration containing the node 68 @type launch_config: L{LaunchConfig} 69 @param force2host: start the node on given host. 70 @type force2host: L{str} 71 @param masteruri: force the masteruri. 72 @type masteruri: L{str} 73 @raise StartException: if the screen is not available on host. 74 @raise Exception: on errors while resolving host 75 @see: L{node_manager_fkie.is_local()} 76 ''' 77 #'print "RUN node", node, time.time() 78 n = launch_config.getNode(node) 79 if n is None: 80 raise StartException(''.join(["Node '", node, "' not found!"])) 81 82 env = list(n.env_args) 83 prefix = n.launch_prefix if not n.launch_prefix is None else '' 84 if prefix.lower() == 'screen' or prefix.lower().find('screen ') != -1: 85 rospy.loginfo("SCREEN prefix removed before start!") 86 prefix = '' 87 args = [''.join(['__ns:=', n.namespace]), ''.join(['__name:=', n.name])] 88 if not (n.cwd is None): 89 args.append(''.join(['__cwd:=', n.cwd])) 90 91 # add remaps 92 for remap in n.remap_args: 93 args.append(''.join([remap[0], ':=', remap[1]])) 94 95 # get host of the node 96 host = launch_config.hostname 97 env_loader = '' 98 if n.machine_name: 99 machine = launch_config.Roscfg.machines[n.machine_name] 100 host = machine.address 101 #TODO: env-loader support? 102 # if hasattr(machine, "env_loader") and machine.env_loader: 103 # env_loader = machine.env_loader 104 # set the host to the given host 105 if not force2host is None: 106 host = force2host 107 108 if masteruri is None: 109 masteruri = nm.nameres().masteruri(n.machine_name) 110 # set the ROS_MASTER_URI 111 if masteruri is None: 112 masteruri = nm.masteruri_from_ros() 113 env.append(('ROS_MASTER_URI', masteruri)) 114 115 abs_paths = list() # tuples of (parameter name, old value, new value) 116 not_found_packages = list() # package names 117 # set the global parameter 118 if not masteruri is None and not masteruri in launch_config.global_param_done: 119 global_node_names = cls.getGlobalParams(launch_config.Roscfg) 120 rospy.loginfo("Register global parameter:\n%s", '\n'.join(global_node_names)) 121 abs_paths[len(abs_paths):], not_found_packages[len(not_found_packages):] = cls._load_parameters(masteruri, global_node_names, []) 122 launch_config.global_param_done.append(masteruri) 123 124 # add params 125 if not masteruri is None: 126 nodens = ''.join([n.namespace, n.name, '/']) 127 params = dict() 128 for param, value in launch_config.Roscfg.params.items(): 129 if param.startswith(nodens): 130 params[param] = value 131 clear_params = [] 132 for cparam in launch_config.Roscfg.clear_params: 133 if cparam.startswith(nodens): 134 clear_params.append(param) 135 rospy.loginfo("Register parameter:\n%s", '\n'.join(params)) 136 abs_paths[len(abs_paths):], not_found_packages[len(not_found_packages):] = cls._load_parameters(masteruri, params, clear_params) 137 #'print "RUN prepared", node, time.time() 138 139 if nm.is_local(host): 140 nm.screen().testScreen() 141 try: 142 cmd = roslib.packages.find_node(n.package, n.type) 143 except (Exception, roslib.packages.ROSPkgException) as e: 144 # multiple nodes, invalid package 145 raise StartException(''.join(["Can't find resource: ", str(e)])) 146 # handle diferent result types str or array of string 147 import types 148 if isinstance(cmd, types.StringTypes): 149 cmd = [cmd] 150 cmd_type = '' 151 if cmd is None or len(cmd) == 0: 152 raise StartException(' '.join([n.type, 'in package [', n.package, '] not found!\n\nThe package was created?\nIs the binary executable?\n'])) 153 if len(cmd) > 1: 154 # Open selection for executables 155 try: 156 from PySide import QtGui 157 item, result = QtGui.QInputDialog.getItem(None, ' '.join(['Multiple executables', n.type, 'in', n.package]), 158 'Select an executable', 159 cmd, 0, False) 160 if result: 161 #open the selected screen 162 cmd_type = item 163 else: 164 return 165 except: 166 raise StartException('Multiple executables with same name in package found!') 167 else: 168 cmd_type = cmd[0] 169 # determine the current working path, Default: the package of the node 170 cwd = nm.get_ros_home() 171 if not (n.cwd is None): 172 if n.cwd == 'ROS_HOME': 173 cwd = nm.get_ros_home() 174 elif n.cwd == 'node': 175 cwd = os.path.dirname(cmd_type) 176 # else: 177 # cwd = LaunchConfig.packageName(os.path.dirname(cmd_type)) 178 cls._prepareROSMaster(masteruri) 179 node_cmd = [nm.RESPAWN_SCRIPT if n.respawn else '', prefix, cmd_type] 180 cmd_args = [nm.screen().getSceenCmd(node)] 181 cmd_args[len(cmd_args):] = node_cmd 182 cmd_args.append(str(n.args)) 183 cmd_args[len(cmd_args):] = args 184 rospy.loginfo("RUN: %s", ' '.join(cmd_args)) 185 if not masteruri is None: 186 new_env=dict(os.environ) 187 new_env['ROS_MASTER_URI'] = masteruri 188 ps = subprocess.Popen(shlex.split(str(' '.join(cmd_args))), cwd=cwd, env=new_env) 189 else: 190 ps = subprocess.Popen(shlex.split(str(' '.join(cmd_args))), cwd=cwd) 191 # wait for process to avoid 'defunct' processes 192 thread = threading.Thread(target=ps.wait) 193 thread.setDaemon(True) 194 thread.start() 195 else: 196 #'print "RUN REMOTE", node, time.time() 197 # start remote 198 if launch_config.PackageName is None: 199 raise StartException(''.join(["Can't run remote without a valid package name!"])) 200 # thus the prefix parameters while the transfer are not separated 201 if prefix: 202 prefix = ''.join(['"', prefix, '"']) 203 # setup environment 204 env_command = '' 205 if env_loader: 206 rospy.logwarn("env_loader in machine tag currently not supported") 207 raise StartException("env_loader in machine tag currently not supported") 208 if env: 209 env_command = "env "+' '.join(["%s=%s"%(k,v) for (k, v) in env]) 210 211 startcmd = [env_command, nm.STARTER_SCRIPT, 212 '--package', str(n.package), 213 '--node_type', str(n.type), 214 '--node_name', str(node), 215 '--node_respawn true' if n.respawn else ''] 216 if not masteruri is None: 217 startcmd.append('--masteruri') 218 startcmd.append(masteruri) 219 if prefix: 220 startcmd[len(startcmd):] = ['--prefix', prefix] 221 222 #rename the absolute paths in the args of the node 223 node_args = [] 224 try: 225 for a in n.args.split(): 226 a_value, is_abs_path, found, package = cls._resolve_abs_paths(a, host) 227 node_args.append(a_value) 228 if is_abs_path: 229 abs_paths.append(('ARGS', a, a_value)) 230 if not found and package: 231 not_found_packages.append(package) 232 233 startcmd[len(startcmd):] = node_args 234 startcmd[len(startcmd):] = args 235 rospy.loginfo("Run remote on %s: %s", host, str(' '.join(startcmd))) 236 #'print "RUN CALL", node, time.time() 237 output, error, ok = nm.ssh().ssh_exec(host, startcmd, user, pw, auto_pw_request) 238 #'print "RUN CALLOK", node, time.time() 239 except nm.AuthenticationRequest as e: 240 raise nm.InteractionNeededError(e, cls.runNode, (node, launch_config, force2host, masteruri, auto_pw_request)) 241 242 if ok: 243 if error: 244 rospy.logwarn("ERROR while start '%s': %s", node, error) 245 raise StartException(str(''.join(['The host "', host, '" reports:\n', error]))) 246 if output: 247 rospy.loginfo("STDOUT while start '%s': %s", node, output) 248 # inform about absolute paths in parameter value 249 if len(abs_paths) > 0: 250 rospy.loginfo("Absolute paths found while start:\n%s", str('\n'.join([''.join([p, '\n OLD: ', ov, '\n NEW: ', nv]) for p, ov, nv in abs_paths]))) 251 252 if len(not_found_packages) > 0: 253 packages = '\n'.join(not_found_packages) 254 raise StartException(str('\n'.join(['Some absolute paths are not renamed because following packages are not found on remote host:', packages])))

255 # if len(abs_paths) > 0: 256 # parameters = '\n'.join(abs_paths) 257 # raise nm.StartException(str('\n'.join(['Following parameter seems to use an absolute local path for remote host:', parameters, 'Use "cwd" attribute of the "node" tag to specify relative paths for remote usage!']))) 258 #'print "RUN OK", node, time.time() 259 260 @classmethod

261 - def _load_parameters(cls, masteruri, params, clear_params):

262 """ 263 Load parameters onto the parameter server 264 """ 265 import roslaunch 266 import roslaunch.launch 267 import xmlrpclib 268 param_server = xmlrpclib.ServerProxy(masteruri) 269 p = None 270 abs_paths = list() # tuples of (parameter name, old value, new value) 271 not_found_packages = list() # pacakges names 272 try: 273 socket.setdefaulttimeout(6) 274 # multi-call style xmlrpc 275 param_server_multi = xmlrpclib.MultiCall(param_server) 276 277 # clear specified parameter namespaces 278 # #2468 unify clear params to prevent error 279 for p in clear_params: 280 param_server_multi.deleteParam(rospy.get_name(), p) 281 r = param_server_multi() 282 # for code, msg, _ in r: 283 # if code != 1: 284 # raise StartException("Failed to clear parameter: %s"%(msg)) 285 286 # multi-call objects are not reusable 287 param_server_multi = xmlrpclib.MultiCall(param_server) 288 for p in params.itervalues(): 289 # suppressing this as it causes too much spam 290 value, is_abs_path, found, package = cls._resolve_abs_paths(p.value, nm.nameres().address(masteruri)) 291 if is_abs_path: 292 abs_paths.append((p.key, p.value, value)) 293 if not found and package: 294 not_found_packages.append(package) 295 if p.value is None: 296 raise StartException("The parameter '%s' is invalid!"%(p.value)) 297 param_server_multi.setParam(rospy.get_name(), p.key, value if is_abs_path else p.value) 298 r = param_server_multi() 299 for code, msg, _ in r: 300 if code != 1: 301 raise StartException("Failed to set parameter: %s"%(msg)) 302 except roslaunch.core.RLException, e: 303 raise StartException(e) 304 except Exception as e: 305 raise #re-raise as this is fatal 306 finally: 307 socket.setdefaulttimeout(None) 308 return abs_paths, not_found_packages

309 310 @classmethod

311 - def _resolve_abs_paths(cls, value, host):

312 ''' 313 Replaces the local absolute path by remote absolute path. Only valid ROS 314 package paths are resolved. 315 @return: value, is absolute path, remote package found (ignore it on local host or if is not absolute path!), package name (if absolute path and remote package NOT found) 316 ''' 317 if isinstance(value, types.StringTypes) and value.startswith('/') and (os.path.isfile(value) or os.path.isdir(value)): 318 if nm.is_local(host): 319 return value, True, True, '' 320 else: 321 # print "ABS PATH:", value, os.path.dirname(value) 322 dir = os.path.dirname(value) if os.path.isfile(value) else value 323 package, package_path = LaunchConfig.packageName(dir) 324 if package: 325 output, error, ok = nm.ssh().ssh_exec(host, ['rospack', 'find', package]) 326 if ok: 327 if output: 328 # print " RESOLVED:", output 329 # print " PACK_PATH:", package_path 330 value.replace(package_path, output) 331 # print " RENAMED:", value.replace(package_path, output.strip()) 332 return value.replace(package_path, output.strip()), True, True, package 333 else: 334 # package on remote host not found! 335 # TODO add error message 336 # error = stderr.read() 337 pass 338 return value, True, False, '' 339 else: 340 return value, False, False, ''

341 342 @classmethod

343 - def runNodeWithoutConfig(cls, host, package, type, name, args=[], masteruri=None, auto_pw_request=True, user=None, pw=None):

344 ''' 345 Start a node with using a launch configuration. 346 @param host: the host or ip to run the node 347 @type host: C{str} 348 @param package: the ROS package containing the binary 349 @type package: C{str} 350 @param type: the binary of the node to execute 351 @type type: C{str} 352 @param name: the ROS name of the node (with name space) 353 @type name: C{str} 354 @param args: the list with arguments passed to the binary 355 @type args: C{[str, ...]} 356 @raise Exception: on errors while resolving host 357 @see: L{node_manager_fkie.is_local()} 358 ''' 359 # create the name with namespace 360 args2 = list(args) 361 fullname = roslib.names.ns_join(roslib.names.SEP, name) 362 for a in args: 363 if a.startswith('__ns:='): 364 fullname = roslib.names.ns_join(a.replace('__ns:=', ''), name) 365 args2.append(''.join(['__name:=', name])) 366 # run on local host 367 if nm.is_local(host): 368 try: 369 cmd = roslib.packages.find_node(package, type) 370 except roslib.packages.ROSPkgException as e: 371 # multiple nodes, invalid package 372 raise StartException(str(e)) 373 # handle different result types str or array of string 374 import types 375 if isinstance(cmd, types.StringTypes): 376 cmd = [cmd] 377 cmd_type = '' 378 if cmd is None or len(cmd) == 0: 379 raise StartException(' '.join([type, 'in package [', package, '] not found!'])) 380 if len(cmd) > 1: 381 # Open selection for executables 382 # try: 383 # from PySide import QtGui 384 # item, result = QtGui.QInputDialog.getItem(None, ' '.join(['Multiple executables', type, 'in', package]), 385 # 'Select an executable', 386 # cmd, 0, False) 387 # if result: 388 # #open the selected screen 389 # cmd_type = item 390 # else: 391 # return 392 # except: 393 err = [''.join(['Multiple executables with same name in package [', package, '] found:'])] 394 err.extend(cmd) 395 raise StartException('\n'.join(err)) 396 else: 397 cmd_type = cmd[0] 398 cmd_str = str(' '.join([nm.screen().getSceenCmd(fullname), cmd_type, ' '.join(args2)])) 399 rospy.loginfo("Run without config: %s", cmd_str) 400 ps = None 401 if not masteruri is None: 402 cls._prepareROSMaster(masteruri) 403 new_env=dict(os.environ) 404 new_env['ROS_MASTER_URI'] = masteruri 405 ps = subprocess.Popen(shlex.split(cmd_str), env=new_env) 406 else: 407 ps = subprocess.Popen(shlex.split(cmd_str)) 408 # wait for process to avoid 'defunct' processes 409 thread = threading.Thread(target=ps.wait) 410 thread.setDaemon(True) 411 thread.start() 412 else: 413 # run on a remote machine 414 startcmd = [nm.STARTER_SCRIPT, 415 '--package', str(package), 416 '--node_type', str(type), 417 '--node_name', str(fullname)] 418 startcmd[len(startcmd):] = args2 419 if not masteruri is None: 420 startcmd.append('--masteruri') 421 startcmd.append(masteruri) 422 rospy.loginfo("Run remote on %s: %s", host, ' '.join(startcmd)) 423 try: 424 output, error, ok = nm.ssh().ssh_exec(host, startcmd, user, pw, auto_pw_request) 425 if ok: 426 if error: 427 rospy.logwarn("ERROR while start '%s': %s", name, error) 428 raise StartException(''.join(['The host "', host, '" reports:\n', error])) 429 # from PySide import QtGui 430 # QtGui.QMessageBox.warning(None, 'Error while remote start %s'%str(name), 431 # str(''.join(['The host "', host, '" reports:\n', error])), 432 # QtGui.QMessageBox.Ok) 433 if output: 434 rospy.logdebug("STDOUT while start '%s': %s", name, output) 435 else: 436 if error: 437 rospy.logwarn("ERROR while start '%s': %s", name, error) 438 raise StartException(''.join(['The host "', host, '" reports:\n', error])) 439 except nm.AuthenticationRequest as e: 440 raise nm.InteractionNeededError(e, cls.runNodeWithoutConfig, (host, package, type, name, args, masteruri, auto_pw_request))

441 442 @classmethod

443 - def _prepareROSMaster(cls, masteruri):

444 if not masteruri: 445 masteruri = roslib.rosenv.get_master_uri() 446 #start roscore, if needed 447 try: 448 log_path = nm.ScreenHandler.LOG_PATH 449 if not os.path.isdir(log_path): 450 os.makedirs(log_path) 451 socket.setdefaulttimeout(3) 452 master = xmlrpclib.ServerProxy(masteruri) 453 master.getUri(rospy.get_name()) 454 except: 455 # socket.setdefaulttimeout(None) 456 # import traceback 457 # print traceback.format_exc() 458 print "Start ROS-Master with", masteruri, "..." 459 # run a roscore 460 from urlparse import urlparse 461 master_port = str(urlparse(masteruri).port) 462 new_env = dict(os.environ) 463 new_env['ROS_MASTER_URI'] = masteruri 464 cmd_args = [nm.ScreenHandler.getSceenCmd(''.join(['/roscore', '--', master_port])), 'roscore', '--port', master_port] 465 subprocess.Popen(shlex.split(' '.join([str(c) for c in cmd_args])), env=new_env) 466 # wait for roscore to avoid connection problems while init_node 467 result = -1 468 count = 0 469 while result == -1 and count < 3: 470 try: 471 print " retry connect to ROS master" 472 master = xmlrpclib.ServerProxy(masteruri) 473 result, uri, msg = master.getUri(rospy.get_name()) 474 except: 475 time.sleep(1) 476 count += 1 477 if count >= 3: 478 raise StartException('Cannot connect to the ROS-Master: '+ str(masteruri)) 479 finally: 480 socket.setdefaulttimeout(None)

481 482

483 - def callService(self, service_uri, service, service_type, service_args=[]):

484 ''' 485 Calls the service and return the response. 486 To call the service the ServiceProxy can't be used, because it uses 487 environment variables to determine the URI of the running service. In our 488 case this service can be running using another ROS master. The changes on the 489 environment variables is not thread safe. 490 So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified. 491 492 @param service_uri: the URI of the service 493 @type service_uri: C{str} 494 @param service: full service name (with name space) 495 @type service: C{str} 496 @param service_type: service class 497 @type service_type: ServiceDefinition: service class 498 @param args: arguments 499 @return: the tuple of request and response. 500 @rtype: C{(request object, response object)} 501 @raise StartException: on error 502 503 @see: L{rospy.SerivceProxy} 504 505 ''' 506 rospy.loginfo("Call service %s[%s]: %s, %s", str(service), str(service_uri), str(service_type), str(service_args)) 507 from rospy.core import parse_rosrpc_uri, is_shutdown 508 from rospy.msg import args_kwds_to_message 509 from rospy.exceptions import TransportInitError, TransportException 510 from rospy.impl.tcpros_base import TCPROSTransport, TCPROSTransportProtocol, DEFAULT_BUFF_SIZE 511 from rospy.impl.tcpros_service import TCPROSServiceClient 512 from rospy.service import ServiceException 513 request = service_type._request_class() 514 import genpy 515 try: 516 now = rospy.get_rostime() 517 import std_msgs.msg 518 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) } 519 genpy.message.fill_message_args(request, service_args, keys) 520 except genpy.MessageException as e: 521 def argsummary(args): 522 if type(args) in [tuple, list]: 523 return '\n'.join([' * %s (type %s)'%(a, type(a).__name__) for a in args]) 524 else: 525 return ' * %s (type %s)'%(args, type(args).__name__)

526 raise StartException("Incompatible arguments to call service:\n%s\nProvided arguments are:\n%s\n\nService arguments are: [%s]"%(e, argsummary(service_args), genpy.message.get_printable_message_args(request))) 527 528 # request = args_kwds_to_message(type._request_class, args, kwds) 529 transport = None 530 protocol = TCPROSServiceClient(service, service_type, headers={}) 531 transport = TCPROSTransport(protocol, service) 532 # initialize transport 533 dest_addr, dest_port = parse_rosrpc_uri(service_uri) 534 535 # connect to service 536 transport.buff_size = DEFAULT_BUFF_SIZE 537 try: 538 transport.connect(dest_addr, dest_port, service_uri, timeout=5) 539 except TransportInitError as e: 540 # can be a connection or md5sum mismatch 541 raise StartException(''.join(["unable to connect to service: ", str(e)])) 542 transport.send_message(request, 0) 543 try: 544 responses = transport.receive_once() 545 if len(responses) == 0: 546 raise StartException("service [%s] returned no response"%service) 547 elif len(responses) > 1: 548 raise StartException("service [%s] returned multiple responses: %s"%(service, len(responses))) 549 except TransportException as e: 550 # convert lower-level exception to exposed type 551 if is_shutdown(): 552 raise StartException("node shutdown interrupted service call") 553 else: 554 raise StartException("transport error completing service call: %s"%(str(e))) 555 except ServiceException, e: 556 raise StartException("Service error: %s"%(str(e))) 557 finally: 558 transport.close() 559 transport = None 560 return request, responses[0] if len(responses) > 0 else None

Source Code for Module node_manager_fkie.start_handler