Package roslaunch :: Module parent
[frames] | no frames]

Source Code for Module roslaunch.parent

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2008, Willow Garage, Inc. 
  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 Willow Garage, Inc. 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  # Revision $Id$ 
 34   
 35  """ 
 36  roslaunch.parent providees the L{ROSLaunchParent} implementation, 
 37  which represents the main 'parent' roslaunch process.  
 38   
 39  ROSLaunch has a client/server architecture for running remote 
 40  processes. When a user runs roslaunch, this creates a "parent" 
 41  roslaunch process, which is responsible for managing local 
 42  processes. This parent process will also start "child" processes on 
 43  remote machines. The parent can then invoke methods on this child 
 44  process to launch remote processes, and the child can invoke methods 
 45  on the parent to provide feedback. 
 46  """ 
 47   
 48  import logging 
 49   
 50  import roslaunch.config 
 51  from roslaunch.core import printlog_bold, printerrlog, RLException 
 52  import roslaunch.launch 
 53  import roslaunch.pmon 
 54  import roslaunch.server 
 55  import roslaunch.xmlloader  
 56   
 57  from rosmaster.master_api import NUM_WORKERS 
 58  from roslaunch.nodeprocess import DEFAULT_TIMEOUT_SIGINT, DEFAULT_TIMEOUT_SIGTERM 
 59   
 60  #TODO: probably move process listener infrastructure into here 
 61   
 62  # TODO: remove after wg_hardware_roslaunch has been updated 
 63  # qualification accesses this API, which has been relocated 
 64  load_config_default = roslaunch.config.load_config_default 
 65   
66 -class ROSLaunchParent(object):
67 """ 68 ROSLaunchParent represents the main 'parent' roslaunch process. It 69 is responsible for loading the launch files, assigning machines, 70 and then starting up any remote processes. The __main__ method 71 delegates most of runtime to ROSLaunchParent. 72 73 This must be called from the Python Main thread due to signal registration. 74 """ 75
76 - def __init__(self, run_id, roslaunch_files, is_core=False, port=None, local_only=False, process_listeners=None, 77 verbose=False, force_screen=False, force_log=False, is_rostest=False, roslaunch_strs=None, num_workers=NUM_WORKERS, timeout=None, master_logger_level=False, show_summary=True, force_required=False, 78 sigint_timeout=DEFAULT_TIMEOUT_SIGINT, sigterm_timeout=DEFAULT_TIMEOUT_SIGTERM):
79 """ 80 @param run_id: UUID of roslaunch session 81 @type run_id: str 82 @param roslaunch_files: list of launch configuration 83 files to load 84 @type roslaunch_files: [str] 85 @param is_core bool: if True, this launch is a roscore 86 instance. This affects the error behavior if a master is 87 already running (i.e. it fails). 88 @type is_core: bool 89 @param process_listeners: (optional) list of process listeners 90 to register with process monitor once launch is running 91 @type process_listeners: [L{roslaunch.pmon.ProcessListener}] 92 @param port: (optional) override master port number from what is specified in the master URI. 93 @type port: int 94 @param verbose: (optional) print verbose output 95 @type verbose: boolean 96 @param show_summary: (optional) whether to show a summary or not 97 @type show_summary: boolean 98 @param force_screen: (optional) force output of all nodes to screen 99 @type force_screen: boolean 100 @param force_log: (optional) force output of all nodes to log 101 @type force_log: boolean 102 @param is_rostest bool: if True, this launch is a rostest 103 instance. This affects validation checks. 104 @type is_rostest: bool 105 @param num_workers: If this is the core, the number of worker-threads to use. 106 @type num_workers: int 107 @param timeout: If this is the core, the socket-timeout to use. 108 @type timeout: Float or None 109 @throws RLException 110 @param master_logger_level: Specify roscore's rosmaster.master logger level, use default if it is False. 111 @type master_logger_level: str or False 112 @param force_required: (optional) whether to make all nodes required 113 @type force_required: boolean 114 @param sigint_timeout: The SIGINT timeout used when killing nodes (in seconds). 115 @type sigint_timeout: float 116 @param sigterm_timeout: The SIGTERM timeout used when killing nodes if SIGINT does not stop the node (in seconds). 117 @type sigterm_timeout: float 118 @raise RLException: If sigint_timeout or sigterm_timeout are nonpositive. 119 """ 120 if sigint_timeout <= 0: 121 raise RLException("sigint_timeout must be a positive number, received %f" % sigint_timeout) 122 if sigterm_timeout <= 0: 123 raise RLException("sigterm_timeout must be a positive number, received %f" % sigterm_timeout) 124 125 self.logger = logging.getLogger('roslaunch.parent') 126 self.run_id = run_id 127 self.process_listeners = process_listeners 128 129 self.roslaunch_files = roslaunch_files 130 self.roslaunch_strs = roslaunch_strs 131 self.is_core = is_core 132 self.is_rostest = is_rostest 133 self.port = port 134 self.local_only = local_only 135 self.verbose = verbose 136 self.show_summary = show_summary 137 self.num_workers = num_workers 138 self.timeout = timeout 139 self.master_logger_level = master_logger_level 140 self.sigint_timeout = sigint_timeout 141 self.sigterm_timeout = sigterm_timeout 142 143 # I don't think we should have to pass in so many options from 144 # the outside into the roslaunch parent. One possibility is to 145 # allow alternate config loaders to be passed in. 146 self.force_screen = force_screen 147 self.force_log = force_log 148 self.force_required = force_required 149 150 # flag to prevent multiple shutdown attempts 151 self._shutting_down = False 152 153 self.config = self.runner = self.server = self.pm = self.remote_runner = None
154
155 - def _load_config(self):
156 self.config = roslaunch.config.load_config_default(self.roslaunch_files, self.port, 157 roslaunch_strs=self.roslaunch_strs, verbose=self.verbose) 158 159 # #2370 (I really want to move this logic outside of parent) 160 if self.force_screen: 161 for n in self.config.nodes: 162 n.output = 'screen' 163 if self.force_log: 164 for n in self.config.nodes: 165 n.output = 'log' 166 if self.force_required: 167 for n in self.config.nodes: 168 n.required = True 169 if n.respawn and n.required: 170 raise ValueError("respawn and required cannot both be set to true")
171 172
173 - def _start_pm(self):
174 """ 175 Start the process monitor 176 """ 177 self.pm = roslaunch.pmon.start_process_monitor()
178
179 - def _init_runner(self):
180 """ 181 Initialize the roslaunch runner 182 """ 183 if self.config is None: 184 raise RLException("config is not initialized") 185 if self.pm is None: 186 raise RLException("pm is not initialized") 187 if self.server is None: 188 raise RLException("server is not initialized") 189 self.runner = roslaunch.launch.ROSLaunchRunner(self.run_id, self.config, server_uri=self.server.uri, pmon=self.pm, is_core=self.is_core, remote_runner=self.remote_runner, is_rostest=self.is_rostest, num_workers=self.num_workers, timeout=self.timeout, master_logger_level=self.master_logger_level, 190 sigint_timeout=self.sigint_timeout, sigterm_timeout=self.sigterm_timeout) 191 192 # print runner info to user, put errors last to make the more visible 193 if self.is_core: 194 print("ros_comm version %s" % (self.config.params['/rosversion'].value)) 195 if self.show_summary: 196 print(self.config.summary(local=self.remote_runner is None)) 197 if self.config: 198 for err in self.config.config_errors: 199 printerrlog("WARNING: %s"%err)
200
201 - def _start_server(self):
202 """ 203 Initialize the roslaunch parent XML-RPC server 204 """ 205 if self.config is None: 206 raise RLException("config is not initialized") 207 if self.pm is None: 208 raise RLException("pm is not initialized") 209 210 self.logger.info("starting parent XML-RPC server") 211 self.server = roslaunch.server.ROSLaunchParentNode(self.config, self.pm) 212 self.server.start() 213 if not self.server.uri: 214 raise RLException("server URI did not initialize") 215 self.logger.info("... parent XML-RPC server started")
216
217 - def _init_remote(self):
218 """ 219 Initialize the remote process runner, if required. Subroutine 220 of _start_remote, separated out for easier testing 221 """ 222 if self.config is None: 223 raise RLException("config is not initialized") 224 if self.pm is None: 225 raise RLException("pm is not initialized") 226 if self.server is None: 227 raise RLException("server is not initialized") 228 229 if not self.local_only and self.config.has_remote_nodes(): 230 # keep the remote package lazy-imported 231 import roslaunch.remote 232 self.remote_runner = roslaunch.remote.ROSRemoteRunner(self.run_id, self.config, self.pm, self.server, 233 sigint_timeout=self.sigint_timeout, 234 sigterm_timeout=self.sigterm_timeout) 235 elif self.local_only: 236 printlog_bold("LOCAL\nlocal only launch specified, will not launch remote nodes\nLOCAL\n")
237
238 - def _start_remote(self):
239 """ 240 Initializes and runs the remote process runner, if required 241 """ 242 if self.remote_runner is None: 243 self._init_remote() 244 245 if self.remote_runner is not None: 246 # start_servers() runs the roslaunch children 247 self.remote_runner.start_children()
248
249 - def _start_infrastructure(self):
250 """ 251 load config, start XMLRPC servers and process monitor 252 """ 253 if self.config is None: 254 self._load_config() 255 256 # Start the process monitor 257 if self.pm is None: 258 self._start_pm() 259 260 # Startup the roslaunch runner and XMLRPC server. 261 # Requires pm 262 if self.server is None: 263 self._start_server() 264 265 # Startup the remote infrastructure. 266 # Requires config, pm, and server 267 self._start_remote()
268
269 - def _stop_infrastructure(self):
270 """ 271 Tear down server and process monitor. Not multithread safe. 272 """ 273 #TODO more explicit teardown of remote infrastructure 274 275 # test and set flag so we don't shutdown multiple times 276 if self._shutting_down: 277 return 278 self._shutting_down = True 279 280 if self.server: 281 try: 282 self.server.shutdown("roslaunch parent complete") 283 except: 284 # don't let exceptions halt the rest of the shutdown 285 pass 286 if self.pm: 287 self.pm.shutdown() 288 self.pm.join()
289
290 - def start(self, auto_terminate=True):
291 """ 292 Run the parent roslaunch. 293 294 @param auto_terminate: stop process monitor once there are no 295 more processes to monitor (default True). This defaults to 296 True, which is the command-line behavior of roslaunch. Scripts 297 may wish to set this to False if they wish to keep the 298 roslauch infrastructure up regardless of processes being 299 monitored. 300 """ 301 self.logger.info("starting roslaunch parent run") 302 303 # load config, start XMLRPC servers and process monitor 304 try: 305 self._start_infrastructure() 306 except: 307 # infrastructure did not initialize, do teardown on whatever did come up 308 self._stop_infrastructure() 309 raise 310 311 # Initialize the actual runner. 312 # Requires config, pm, server and remote_runner 313 self._init_runner() 314 315 # Start the launch 316 self.runner.launch() 317 318 # inform process monitor that we are done with process registration 319 if auto_terminate: 320 self.pm.registrations_complete() 321 322 self.logger.info("... roslaunch parent running, waiting for process exit") 323 if self.process_listeners: 324 for l in self.process_listeners: 325 self.runner.pm.add_process_listener(l) 326 # Add listeners to server as well, otherwise they won't be 327 # called when a node on a remote machine dies. 328 self.server.add_process_listener(l)
329
330 - def spin_once(self):
331 """ 332 Run the parent roslaunch event loop once 333 """ 334 if self.runner: 335 self.runner.spin_once()
336
337 - def spin(self):
338 """ 339 Run the parent roslaunch until exit 340 """ 341 if not self.runner: 342 raise RLException("parent not started yet") 343 try: 344 # Blocks until all processes dead/shutdown 345 self.runner.spin() 346 finally: 347 self._stop_infrastructure()
348
349 - def shutdown(self):
350 """ 351 Stop the parent roslaunch. 352 """ 353 self._stop_infrastructure()
354