Enable nodelet support

rocon_app_manager/launch/includes/_app_manager.xml

@@ -12,6 +12,7 @@
   <arg name="rapp_package_blacklist" default="[]"/>
   <arg name="auto_start_rapp" default=""/> <!-- autostart a rapp, e.g. rocon_apps/chirp -->
   <arg name="screen" default="false"/>  <!-- verbose output from running apps -->
+  <arg name="capability_server_name" default="capability_server"/>  <!-- name of the capability server -->
 
   <!-- **************************** App Manager ******************************** -->
   <node pkg="rocon_app_manager" type="rapp_manager.py" name="app_manager">
@@ -24,6 +25,7 @@
     <param name="auto_start_rapp" value="$(arg auto_start_rapp)"/>
     <param name="local_remote_controllers_only" value="$(arg local_remote_controllers_only)"/>
     <param name="screen" value="$(arg screen)"/>
+    <param name="capability_server_name" value="$(arg capability_server_name)"/>
     <remap from="app_manager/gateway_info" to="gateway/gateway_info"/>
     <remap from="app_manager/remote_gateway_info" to="gateway/remote_gateway_info"/>
     <remap from="app_manager/force_update" to="gateway/force_update"/>

rocon_app_manager/launch/paired_private.launch

@@ -17,6 +17,7 @@
   <arg name="auto_start_rapp" default=""/> <!-- autostart a rapp, e.g. rocon_apps/chirp -->
   <arg name="local_remote_controllers_only" default="false"/> <!-- allow remote control for local machine tests only -->
   <arg name="screen" default="false"/>  <!-- verbose output from running apps -->
+  <arg name="capability_server_name" default="capability_server"/>  <!-- name of the capability server -->
 
   <!-- ******************************* Zeroconf ******************************** -->
   <node ns="zeroconf" pkg="zeroconf_avahi" type="zeroconf" name="zeroconf"/>
@@ -40,5 +41,6 @@
     <arg name="auto_start_rapp" value="$(arg auto_start_rapp)" />
     <arg name="local_remote_controllers_only" value="$(arg local_remote_controllers_only)" />
     <arg name="screen" value="$(arg screen)" />
+    <arg name="capability_server_name" default="$(arg capability_server_name)"/>
   </include>
 </launch>

rocon_app_manager/src/rocon_app_manager/caps_list.py

@@ -12,12 +12,14 @@
 # Imports
 ##############################################################################
 
-import rospy
 from capabilities import client
 from capabilities import discovery
 from capabilities import service_discovery
 import capabilities.srv as capabilities_srvs
+import rocon_python_comms
+import rospy
 from .exceptions import MissingCapabilitiesException
+from .exceptions import NotFoundException
 
 ##############################################################################
 # Class
@@ -28,33 +30,51 @@ class CapsList(object):
     '''
     CapsLists stores the data about the available capabilities retrieved from the capability server
     '''
-    def __init__(self, capability_server_node_name='capability_server'):
+    def __init__(self):
         '''
         Sets up a client for the capability server, including a bond.
         Also, retrieves the specifications for the available interfaces and providers from the capability server
 
-        :param capability_server_node_name: name of the remote capability server
-        :type capability_server_node_name: str
-
         @raise rospy.exceptions.ROSException: Exception is raised when retrieving the capability data
                                               from the capability server returned errors
-                                              or when waiting for the capability server's services times out.
+                                              or when waiting for the capability server's services times out
+                                              or when failing to retrieve the capability server's nodelet manager name
         '''
+
         self._default_timeout = 3.0
+        capability_server_node_name = rospy.get_param("~capability_server_name", "capability_server")
+
+        # look for fully resolved name of the capability server
+        try:
+            fully_resolved_name = rocon_python_comms.find_node(capability_server_node_name, unique=True)[0]
+        except rocon_python_comms.NotFoundException as e:
+            raise NotFoundException("Couldn't find capability server node. Error: " + str(e))
 
         # set up client
-        self._caps_client = client.CapabilitiesClient(capability_server_node_name)
+        self._caps_client = client.CapabilitiesClient(fully_resolved_name)
         if not self._caps_client.wait_for_services(self._default_timeout):
-            raise rospy.exceptions.ROSException("Timed out when waiting for the capability server.")
-
+            raise NotFoundException("Timed out when waiting for the capability server (" + fully_resolved_name + ").")
         # establish_bond
         self._caps_client.establish_bond(self._default_timeout)
 
+        # get the name of the capability server's nodelet manager
+        service_name = fully_resolved_name + '/get_nodelet_manager_name'
+        cap_server_nodelet_manager_srv = rospy.ServiceProxy(service_name, capabilities_srvs.GetNodeletManagerName)
+        try:
+            resp = cap_server_nodelet_manager_srv()
+        except rospy.ServiceException as exc:
+            raise NotFoundException("Couldn't not get capability server's nodelet manager name: " + str(exc))
+        self.nodelet_manager_name = resp.nodelet_manager_name
+
         # get spec index
-        self._spec_index, errors = service_discovery.spec_index_from_service(capability_server_node_name,
-                                                                             self._default_timeout)
+        try:
+            self._spec_index, errors = service_discovery.spec_index_from_service(fully_resolved_name,
+                                                                                 self._default_timeout)
+        except rospy.exceptions.ROSException as e:
+            raise NotFoundException("Couldn't get specification index. Error: " + str(e))
+
         if errors:
-            raise rospy.exceptions.ROSException("Couldn't get specification index. Error: " + str(errors))
+            raise NotFoundException("Couldn't get specification index. Error: " + str(errors))
 
         self._available_interfaces = []
         self._available_semantic_interfaces = []

rocon_app_manager/src/rocon_app_manager/rapp.py

@@ -63,7 +63,7 @@ class Rapp(object):
     # args that can be parsed for inside a rapp launcher, these args
     # get passed down in the temporary launcher that gets constructed when
     # pushing down a namespace.
-    standard_args = ['gateway_name', 'application_namespace', 'rocon_uri']
+    standard_args = ['gateway_name', 'application_namespace', 'rocon_uri', 'capability_server_nodelet_manager_name']
 
     def __init__(self, package, package_relative_rapp_filename):
         '''
@@ -231,13 +231,19 @@ def start(self, application_namespace, gateway_name, rocon_uri_string, remapping
         try:
             # Create modified roslaunch file include the application namespace (robot name + 'application')
             temp = tempfile.NamedTemporaryFile(mode='w+t', delete=False)
-            launch_text = _prepare_launch_text(
-                                    data['launch'],
-                                    data['launch_args'],
-                                    application_namespace,
-                                    gateway_name,
-                                    rocon_uri_string
-                                    )
+            if caps_list:
+                launch_text = _prepare_launch_text(data['launch'],
+                                                   data['launch_args'],
+                                                   application_namespace,
+                                                   gateway_name,
+                                                   rocon_uri_string,
+                                                   caps_list.nodelet_manager_name)
+            else:
+                launch_text = _prepare_launch_text(data['launch'],
+                                                   data['launch_args'],
+                                                   application_namespace,
+                                                   gateway_name,
+                                                   rocon_uri_string)
             temp.write(launch_text)
             temp.close()  # unlink it later
 
@@ -409,7 +415,7 @@ def get_standard_args(roslaunch_file):
 
 
 def _prepare_launch_text(launch_file, launch_args, application_namespace,
-                        gateway_name, rocon_uri_string):
+                        gateway_name, rocon_uri_string, capability_server_nodelet_manager_name=None):
     '''
       Prepare the launch file text. This essentially wraps the rapp launcher
       with the following roslaunch elements:
@@ -446,6 +452,7 @@ def _prepare_launch_text(launch_file, launch_args, application_namespace,
     launch_arg_mapping['application_namespace'] = application_namespace
     launch_arg_mapping['gateway_name'] = gateway_name
     launch_arg_mapping['rocon_uri'] = rocon_uri_string
+    launch_arg_mapping['capability_server_nodelet_manager_name'] = capability_server_nodelet_manager_name
 
     launch_text = '<launch>\n  <include ns="%s" file="%s">\n' % (application_namespace, launch_file)
     for arg in launch_args:

rocon_app_manager/src/rocon_app_manager/rapp_manager.py

@@ -226,9 +226,9 @@ def _determine_runnable_rapps(self, rapps):
         no_caps_available = False
         try:
             self.caps_list = CapsList()
-        except rospy.exceptions.ROSException as e:
-            if 'timeout' in str(e):
-                rospy.loginfo("App Manager : disabling apps requiring capabilities [timed out looking for the capability server]")
+        except exceptions.NotFoundException as e:
+            if 'Timed out' in str(e) or "Couldn't find" in str(e):
+                rospy.loginfo("App Manager : disabling apps requiring capabilities [%s]" % str(e))
             else:
                 rospy.logwarn("App Manager : disabling apps requiring capabilities [%s]" % str(e))
             no_caps_available = True