def __monitor_process(self, proc): try: with self.__running_launch_files_lock: if proc.pid not in self.__running_launch_files: raise RuntimeError("Unknown process id: " + str(proc.pid)) provider = self.__running_launch_files[proc.pid][2] if proc.stdout is not None: while proc.returncode is None: try: for line in iter(proc.stdout.readline, ''): for output in self.__outputs: output.write(line) except KeyboardInterrupt: # pragma: no cover pass proc.poll() else: proc.wait() msg = CapabilityEvent() msg.header.stamp = rospy.Time.now() msg.capability = provider.implements msg.provider = provider.name msg.type = msg.TERMINATED msg.pid = proc.pid self.__event_publisher.publish(msg) except Exception as exc: rospy.logerr('{0}: {1}'.format(exc.__class__.__name__, str(exc))) raise
def run_launch_file(self, launch_file, provider, manager=False): with self.__running_launch_files_lock: if launch_file is not None and launch_file in [x[3] for x in self.__running_launch_files.values()]: raise RuntimeError("Launch file at '{0}' is already running." .format(launch_file)) if launch_file is None: cmd = [self.__python_exec, _placeholder_script] else: if self.__screen: cmd = [self.__roslaunch_exec, '--screen', launch_file] else: cmd = [self.__roslaunch_exec, launch_file] if manager: cmd.append("capability_server_nodelet_manager_name:=" + self.__nodelet_manager_name.split('/')[-1]) else: cmd.append("capability_server_nodelet_manager_name:=" + self.__nodelet_manager_name) if self.__quiet: env = copy.deepcopy(os.environ) env['PYTHONUNBUFFERED'] = 'x' proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, env=env) else: proc = subprocess.Popen(cmd) thread = self.__start_communication_thread(proc) msg = CapabilityEvent() msg.header.stamp = rospy.Time.now() msg.capability = provider.implements msg.provider = provider.name msg.pid = proc.pid msg.type = msg.LAUNCHED self.__running_launch_files[proc.pid] = [ proc, thread, provider, launch_file ] self.__event_publisher.publish(msg) thread.start()
def run_capability_provider(self, provider, provider_path): """Runs a given capability provider by launching its launch file :param provider: provider that should be launched :type provider: :py:class:`capabilities.specs.provider.CapabilityProvider` :param provider_path: path which the provider spec file is located at :type provider_path: str """ if os.path.isfile(provider_path): provider_path = os.path.dirname(provider_path) launch_file = os.path.join(provider_path, provider.launch_file) with self.__running_launch_files_lock: if launch_file in [x[3] for x in self.__running_launch_files]: raise RuntimeError("Launch file at '{0}' is already running." .format(launch_file)) cmd = [self.__roslaunch_exec, launch_file] proc = subprocess.Popen( cmd) thread = self.__start_communication_thread(proc) msg = CapabilityEvent() msg.header.stamp = rospy.Time.now() msg.capability = provider.implements msg.provider = provider.name msg.pid = proc.pid msg.type = msg.LAUNCHED self.__running_launch_files[proc.pid] = [ proc, thread, provider, launch_file ] self.__event_publisher.publish(msg) thread.start()
def test_event_handler(self): invalid_specs_dir = os.path.join(TEST_DIR, 'unit', 'discovery_workspaces', 'invalid_specs') ros_package_path = [invalid_specs_dir] capability_server = server.CapabilityServer(ros_package_path) capability_server._CapabilityServer__load_capabilities() capability_server._CapabilityServer__populate_default_providers() rospy.Subscriber('~events', CapabilityEvent, capability_server.handle_capability_events) pub = rospy.Publisher("~events", CapabilityEvent) rospy.sleep(1) msg = CapabilityEvent() msg.capability = 'some_pkg/NotACapability' msg.provider = 'doesnt matter' msg.type = 'doesnt matter' pub.publish(msg) rospy.sleep(1) # Allow time for the publish to happen
def run_capability_provider(self, provider, provider_path): """Runs a given capability provider by launching its launch file :param provider: provider that should be launched :type provider: :py:class:`capabilities.specs.provider.CapabilityProvider` :param provider_path: path which the provider spec file is located at :type provider_path: str """ if os.path.isfile(provider_path): provider_path = os.path.dirname(provider_path) if provider.launch_file is None: launch_file = None rospy.loginfo("Provider '{0}' does not have a launch file, running a placeholder." .format(provider.name)) else: launch_file = os.path.join(provider_path, provider.launch_file) with self.__running_launch_files_lock: if launch_file is not None and launch_file in [x[3] for x in self.__running_launch_files.values()]: raise RuntimeError("Launch file at '{0}' is already running." .format(launch_file)) if launch_file is None: cmd = [self.__python_exec, _placeholder_script] else: if self.__screen: cmd = [self.__roslaunch_exec, '--screen', launch_file] else: cmd = [self.__roslaunch_exec, launch_file] if self.__quiet: env = copy.deepcopy(os.environ) env['PYTHONUNBUFFERED'] = 'x' proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, env=env) else: proc = subprocess.Popen(cmd) thread = self.__start_communication_thread(proc) msg = CapabilityEvent() msg.header.stamp = rospy.Time.now() msg.capability = provider.implements msg.provider = provider.name msg.pid = proc.pid msg.type = msg.LAUNCHED self.__running_launch_files[proc.pid] = [ proc, thread, provider, launch_file ] self.__event_publisher.publish(msg) thread.start()
def __monitor_process(self, proc): try: with self.__running_launch_files_lock: if proc.pid not in self.__running_launch_files: raise RuntimeError("Unknown process id: " + str(proc.pid)) provider = self.__running_launch_files[proc.pid][2] if proc.stdout is not None: while proc.returncode is None: for line in iter(proc.stdout.readline, ''): for outputs in self.__outputs: outputs.write(line) proc.wait() msg = CapabilityEvent() msg.header.stamp = rospy.Time.now() msg.capability = provider.implements msg.provider = provider.name msg.type = msg.TERMINATED msg.pid = proc.pid self.__event_publisher.publish(msg) except Exception as e: print(str(e), file=sys.stderr)
def run_capability_provider(self, provider, provider_path): """Runs a given capability provider by launching its launch file :param provider: provider that should be launched :type provider: :py:class:`capabilities.specs.provider.CapabilityProvider` :param provider_path: path which the provider spec file is located at :type provider_path: str """ if os.path.isfile(provider_path): provider_path = os.path.dirname(provider_path) if provider.launch_file is None: launch_file = None rospy.loginfo( "Provider '{0}' does not have a launch file, running a placeholder." .format(provider.name)) else: launch_file = os.path.join(provider_path, provider.launch_file) with self.__running_launch_files_lock: if launch_file is not None and launch_file in [ x[3] for x in self.__running_launch_files.values() ]: raise RuntimeError( "Launch file at '{0}' is already running.".format( launch_file)) if launch_file is None: cmd = [self.__python_exec, _placeholder_script] else: if self.__screen: cmd = [self.__roslaunch_exec, '--screen', launch_file] else: cmd = [self.__roslaunch_exec, launch_file] if self.__quiet: env = copy.deepcopy(os.environ) env['PYTHONUNBUFFERED'] = 'x' proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, env=env) else: proc = subprocess.Popen(cmd) thread = self.__start_communication_thread(proc) msg = CapabilityEvent() msg.header.stamp = rospy.Time.now() msg.capability = provider.implements msg.provider = provider.name msg.pid = proc.pid msg.type = msg.LAUNCHED self.__running_launch_files[proc.pid] = [ proc, thread, provider, launch_file ] self.__event_publisher.publish(msg) thread.start()
def spin(self): """Starts the capability server by setting up ROS comms, then spins""" self.__package_whitelist = rospy.get_param('~package_whitelist', None) if not isinstance(self.__package_whitelist, (list, tuple, type(None))): msg = "~package_whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist)) rospy.logerr(msg) self.__package_whitelist = None self.__package_blacklist = rospy.get_param('~package_blacklist', None) if not isinstance(self.__package_blacklist, (list, tuple, type(None))): msg = "~package_blacklist must be a list or null, got a '{0}'".format(type(self.__whitelist)) rospy.logerr(msg) self.__package_blacklist = None self.__whitelist = rospy.get_param('~whitelist', None) if not isinstance(self.__whitelist, (list, tuple, type(None))): msg = "~whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist)) rospy.logerr(msg) self.__whitelist = None self.__blacklist = rospy.get_param('~blacklist', None) if not isinstance(self.__blacklist, (list, tuple, type(None))): msg = "~blacklist must be a list or null, got a '{0}'".format(type(self.__blacklist)) rospy.logerr(msg) self.__blacklist = None self.__debug = rospy.get_param('~debug', False) if self.__debug: logger = logging.getLogger('rosout') logger.setLevel(logging.DEBUG) rospy.logdebug('Debug messages enabled.') self.__load_capabilities() self.__bond_topic = rospy.get_name() + "/bonds" # Collect default arguments self.__populate_default_providers() rospy.Subscriber( '~events', CapabilityEvent, self.handle_capability_events) self.__start_capability_service = rospy.Service( '~start_capability', StartCapability, self.handle_start_capability) self.__stop_capability_service = rospy.Service( '~stop_capability', StopCapability, self.handle_stop_capability) self.__establish_bond_service = rospy.Service( '~establish_bond', EstablishBond, self.handle_establish_bond) self.__free_capability_service = rospy.Service( '~free_capability', FreeCapability, self.handle_free_capability) self.__use_capability_service = rospy.Service( '~use_capability', UseCapability, self.handle_use_capability) self.__reload_service = rospy.Service( '~reload_capabilities', Empty, self.handle_reload_request) self.__interfaces_service = rospy.Service( '~get_interfaces', GetInterfaces, self.handle_get_interfaces) self.__providers_service = rospy.Service( '~get_providers', GetProviders, self.handle_get_providers) self.__semantic_interfaces_service = rospy.Service( '~get_semantic_interfaces', GetSemanticInterfaces, self.handle_get_semantic_interfaces) self.__running_capabilities = rospy.Service( '~get_running_capabilities', GetRunningCapabilities, self.handle_get_running_capabilities) self.__capability_specs = rospy.Service( '~get_capability_specs', GetCapabilitySpecs, self.handle_get_capability_specs) self.__capability_spec = rospy.Service( '~get_capability_spec', GetCapabilitySpec, self.handle_get_capability_spec) self.__get_nodelet_manager_name_service = rospy.Service( '~get_nodelet_manager_name', GetNodeletManagerName, self.handle_get_nodelet_manager_name) self.__get_remappings_service = rospy.Service( '~get_remappings', GetRemappings, self.handle_get_remappings) rospy.loginfo("Capability Server Ready") rospy.Publisher("~events", CapabilityEvent, queue_size=1000).publish( CapabilityEvent(type=CapabilityEvent.SERVER_READY)) rospy.spin()
def test_external_event(self): # publish even from external rospy instance pub = rospy.Publisher("~events", CapabilityEvent) rospy.sleep(1) pub.publish(CapabilityEvent()) rospy.sleep(1)