Source Code for Module rocon_utilities.launch'

  1  #!/usr/bin/env python 
  2  # 
  3  # License: BSD 
  4  #   https://raw.github.com/robotics-in-concert/rocon_multimaster/master/rocon_utilities/LICENSE 
  5  # 
  6   
  7  ############################################################################## 
  8  # Imports 
  9  ############################################################################## 
 10   
 11  import os 
 12  import argparse 
 13  import subprocess 
 14  import signal 
 15  import sys 
 16  from time import sleep 
 17  import roslaunch 
 18  import tempfile 
 19   
 20  # Local imports 
 21  from .system import which, wait_pid 
 22  import rocon_utilities.console as console 
 23  import xml.etree.ElementTree as ElementTree 
 24  import ros_utilities 
 25   
 26  ############################################################################## 
 27  # Global variables 
 28  ############################################################################## 
 29   
 30  processes = [] 
 31  roslaunch_pids = [] 
 32   
 33  ############################################################################## 
 34  # Methods 
 35  ############################################################################## 
 36   
 37   
 38 -def preexec(): 
 39      ''' 
 40        Don't forward signals. 
 41   
 42        http://stackoverflow.com/questions/3791398/how-to-stop-python-from-propagating-signals-to-subprocesses 
 43      ''' 
 44      os.setpgrp()  # setpgid(0,0) 
 45   
 46   
 47 -def get_roslaunch_pid(parent_pid): 
 48      ''' 
 49        Get the pid of the roslaunch process running in the terminal 
 50        specified by the parent pid. 
 51      ''' 
 52      ps_command = subprocess.Popen("ps -o pid -o comm --ppid %d --noheaders" % parent_pid, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) 
 53      ps_output = ps_command.stdout.read() 
 54      retcode = ps_command.wait() 
 55      pids = [] 
 56      if retcode == 0: 
 57          for pair in ps_output.split("\n")[:-1]: 
 58              [pid, command] = pair.lstrip(' ').split(" ") 
 59              if command == 'roslaunch': 
 60                  pids.append(int(pid)) 
 61      else: 
 62          # Presume this roslaunch was killed by ctrl-c or terminated already. 
 63          # Am not worrying about classifying between the above presumption and real errors for now 
 64          pass 
 65      return pids 
 66   
 67   
 68 -def signal_handler(sig, frame): 
 69      global processes 
 70      global roslaunch_pids 
 71      for p in processes: 
 72          roslaunch_pids.extend(get_roslaunch_pid(p.pid)) 
 73      # kill roslaunch's 
 74      for pid in roslaunch_pids: 
 75          try: 
 76              os.kill(pid, signal.SIGHUP) 
 77          except OSError: 
 78              continue 
 79      for pid in roslaunch_pids: 
 80          console.pretty_println("Terminating roslaunch [pid: %d]" % pid, console.bold) 
 81          wait_pid(pid) 
 82          #console.pretty_println("Terminated roslaunch [pid: %d]" % pid, console.bold) 
 83      sleep(1) 
 84      # now kill konsoles 
 85      for p in processes: 
 86          p.terminate() 
 87   
 88   
 89 -def parse_rocon_launcher(rocon_launcher, default_roslaunch_options): 
 90      ''' 
 91        Parses an rocon multi-launcher (xml file). 
 92   
 93        @param rocon_launcher : xml file in rocon_launch format 
 94        @param default_roslaunch_options : options to pass to roslaunch (usually "--screen") 
 95        @return launchers : list with launcher parameters as dictionary elements of the list. 
 96   
 97        @raise IOError : if it can't find any of the individual launchers on the filesystem. 
 98      ''' 
 99      tree = ElementTree.parse(rocon_launcher) 
100      root = tree.getroot() 
101      # should check for root concert tag 
102      launchers = [] 
103      ports = [] 
104      default_port = 11311 
105      for launch in root.findall('launch'): 
106          parameters = {} 
107          parameters['options'] = default_roslaunch_options 
108          parameters['package'] = launch.get('package') 
109          parameters['name'] = launch.get('name') 
110          parameters['path'] = ros_utilities.find_resource(parameters['package'], parameters['name'])  # raises an IO error if there is a problem. 
111          parameters['port'] = launch.get('port', str(default_port)) 
112          if parameters['port'] == str(default_port): 
113              default_port += 1 
114          if parameters['port'] in ports: 
115              parameters['options'] = parameters['options'] + " " + "--wait" 
116          else: 
117              ports.append(parameters['port']) 
118          launchers.append(parameters) 
119      return launchers 
120   
121   
122 -def parse_arguments(): 
123      parser = argparse.ArgumentParser(description="Rocon's multiple master launcher.") 
124      terminal_group = parser.add_mutually_exclusive_group() 
125      terminal_group.add_argument('-k', '--konsole', default=False, action='store_true', help='spawn individual ros systems via multiple konsole terminals') 
126      terminal_group.add_argument('-g', '--gnome', default=False, action='store_true', help='spawn individual ros systems via multiple gnome terminals') 
127      parser.add_argument('--screen', action='store_true', help='run each roslaunch with the --screen option') 
128      parser.add_argument('--no-terminals', action='store_true', help='do not spawn terminals for each roslaunch') 
129      # Force package, launcher pairs, I like this better than roslaunch style which is a bit vague 
130      parser.add_argument('package', nargs='?', default='', help='name of the package in which to find the concert launcher') 
131      parser.add_argument('launcher', nargs=1, help='name of the concert launch configuration (xml) file') 
132      #parser.add_argument('launchers', nargs='+', help='package and concert launch configuration (xml) file configurations, roslaunch style') 
133      args = parser.parse_args() 
134      return args 
135   
136   
137 -def choose_terminal(gnome_flag, konsole_flag): 
138      ''' 
139        Use ubuntu's x-terminal-emulator to choose the shell, or over-ride if it there is a flag. 
140      ''' 
141      if konsole_flag: 
142          if not which('konsole'): 
143              console.error("Cannot find 'konsole' [hint: try --gnome for gnome-terminal instead]") 
144              sys.exit(1) 
145          return 'konsole' 
146      elif gnome_flag: 
147          if not which('gnome-terminal'): 
148              console.error("Cannot find 'gnome' [hint: try --konsole for konsole instead]") 
149              sys.exit(1) 
150          return 'gnome-terminal' 
151      else: 
152          if not which('x-terminal-emulator'): 
153              console.error("Cannot find 'x-terminal-emulator' [hint: try --gnome or --konsole instead]") 
154              sys.exit(1) 
155          p = subprocess.Popen([which('update-alternatives'), '--query', 'x-terminal-emulator'], stdout=subprocess.PIPE) 
156          terminal = None 
157          for line in p.stdout: 
158              if line.startswith("Value:"): 
159                  terminal = os.path.basename(line.split()[1]) 
160                  break 
161          if terminal not in ["gnome-terminal", "gnome-terminal.wrapper", "konsole"]: 
162              console.warning("You are using an esoteric unsupported terminal [%s]" % terminal) 
163              if which('konsole'): 
164                  terminal = 'konsole' 
165                  console.warning(" --> falling back to 'konsole'") 
166              elif which('gnome-terminal'): 
167                  console.warning(" --> falling back to 'gnome-terminal'") 
168                  terminal = 'gnome-terminal' 
169              else: 
170                  console.error("Unsupported terminal set for 'x-terminal-emulator' [%s][hint: try --gnome or --konsole instead]" % terminal) 
171                  sys.exit(1) 
172          return terminal 
173   
174   
175 -def main(): 
176      global processes 
177      global roslaunch_pids 
178      signal.signal(signal.SIGINT, signal_handler) 
179      args = parse_arguments() 
180      terminal = None 
181      if not args.no_terminals: 
182          if not which('konsole') and not which('gnome-terminal')and not which('x-terminal-emulator'): 
183              console.error("Cannot find a suitable terminal [x-terminal-emulator, konsole, gnome-termional]") 
184              sys.exit(1) 
185          terminal = choose_terminal(args.gnome, args.konsole) 
186   
187      if args.package == '': 
188          rocon_launcher = roslaunch.rlutil.resolve_launch_arguments(args.launcher)[0] 
189      else: 
190          rocon_launcher = roslaunch.rlutil.resolve_launch_arguments([args.package] + args.launcher)[0] 
191      if args.screen: 
192          roslaunch_options = "--screen" 
193      else: 
194          roslaunch_options = "" 
195      launchers = parse_rocon_launcher(rocon_launcher, roslaunch_options) 
196      temporary_launchers = [] 
197      for launcher in launchers: 
198          console.pretty_println("Launching [%s, %s] on port %s" % (launcher['package'], launcher['name'], launcher['port']), console.bold) 
199          ########################## 
200          # Customise the launcher 
201          ########################## 
202          temp = tempfile.NamedTemporaryFile(mode='w+t', delete=False) 
203          print("Launching %s" % temp.name) 
204          launcher_filename = ros_utilities.find_resource(launcher['package'], launcher['name']) 
205          launch_text = '<launch>\n  <include file="%s"/>\n' % launcher_filename 
206          if args.screen: 
207              launch_text += '  <param name="rocon/screen" value="true"/>\n' 
208          else: 
209              launch_text += '  <param name="rocon/screen" value="false"/>\n' 
210          launch_text += '</launch>\n' 
211          temp.write(launch_text) 
212          temp.close()  # unlink it later 
213          temporary_launchers.append(temp) 
214          ########################## 
215          # Start the terminal 
216          ########################## 
217          if terminal == 'konsole': 
218              p = subprocess.Popen([terminal, '--nofork', '--hold', '-e', "/bin/bash", "-c", "roslaunch %s --port %s %s" % 
219                                (launcher['options'], launcher['port'], temp.name)], preexec_fn=preexec) 
220          elif terminal == 'gnome-terminal.wrapper' or terminal == 'gnome-terminal': 
221              # --disable-factory inherits the current environment, bit wierd. 
222              p = subprocess.Popen(['gnome-terminal', '--disable-factory', '-e', "/bin/bash", "-e", "roslaunch %s --port %s %s" % 
223                                (launcher['options'], launcher['port'], temp.name)], preexec_fn=preexec) 
224          else: 
225              cmd = ["roslaunch"] 
226              if launcher['options']: 
227                  cmd.append(launcher['options']) 
228              cmd.extend(["--port", launcher['port'], temp.name]) 
229              p = subprocess.Popen(cmd, preexec_fn=preexec) 
230          processes.append(p) 
231      signal.pause() 
232      # Have to unlink them here rather than in the for loop above, because the whole gnome-terminal 
233      # subprocess takes a while to kick in (in the background) and the unlinking may occur before 
234      # it actually runs the roslaunch that needs the file. 
235      for temporary_launcher in temporary_launchers: 
236          os.unlink(temporary_launcher.name) 
237