1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
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
50
52 ''' '''
53
55 Exception.__init__(self)
56 self.choices = choices
57 self.error = error
58
60 return "BinarySelectionRequest from " + self.choices + "::" + repr(self.error)
61
62
63 CACHED_PKG_PATH = dict()
66 '''
67 This class contains the methods to run the nodes on local and remote machines
68 in a screen terminal.
69 '''
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
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
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
115 for remap in n.remap_args:
116 args.append(''.join([remap[0], ':=', remap[1]]))
117
118
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
128
129
130
131 if not force2host is None:
132 host = force2host
133
134
135 if masteruri is None:
136 masteruri = masteruri_from_ros()
137 env.append(('ROS_MASTER_URI', masteruri))
138
139 abs_paths = list()
140 not_found_packages = list()
141
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
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
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
173 raise StartException(''.join(["Can't find resource: ", str(e)]))
174
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
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
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
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
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
224 thread = threading.Thread(target=ps.wait)
225 thread.setDaemon(True)
226 thread.start()
227 else:
228
229
230 if launch_config.PackageName is None:
231 raise StartException(''.join(["Can't run remote without a valid package name!"]))
232
233 if prefix:
234 prefix = ''.join(['"', prefix, '"'])
235
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
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
280 output, error, ok = nm.ssh().ssh_exec(host, startcmd, user, pw, auto_pw_request)
281
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
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
299
300
301
302
303
304 @classmethod
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()
334 not_found_packages = list()
335 try:
336 socket.setdefaulttimeout(6+len(clear_params))
337
338 param_server_multi = xmlrpclib.MultiCall(param_server)
339
340
341
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
349
350
351 socket.setdefaulttimeout(6+len(params))
352 param_server_multi = xmlrpclib.MultiCall(param_server)
353 for p in params.itervalues():
354
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
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
373 finally:
374 socket.setdefaulttimeout(None)
375 return abs_paths, not_found_packages
376
377 @classmethod
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
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
396
397 value.replace(package_path, output)
398
399 return value.replace(package_path, output.strip()), True, True, package
400 else:
401
402
403
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
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
438 if nm.is_local(host):
439 try:
440 cmd = roslib.packages.find_node(package, type)
441 except roslib.packages.ROSPkgException as e:
442
443 raise StartException(str(e))
444
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
453
454
455
456
457
458
459
460
461
462
463
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
482 thread = threading.Thread(target=ps.wait)
483 thread.setDaemon(True)
484 thread.start()
485 else:
486
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
503
504
505
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
517 if not masteruri:
518 masteruri = roslib.rosenv.get_master_uri()
519
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
528
529
530
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
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
591 from rospy.exceptions import TransportInitError, TransportException
592 from rospy.impl.tcpros_base import TCPROSTransport, DEFAULT_BUFF_SIZE
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
611 transport = None
612 protocol = TCPROSServiceClient(service, service_type, headers={})
613 transport = TCPROSTransport(protocol, service)
614
615 dest_addr, dest_port = parse_rosrpc_uri(service_uri)
616
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
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
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
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
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
713 thread = threading.Thread(target=ps.wait)
714 thread.setDaemon(True)
715 thread.start()
716 found = True
717
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
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
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
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
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
810 if nm.is_local(host):
811
812 return
813 (pkg_name, pkg_path) = package_name(os.path.dirname(file))
814 if not pkg_name is None:
815
816 subfile_path = file.replace(pkg_path, '')
817
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