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 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 __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 __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)
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)
