Package node_manager_fkie :: Module start_handler
[frames] | no frames]

Source Code for Module node_manager_fkie.start_handler

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2012, Fraunhofer FKIE/US, Alexander Tiderko 
  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 name of Fraunhofer nor the names of its 
 17  #    contributors 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  import os, shlex, subprocess 
 34  import socket 
 35  import types 
 36  import time 
 37  import signal 
 38   
 39  import roslib 
 40  import rospy 
 41  import threading 
 42  import xmlrpclib 
 43   
 44  import node_manager_fkie as nm 
 45  from common import get_ros_home, masteruri_from_ros, package_name, package_name 
46 47 48 -class StartException(Exception):
49 pass
50
51 -class BinarySelectionRequest(Exception):
52 ''' ''' 53
54 - def __init__(self, choices, error):
55 Exception.__init__(self) 56 self.choices = choices 57 self.error = error
58
59 - def __str__(self):
60 return "BinarySelectionRequest from " + self.choices + "::" + repr(self.error)
61 62 63 CACHED_PKG_PATH = dict() # {host : {pkg: path}}
64 65 -class StartHandler(object):
66 ''' 67 This class contains the methods to run the nodes on local and remote machines 68 in a screen terminal. 69 '''
70 - def __init__(self):
71 pass
72 73 @classmethod
74 - def runNode(cls, node, launch_config, force2host=None, masteruri=None, auto_pw_request=False, user=None, pw=None, item=None):
75 ''' 76 Start the node with given name from the given configuration. 77 @param node: the name of the node (with name space) 78 @type node: C{str} 79 @param launch_config: the configuration containing the node 80 @type launch_config: L{LaunchConfig} 81 @param force2host: start the node on given host. 82 @type force2host: L{str} 83 @param masteruri: force the masteruri. 84 @type masteruri: L{str} 85 @param auto_pw_request: opens question dialog directly, use True only if the method is called from the main GUI thread 86 @type auto_pw_request: bool 87 @raise StartException: if the screen is not available on host. 88 @raise Exception: on errors while resolving host 89 @see: L{node_manager_fkie.is_local()} 90 ''' 91 #'print "RUN node", node, time.time() 92 n = launch_config.getNode(node) 93 if n is None: 94 raise StartException(''.join(["Node '", node, "' not found!"])) 95 96 env = list(n.env_args) 97 if n.respawn: 98 # set the respawn environment variables 99 respawn_params = cls._get_respawn_params(rospy.names.ns_join(n.namespace, n.name), launch_config.Roscfg.params) 100 if respawn_params['max'] > 0: 101 env.append(('RESPAWN_MAX', '%d'%respawn_params['max'])) 102 if respawn_params['min_runtime'] > 0: 103 env.append(('RESPAWN_MIN_RUNTIME', '%d'%respawn_params['min_runtime'])) 104 if respawn_params['delay'] > 0: 105 env.append(('RESPAWN_DELAY', '%d'%respawn_params['delay'])) 106 prefix = n.launch_prefix if not n.launch_prefix is None else '' 107 if prefix.lower() == 'screen' or prefix.lower().find('screen ') != -1: 108 rospy.loginfo("SCREEN prefix removed before start!") 109 prefix = '' 110 args = [''.join(['__ns:=', n.namespace.rstrip(rospy.names.SEP)]), ''.join(['__name:=', n.name])] 111 if not (n.cwd is None): 112 args.append(''.join(['__cwd:=', n.cwd])) 113 114 # add remaps 115 for remap in n.remap_args: 116 args.append(''.join([remap[0], ':=', remap[1]])) 117 118 # get host of the node 119 host = launch_config.hostname 120 env_loader = '' 121 if n.machine_name: 122 machine = launch_config.Roscfg.machines[n.machine_name] 123 if not machine.address in ['localhost', '127.0.0.1']: 124 host = machine.address 125 if masteruri is None: 126 masteruri = nm.nameres().masteruri(n.machine_name) 127 #TODO: env-loader support? 128 # if hasattr(machine, "env_loader") and machine.env_loader: 129 # env_loader = machine.env_loader 130 # set the host to the given host 131 if not force2host is None: 132 host = force2host 133 134 # set the ROS_MASTER_URI 135 if masteruri is None: 136 masteruri = masteruri_from_ros() 137 env.append(('ROS_MASTER_URI', masteruri)) 138 139 abs_paths = list() # tuples of (parameter name, old value, new value) 140 not_found_packages = list() # package names 141 # set the global parameter 142 if not masteruri is None and not masteruri in launch_config.global_param_done: 143 global_node_names = cls.getGlobalParams(launch_config.Roscfg) 144 rospy.loginfo("Register global parameter:\n %s", '\n '.join("%s%s"%(str(v)[:80],'...' if len(str(v))>80 else'') for v in global_node_names.values())) 145 abs_paths[len(abs_paths):], not_found_packages[len(not_found_packages):] = cls._load_parameters(masteruri, global_node_names, [], user, pw, auto_pw_request) 146 launch_config.global_param_done.append(masteruri) 147 148 # add params 149 if not masteruri is None: 150 nodens = ''.join([n.namespace, n.name, rospy.names.SEP]) 151 params = dict() 152 for param, value in launch_config.Roscfg.params.items(): 153 if param.startswith(nodens): 154 params[param] = value 155 clear_params = [] 156 for cparam in launch_config.Roscfg.clear_params: 157 if cparam.startswith(nodens): 158 clear_params.append(cparam) 159 rospy.loginfo("Delete parameter:\n %s", '\n '.join(clear_params)) 160 rospy.loginfo("Register parameter:\n %s", '\n '.join("%s%s"%(str(v)[:80],'...' if len(str(v))>80 else'') for v in params.values())) 161 abs_paths[len(abs_paths):], not_found_packages[len(not_found_packages):] = cls._load_parameters(masteruri, params, clear_params, user, pw, auto_pw_request) 162 #'print "RUN prepared", node, time.time() 163 164 if nm.is_local(host): 165 nm.screen().testScreen() 166 if item: 167 cmd_type = item 168 else: 169 try: 170 cmd = roslib.packages.find_node(n.package, n.type) 171 except (Exception, roslib.packages.ROSPkgException) as e: 172 # multiple nodes, invalid package 173 raise StartException(''.join(["Can't find resource: ", str(e)])) 174 # handle diferent result types str or array of string 175 if isinstance(cmd, types.StringTypes): 176 cmd = [cmd] 177 cmd_type = '' 178 if cmd is None or len(cmd) == 0: 179 raise StartException(' '.join([n.type, 'in package [', n.package, '] not found!\n\nThe package was created?\nIs the binary executable?\n'])) 180 if len(cmd) > 1: 181 if auto_pw_request: 182 # Open selection for executables, only if the method is called from the main GUI thread 183 try: 184 from python_qt_binding import QtGui 185 item, result = QtGui.QInputDialog.getItem(None, ' '.join(['Multiple executables', n.type, 'in', n.package]), 186 'Select an executable', 187 cmd, 0, False) 188 if result: 189 #open the selected screen 190 cmd_type = item 191 else: 192 return 193 except: 194 raise StartException('Multiple executables with same name in package found!') 195 else: 196 err = BinarySelectionRequest(cmd, 'Multiple executables') 197 raise nm.InteractionNeededError(err, 198 cls.runNode, (node, launch_config, force2host, masteruri, auto_pw_request, user, pw)) 199 else: 200 cmd_type = cmd[0] 201 # determine the current working path, Default: the package of the node 202 cwd = get_ros_home() 203 if not (n.cwd is None): 204 if n.cwd == 'ROS_HOME': 205 cwd = get_ros_home() 206 elif n.cwd == 'node': 207 cwd = os.path.dirname(cmd_type) 208 cls._prepareROSMaster(masteruri) 209 node_cmd = [nm.Settings().respawn_script if n.respawn else '', prefix, cmd_type] 210 cmd_args = [nm.screen().getSceenCmd(node)] 211 cmd_args[len(cmd_args):] = node_cmd 212 cmd_args.append(str(n.args)) 213 cmd_args[len(cmd_args):] = args 214 rospy.loginfo("RUN: %s", ' '.join(cmd_args)) 215 new_env = dict(os.environ) 216 new_env['ROS_MASTER_URI'] = masteruri 217 # add the namespace environment parameter to handle some cases, e.g. rqt_cpp plugins 218 if n.namespace: 219 new_env['ROS_NAMESPACE'] = n.namespace 220 for k, v in env: 221 new_env[k] = v 222 ps = subprocess.Popen(shlex.split(str(' '.join(cmd_args))), cwd=cwd, env=new_env) 223 # wait for process to avoid 'defunct' processes 224 thread = threading.Thread(target=ps.wait) 225 thread.setDaemon(True) 226 thread.start() 227 else: 228 #'print "RUN REMOTE", node, time.time() 229 # start remote 230 if launch_config.PackageName is None: 231 raise StartException(''.join(["Can't run remote without a valid package name!"])) 232 # thus the prefix parameters while the transfer are not separated 233 if prefix: 234 prefix = ''.join(['"', prefix, '"']) 235 # setup environment 236 env_command = '' 237 if env_loader: 238 rospy.logwarn("env_loader in machine tag currently not supported") 239 raise StartException("env_loader in machine tag currently not supported") 240 if env: 241 new_env = dict() 242 try: 243 for (k, v) in env: 244 v_value, is_abs_path, found, package = cls._resolve_abs_paths(v, host, user, pw, auto_pw_request) 245 new_env[k] = v_value 246 if is_abs_path: 247 abs_paths.append(('ENV', "%s=%s"%(k,v), "%s=%s"%(k,v_value))) 248 if not found and package: 249 not_found_packages.append(package) 250 env_command = "env "+' '.join(["%s=%s"%(k,v) for (k, v) in new_env.items()]) 251 except nm.AuthenticationRequest as e: 252 raise nm.InteractionNeededError(e, cls.runNode, (node, launch_config, force2host, masteruri, auto_pw_request)) 253 254 startcmd = [env_command, nm.Settings().start_remote_script, 255 '--package', str(n.package), 256 '--node_type', str(n.type), 257 '--node_name', str(node), 258 '--node_respawn true' if n.respawn else ''] 259 if not masteruri is None: 260 startcmd.append('--masteruri') 261 startcmd.append(masteruri) 262 if prefix: 263 startcmd[len(startcmd):] = ['--prefix', prefix] 264 265 #rename the absolute paths in the args of the node 266 node_args = [] 267 try: 268 for a in n.args.split(): 269 a_value, is_abs_path, found, package = cls._resolve_abs_paths(a, host, user, pw, auto_pw_request) 270 node_args.append(a_value) 271 if is_abs_path: 272 abs_paths.append(('ARGS', a, a_value)) 273 if not found and package: 274 not_found_packages.append(package) 275 276 startcmd[len(startcmd):] = node_args 277 startcmd[len(startcmd):] = args 278 rospy.loginfo("Run remote on %s: %s", host, str(' '.join(startcmd))) 279 #'print "RUN CALL", node, time.time() 280 output, error, ok = nm.ssh().ssh_exec(host, startcmd, user, pw, auto_pw_request) 281 #'print "RUN CALLOK", node, time.time() 282 except nm.AuthenticationRequest as e: 283 raise nm.InteractionNeededError(e, cls.runNode, (node, launch_config, force2host, masteruri, auto_pw_request)) 284 285 if ok: 286 if error: 287 rospy.logwarn("ERROR while start '%s': %s", node, error) 288 raise StartException(str(''.join(['The host "', host, '" reports:\n', error]))) 289 if output: 290 rospy.loginfo("STDOUT while start '%s': %s", node, output) 291 # inform about absolute paths in parameter value 292 if len(abs_paths) > 0: 293 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]))) 294 295 if len(not_found_packages) > 0: 296 packages = '\n'.join(not_found_packages) 297 raise StartException(str('\n'.join(['Some absolute paths are not renamed because following packages are not found on remote host:', packages])))
298 # if len(abs_paths) > 0: 299 # parameters = '\n'.join(abs_paths) 300 # 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!']))) 301 #'print "RUN OK", node, time.time() 302 303 304 @classmethod
305 - def _get_respawn_params(cls, node, params):
306 result = { 'max' : 0, 'min_runtime' : 0, 'delay': 0 } 307 respawn_max = rospy.names.ns_join(node, 'respawn/max') 308 respawn_min_runtime = rospy.names.ns_join(node, 'respawn/min_runtime') 309 respawn_delay = rospy.names.ns_join(node, 'respawn/delay') 310 try: 311 result['max'] = int(params[respawn_max].value) 312 except: 313 pass 314 try: 315 result['min_runtime'] = int(params[respawn_min_runtime].value) 316 except: 317 pass 318 try: 319 result['delay'] = int(params[respawn_delay].value) 320 except: 321 pass 322 return result
323 324 @classmethod
325 - def _load_parameters(cls, masteruri, params, clear_params, user, pw, auto_pw_request):
326 """ 327 Load parameters onto the parameter server 328 """ 329 import roslaunch 330 import roslaunch.launch 331 param_server = xmlrpclib.ServerProxy(masteruri) 332 p = None 333 abs_paths = list() # tuples of (parameter name, old value, new value) 334 not_found_packages = list() # packages names 335 try: 336 socket.setdefaulttimeout(6+len(clear_params)) 337 # multi-call style xmlrpc 338 param_server_multi = xmlrpclib.MultiCall(param_server) 339 340 # clear specified parameter namespaces 341 # #2468 unify clear params to prevent error 342 for p in clear_params: 343 param_server_multi.deleteParam(rospy.get_name(), p) 344 r = param_server_multi() 345 for code, msg, _ in r: 346 if code != 1 and not msg.find("is not set"): 347 rospy.logwarn("Failed to clear parameter: %s", msg) 348 # raise StartException("Failed to clear parameter: %s"%(msg)) 349 350 # multi-call objects are not reusable 351 socket.setdefaulttimeout(6+len(params)) 352 param_server_multi = xmlrpclib.MultiCall(param_server) 353 for p in params.itervalues(): 354 # suppressing this as it causes too much spam 355 value, is_abs_path, found, package = cls._resolve_abs_paths(p.value, nm.nameres().address(masteruri), user, pw, auto_pw_request) 356 if is_abs_path: 357 abs_paths.append((p.key, p.value, value)) 358 if not found and package: 359 not_found_packages.append(package) 360 if p.value is None: 361 rospy.logwarn("The parameter '%s' is invalid: '%s'"%(p.key, p.value)) 362 #raise StartException("The parameter '%s' is invalid: '%s'"%(p.key, p.value)) 363 else: 364 param_server_multi.setParam(rospy.get_name(), p.key, value if is_abs_path else p.value) 365 r = param_server_multi() 366 for code, msg, _ in r: 367 if code != 1: 368 raise StartException("Failed to set parameter: %s"%(msg)) 369 except roslaunch.core.RLException, e: 370 raise StartException(e) 371 except Exception as e: 372 raise #re-raise as this is fatal 373 finally: 374 socket.setdefaulttimeout(None) 375 return abs_paths, not_found_packages
376 377 @classmethod
378 - def _resolve_abs_paths(cls, value, host, user, pw, auto_pw_request):
379 ''' 380 Replaces the local absolute path by remote absolute path. Only valid ROS 381 package paths are resolved. 382 @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) 383 ''' 384 if isinstance(value, types.StringTypes) and value.startswith('/') and (os.path.isfile(value) or os.path.isdir(value)): 385 if nm.is_local(host): 386 return value, True, True, '' 387 else: 388 # print "ABS PATH:", value, os.path.dirname(value), host 389 dir = os.path.dirname(value) if os.path.isfile(value) else value 390 package, package_path = package_name(dir) 391 if package: 392 output, error, ok = nm.ssh().ssh_exec(host, ['rospack', 'find', package], user, pw, auto_pw_request) 393 if ok: 394 if output: 395 # print " RESOLVED:", output 396 # print " PACK_PATH:", package_path 397 value.replace(package_path, output) 398 # print " RENAMED:", value.replace(package_path, output.strip()) 399 return value.replace(package_path, output.strip()), True, True, package 400 else: 401 # package on remote host not found! 402 # TODO add error message 403 # error = stderr.read() 404 pass 405 return value, True, False, '' 406 else: 407 return value, False, False, ''
408 409 @classmethod
410 - def runNodeWithoutConfig(cls, host, package, type, name, args=[], masteruri=None, auto_pw_request=False, user=None, pw=None):
411 ''' 412 Start a node with using a launch configuration. 413 @param host: the host or ip to run the node 414 @type host: C{str} 415 @param package: the ROS package containing the binary 416 @type package: C{str} 417 @param type: the binary of the node to execute 418 @type type: C{str} 419 @param name: the ROS name of the node (with name space) 420 @type name: C{str} 421 @param args: the list with arguments passed to the binary 422 @type args: C{[str, ...]} 423 @param auto_pw_request: opens question dialog directly, use True only if the method is called from the main GUI thread 424 @type auto_pw_request: bool 425 @raise Exception: on errors while resolving host 426 @see: L{node_manager_fkie.is_local()} 427 ''' 428 # create the name with namespace 429 args2 = list(args) 430 fullname = roslib.names.ns_join(roslib.names.SEP, name) 431 namespace = '' 432 for a in args: 433 if a.startswith('__ns:='): 434 namespace = a.replace('__ns:=', '') 435 fullname = roslib.names.ns_join(namespace, name) 436 args2.append(''.join(['__name:=', name])) 437 # run on local host 438 if nm.is_local(host): 439 try: 440 cmd = roslib.packages.find_node(package, type) 441 except roslib.packages.ROSPkgException as e: 442 # multiple nodes, invalid package 443 raise StartException(str(e)) 444 # handle different result types str or array of string 445 import types 446 if isinstance(cmd, types.StringTypes): 447 cmd = [cmd] 448 cmd_type = '' 449 if cmd is None or len(cmd) == 0: 450 raise StartException(' '.join([type, 'in package [', package, '] not found!'])) 451 if len(cmd) > 1: 452 # Open selection for executables 453 # try: 454 # from python_qt_binding import QtGui 455 # item, result = QtGui.QInputDialog.getItem(None, ' '.join(['Multiple executables', type, 'in', package]), 456 # 'Select an executable', 457 # cmd, 0, False) 458 # if result: 459 # #open the selected screen 460 # cmd_type = item 461 # else: 462 # return 463 # except: 464 err = [''.join(['Multiple executables with same name in package [', package, '] found:'])] 465 err.extend(cmd) 466 raise StartException('\n'.join(err)) 467 else: 468 cmd_type = cmd[0] 469 cmd_str = str(' '.join([nm.screen().getSceenCmd(fullname), cmd_type, ' '.join(args2)])) 470 rospy.loginfo("Run without config: %s", cmd_str) 471 ps = None 472 new_env=dict(os.environ) 473 if namespace: 474 new_env['ROS_NAMESPACE'] = namespace 475 if not masteruri is None: 476 cls._prepareROSMaster(masteruri) 477 new_env['ROS_MASTER_URI'] = masteruri 478 ps = subprocess.Popen(shlex.split(cmd_str), env=new_env) 479 else: 480 ps = subprocess.Popen(shlex.split(cmd_str), env=new_env) 481 # wait for process to avoid 'defunct' processes 482 thread = threading.Thread(target=ps.wait) 483 thread.setDaemon(True) 484 thread.start() 485 else: 486 # run on a remote machine 487 startcmd = [nm.Settings().start_remote_script, 488 '--package', str(package), 489 '--node_type', str(type), 490 '--node_name', str(fullname)] 491 startcmd[len(startcmd):] = args2 492 if not masteruri is None: 493 startcmd.append('--masteruri') 494 startcmd.append(masteruri) 495 rospy.loginfo("Run remote on %s: %s", host, ' '.join(startcmd)) 496 try: 497 output, error, ok = nm.ssh().ssh_exec(host, startcmd, user, pw, auto_pw_request) 498 if ok: 499 if error: 500 rospy.logwarn("ERROR while start '%s': %s", name, error) 501 raise StartException(''.join(['The host "', host, '" reports:\n', error])) 502 # from python_qt_binding import QtGui 503 # QtGui.QMessageBox.warning(None, 'Error while remote start %s'%str(name), 504 # str(''.join(['The host "', host, '" reports:\n', error])), 505 # QtGui.QMessageBox.Ok) 506 if output: 507 rospy.logdebug("STDOUT while start '%s': %s", name, output) 508 else: 509 if error: 510 rospy.logwarn("ERROR while start '%s': %s", name, error) 511 raise StartException(''.join(['The host "', host, '" reports:\n', error])) 512 except nm.AuthenticationRequest as e: 513 raise nm.InteractionNeededError(e, cls.runNodeWithoutConfig, (host, package, type, name, args, masteruri, auto_pw_request))
514 515 @classmethod
516 - def _prepareROSMaster(cls, masteruri):
517 if not masteruri: 518 masteruri = roslib.rosenv.get_master_uri() 519 #start roscore, if needed 520 try: 521 if not os.path.isdir(nm.ScreenHandler.LOG_PATH): 522 os.makedirs(nm.ScreenHandler.LOG_PATH) 523 socket.setdefaulttimeout(3) 524 master = xmlrpclib.ServerProxy(masteruri) 525 master.getUri(rospy.get_name()) 526 except: 527 # socket.setdefaulttimeout(None) 528 # import traceback 529 # print traceback.format_exc() 530 # run a roscore 531 from urlparse import urlparse 532 master_host = urlparse(masteruri).hostname 533 if nm.is_local(master_host, True): 534 print "Start ROS-Master with", masteruri, "..." 535 master_port = urlparse(masteruri).port 536 new_env = dict(os.environ) 537 new_env['ROS_MASTER_URI'] = masteruri 538 cmd_args = '%s roscore --port %d'%(nm.ScreenHandler.getSceenCmd('/roscore--%d'%master_port), master_port) 539 print " %s"%cmd_args 540 try: 541 subprocess.Popen(shlex.split(cmd_args), env=new_env) 542 # wait for roscore to avoid connection problems while init_node 543 result = -1 544 count = 1 545 while result == -1 and count < 11: 546 try: 547 print " retry connect to ROS master", count, '/', 10 548 master = xmlrpclib.ServerProxy(masteruri) 549 result, uri, msg = master.getUri(rospy.get_name()) 550 except: 551 time.sleep(1) 552 count += 1 553 if count >= 11: 554 raise StartException('Cannot connect to the ROS-Master: '+ str(masteruri)) 555 except Exception as e: 556 import sys 557 print >> sys.stderr, e 558 raise 559 else: 560 raise Exception("ROS master '%s' is not reachable"%masteruri) 561 finally: 562 socket.setdefaulttimeout(None)
563
564 - def callService(self, service_uri, service, service_type, service_args=[]):
565 ''' 566 Calls the service and return the response. 567 To call the service the ServiceProxy can't be used, because it uses 568 environment variables to determine the URI of the running service. In our 569 case this service can be running using another ROS master. The changes on the 570 environment variables is not thread safe. 571 So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified. 572 573 @param service_uri: the URI of the service 574 @type service_uri: C{str} 575 @param service: full service name (with name space) 576 @type service: C{str} 577 @param service_type: service class 578 @type service_type: ServiceDefinition: service class 579 @param args: arguments 580 @return: the tuple of request and response. 581 @rtype: C{(request object, response object)} 582 @raise StartException: on error 583 584 @see: L{rospy.SerivceProxy} 585 586 ''' 587 service = str(service) 588 rospy.loginfo("Call service %s[%s]: %s, %s", str(service), str(service_uri), str(service_type), str(service_args)) 589 from rospy.core import parse_rosrpc_uri, is_shutdown 590 # from rospy.msg import args_kwds_to_message 591 from rospy.exceptions import TransportInitError, TransportException 592 from rospy.impl.tcpros_base import TCPROSTransport, DEFAULT_BUFF_SIZE#,TCPROSTransportProtocol 593 from rospy.impl.tcpros_service import TCPROSServiceClient 594 from rospy.service import ServiceException 595 request = service_type._request_class() 596 import genpy 597 try: 598 now = rospy.get_rostime() 599 import std_msgs.msg 600 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) } 601 genpy.message.fill_message_args(request, service_args, keys) 602 except genpy.MessageException as e: 603 def argsummary(args): 604 if type(args) in [tuple, list]: 605 return '\n'.join([' * %s (type %s)'%(a, type(a).__name__) for a in args]) 606 else: 607 return ' * %s (type %s)'%(args, type(args).__name__)
608 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))) 609 610 # request = args_kwds_to_message(type._request_class, args, kwds) 611 transport = None 612 protocol = TCPROSServiceClient(service, service_type, headers={}) 613 transport = TCPROSTransport(protocol, service) 614 # initialize transport 615 dest_addr, dest_port = parse_rosrpc_uri(service_uri) 616 # connect to service 617 transport.buff_size = DEFAULT_BUFF_SIZE 618 try: 619 transport.connect(dest_addr, dest_port, service_uri, timeout=5) 620 except TransportInitError as e: 621 # can be a connection or md5sum mismatch 622 raise StartException(''.join(["unable to connect to service: ", str(e)])) 623 transport.send_message(request, 0) 624 try: 625 responses = transport.receive_once() 626 if len(responses) == 0: 627 raise StartException("service [%s] returned no response"%service) 628 elif len(responses) > 1: 629 raise StartException("service [%s] returned multiple responses: %s"%(service, len(responses))) 630 except TransportException as e: 631 # convert lower-level exception to exposed type 632 if is_shutdown(): 633 raise StartException("node shutdown interrupted service call") 634 else: 635 raise StartException("transport error completing service call: %s"%(str(e))) 636 except ServiceException, e: 637 raise StartException("Service error: %s"%(str(e))) 638 finally: 639 transport.close() 640 transport = None 641 return request, responses[0] if len(responses) > 0 else None
642 643 644 @classmethod
645 - def getGlobalParams(cls, roscfg):
646 ''' 647 Return the parameter of the configuration file, which are not associated with 648 any nodes in the configuration. 649 @param roscfg: the launch configuration 650 @type roscfg: L{roslaunch.ROSLaunchConfig} 651 @return: the list with names of the global parameter 652 @rtype: C{dict(param:value, ...)} 653 ''' 654 result = dict() 655 nodes = [] 656 for item in roscfg.resolved_node_names: 657 nodes.append(item) 658 for param, value in roscfg.params.items(): 659 nodesparam = False 660 for n in nodes: 661 if param.startswith(n): 662 nodesparam = True 663 break 664 if not nodesparam: 665 result[param] = value 666 return result
667 668 @classmethod
669 - def copylogPath2Clipboards(cls, host, nodes=[], auto_pw_request=False, user=None, pw=None):
670 if nm.is_local(host): 671 if len(nodes) == 1: 672 return nm.screen().getScreenLogFile(node=nodes[0]) 673 else: 674 return nm.screen().LOG_PATH 675 else: 676 request = '[]' if len(nodes) != 1 else nodes[0] 677 try: 678 socket.setdefaulttimeout(3) 679 output, error, ok = nm.ssh().ssh_exec(host, [nm.Settings().start_remote_script, '--ros_log_path', request], user, pw, auto_pw_request) 680 if ok: 681 return output 682 else: 683 raise StartException(str(''.join(['Get log path from "', host, '" failed:\n', error]))) 684 except nm.AuthenticationRequest as e: 685 raise nm.InteractionNeededError(e, cls.copylogPath2Clipboards, (host, nodes, auto_pw_request)) 686 finally: 687 socket.setdefaulttimeout(None)
688 689 690 @classmethod
691 - def openLog(cls, nodename, host, user=None):
692 ''' 693 Opens the log file associated with the given node in a new terminal. 694 @param nodename: the name of the node (with name space) 695 @type nodename: C{str} 696 @param host: the host name or ip where the log file are 697 @type host: C{str} 698 @return: C{True}, if a log file was found 699 @rtype: C{bool} 700 @raise Exception: on errors while resolving host 701 @see: L{node_manager_fkie.is_local()} 702 ''' 703 rospy.loginfo("show log for '%s' on '%s'", str(nodename), str(host)) 704 title_opt = ' '.join(['"LOG', nodename, 'on', host, '"']) 705 if nm.is_local(host): 706 found = False 707 screenLog = nm.screen().getScreenLogFile(node=nodename) 708 if os.path.isfile(screenLog): 709 cmd = nm.settings().terminal_cmd([nm.settings().log_viewer, screenLog], title_opt) 710 rospy.loginfo("open log: %s", cmd) 711 ps = subprocess.Popen(shlex.split(cmd)) 712 # wait for process to avoid 'defunct' processes 713 thread = threading.Thread(target=ps.wait) 714 thread.setDaemon(True) 715 thread.start() 716 found = True 717 #open roslog file 718 roslog = nm.screen().getROSLogFile(nodename) 719 if os.path.isfile(roslog): 720 title_opt = title_opt.replace('LOG', 'ROSLOG') 721 cmd = nm.settings().terminal_cmd([nm.settings().log_viewer, roslog], title_opt) 722 rospy.loginfo("open ROS log: %s", cmd) 723 ps = subprocess.Popen(shlex.split(cmd)) 724 # wait for process to avoid 'defunct' processes 725 thread = threading.Thread(target=ps.wait) 726 thread.setDaemon(True) 727 thread.start() 728 found = True 729 return found 730 else: 731 ps = nm.ssh().ssh_x11_exec(host, [nm.Settings().start_remote_script, '--show_screen_log', nodename], title_opt, user) 732 # wait for process to avoid 'defunct' processes 733 thread = threading.Thread(target=ps.wait) 734 thread.setDaemon(True) 735 thread.start() 736 ps = nm.ssh().ssh_x11_exec(host, [nm.Settings().start_remote_script, '--show_ros_log', nodename], title_opt.replace('LOG', 'ROSLOG'), user) 737 # wait for process to avoid 'defunct' processes 738 thread = threading.Thread(target=ps.wait) 739 thread.setDaemon(True) 740 thread.start() 741 return False
742 743 744 @classmethod
745 - def deleteLog(cls, nodename, host, auto_pw_request=False, user=None, pw=None):
746 ''' 747 Deletes the log file associated with the given node. 748 @param nodename: the name of the node (with name space) 749 @type nodename: C{str} 750 @param host: the host name or ip where the log file are to delete 751 @type host: C{str} 752 @raise Exception: on errors while resolving host 753 @see: L{node_manager_fkie.is_local()} 754 ''' 755 rospy.loginfo("delete log for '%s' on '%s'", str(nodename), str(host)) 756 if nm.is_local(host): 757 screenLog = nm.screen().getScreenLogFile(node=nodename) 758 pidFile = nm.screen().getScreenPidFile(node=nodename) 759 roslog = nm.screen().getROSLogFile(nodename) 760 if os.path.isfile(screenLog): 761 os.remove(screenLog) 762 if os.path.isfile(pidFile): 763 os.remove(pidFile) 764 if os.path.isfile(roslog): 765 os.remove(roslog) 766 else: 767 try: 768 output, error, ok = nm.ssh().ssh_exec(host, [nm.Settings().start_remote_script, '--delete_logs', nodename], user, pw, auto_pw_request) 769 except nm.AuthenticationRequest as e: 770 raise nm.InteractionNeededError(e, cls.deleteLog, (nodename, host, auto_pw_request))
771
772 - def kill(self, host, pid, auto_pw_request=False, user=None, pw=None):
773 ''' 774 Kills the process with given process id on given host. 775 @param host: the name or address of the host, where the process must be killed. 776 @type host: C{str} 777 @param pid: the process id 778 @type pid: C{int} 779 @raise StartException: on error 780 @raise Exception: on errors while resolving host 781 @see: L{node_manager_fkie.is_local()} 782 ''' 783 try: 784 self._kill_wo(host, pid, auto_pw_request, user, pw) 785 except nm.AuthenticationRequest as e: 786 raise nm.InteractionNeededError(e, self.kill, (host, pid, auto_pw_request))
787
788 - def _kill_wo(self, host, pid, auto_pw_request=False, user=None, pw=None):
789 rospy.loginfo("kill %s on %s", str(pid), host) 790 if nm.is_local(host): 791 os.kill(pid, signal.SIGKILL) 792 rospy.loginfo("kill: %s", str(pid)) 793 else: 794 # kill on a remote machine 795 cmd = ['kill -9', str(pid)] 796 output, error, ok = nm.ssh().ssh_exec(host, cmd, user, pw, False) 797 if ok: 798 if error: 799 rospy.logwarn("ERROR while kill %s: %s", str(pid), error) 800 raise StartException(str(''.join(['The host "', host, '" reports:\n', error]))) 801 if output: 802 rospy.logdebug("STDOUT while kill %s on %s: %s", str(pid), host, output)
803 804 @classmethod
805 - def transfer_files(cls, host, file, auto_pw_request=False, user=None, pw=None):
806 ''' 807 Copies the given file to the remote host. Uses caching of remote paths. 808 ''' 809 # get package of the file 810 if nm.is_local(host): 811 #it's local -> no copy needed 812 return 813 (pkg_name, pkg_path) = package_name(os.path.dirname(file)) 814 if not pkg_name is None: 815 # get the subpath of the file 816 subfile_path = file.replace(pkg_path, '') 817 # get the path of the package on the remote machine 818 try: 819 output = '' 820 error = '' 821 ok = True 822 if CACHED_PKG_PATH.has_key(host) and CACHED_PKG_PATH[host].has_key(pkg_name): 823 output = CACHED_PKG_PATH[host][pkg_name] 824 else: 825 if not CACHED_PKG_PATH.has_key(host): 826 CACHED_PKG_PATH[host] = dict() 827 output, error, ok = nm.ssh().ssh_exec(host, [nm.Settings().start_remote_script, '--package', pkg_name], user, pw, auto_pw_request) 828 if ok: 829 if error: 830 rospy.logwarn("ERROR while transfer %s to %s: %s", file, host, error) 831 raise StartException(str(''.join(['The host "', host, '" reports:\n', error]))) 832 if output: 833 CACHED_PKG_PATH[host][pkg_name] = output 834 nm.ssh().transfer(host, file, os.path.join(output.strip(), subfile_path.strip(os.sep)), user) 835 else: 836 raise StartException("Remote host no returned any answer. Is there the new version of node_manager installed?") 837 else: 838 raise StartException("Can't get path from remote host. Is there the new version of node_manager installed?") 839 except nm.AuthenticationRequest as e: 840 raise nm.InteractionNeededError(e, cls.transfer_files, (host, file, auto_pw_request))
841