Merge pull request #47 from osrf/issue_42

added a builtin nodelet manager to the server

.coveralls.yml

@@ -0,0 +1 @@
+service_name: travis-ci

.travis.yml

@@ -7,7 +7,9 @@ install:
   - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list'
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
   - sudo apt-get update
-  - sudo apt-get install ros-hydro-roslaunch ros-hydro-rospy ros-hydro-std-msgs ros-hydro-std-srvs ros-hydro-geometry-msgs ros-hydro-message-generation ros-hydro-message-runtime ros-hydro-catkin ros-hydro-rostest ros-hydro-rosservice
+  - sudo apt-get install ros-hydro-nodelet ros-hydro-roslaunch ros-hydro-rospy ros-hydro-std-msgs ros-hydro-std-srvs ros-hydro-geometry-msgs ros-hydro-message-generation ros-hydro-message-runtime ros-hydro-catkin ros-hydro-rostest ros-hydro-rosservice
+# Install image_proc to get a nodelet instaleld as a workaround to https://github.com/ros/pluginlib/pull/22
+  - sudo apt-get install ros-hydro-image-proc
 # command to run tests
 script:
   - source /opt/ros/hydro/setup.bash

Makefile

@@ -17,8 +17,7 @@ coverage:
 	-rm ~/.ros/.coverage
 	-rm ${BUILD_DIR}/.coverage
 	-rm ./.coverage
-	cd ${BUILD_DIR} && mkdir src
-	ln -s ${SRC_DIR} ${BUILD_DIR}/src
+	-ln -s ${SRC_DIR} ${BUILD_DIR}/src
 	cd ${BUILD_DIR} && catkin_make
 	cd ${BUILD_DIR} && catkin_make tests
 	cd ${BUILD_DIR} && catkin_make -j1 run_tests

package.xml

@@ -25,6 +25,7 @@
   <build_depend>std_srvs</build_depend>
 
   <run_depend>message_runtime</run_depend>
+  <run_depend>nodelet</run_depend>
   <run_depend>python-yaml</run_depend>
   <run_depend>roslaunch</run_depend>
   <run_depend>rospy</run_depend>

src/capabilities/capability_server_nodelet_manager.launch

@@ -0,0 +1,6 @@
+<launch>
+    <arg name="capability_server_nodelet_manager_name" default="capability_server_nodelet_manager" />
+    <node pkg="nodelet" type="nodelet"
+          name="$(arg capability_server_nodelet_manager_name)"
+          output="screen" args="manager" />
+</launch>

src/capabilities/launch_manager.py

@@ -75,15 +75,18 @@ def is_exe(fpath):
                 return exe_file
     return None
 
-_placeholder_script = os.path.join(os.path.dirname(__file__), 'placeholder_script')
+_this_dir = os.path.dirname(__file__)
+_placeholder_script = os.path.join(_this_dir, 'placeholder_script')
+_nodelet_manager_launch_file = os.path.join(_this_dir, 'capability_server_nodelet_manager.launch')
+_special_nodelet_manager_capability = '!!nodelet_manager'
 
 
 class LaunchManager(object):
     """Manages multiple launch files which implement capabilities"""
     __roslaunch_exec = which('roslaunch')
     __python_exec = which('python')
 
-    def __init__(self, quiet=False, screen=False):
+    def __init__(self, quiet=False, screen=False, nodelet_manager_name=None):
         self.__running_launch_files_lock = threading.Lock()
         with self.__running_launch_files_lock:
             self.__running_launch_files = {}
@@ -94,6 +97,8 @@ def __init__(self, quiet=False, screen=False):
         self.stopping = False
         self.__quiet = quiet
         self.__screen = screen
+        self.__nodelet_manager_name = nodelet_manager_name or (rospy.get_name().lstrip('/') + '_nodelet_manager')
+        self.__start_nodelet_manager()
 
     def stop(self):
         """Stops the launch manager, also stopping any running launch files"""
@@ -112,8 +117,9 @@ def __stop_by_pid(self, pid):
         if pid not in self.__running_launch_files:
             raise RuntimeError("No running launch file with PID of '{0}'".format(pid))
         proc, thread, _, _ = self.__running_launch_files[pid]
-        proc.terminate()
-        proc.wait()
+        if proc.poll() is None:
+            proc.terminate()
+            proc.wait()
         thread.join()
 
     def stop_capability_provider(self, pid):
@@ -157,6 +163,9 @@ def run_capability_provider(self, provider, provider_path):
                           .format(provider.name))
         else:
             launch_file = os.path.join(provider_path, provider.launch_file)
+        self.run_launch_file(launch_file, provider)
+
+    def run_launch_file(self, launch_file, provider):
         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."
@@ -168,6 +177,7 @@ def run_capability_provider(self, provider, provider_path):
                     cmd = [self.__roslaunch_exec, '--screen', launch_file]
                 else:
                     cmd = [self.__roslaunch_exec, launch_file]
+                cmd.append("capability_server_nodelet_manager_name:=" + self.__nodelet_manager_name)
             if self.__quiet:
                 env = copy.deepcopy(os.environ)
                 env['PYTHONUNBUFFERED'] = 'x'
@@ -187,6 +197,14 @@ def run_capability_provider(self, provider, provider_path):
         self.__event_publisher.publish(msg)
         thread.start()
 
+    def __start_nodelet_manager(self):
+        class MockProvider:
+            implements = _special_nodelet_manager_capability
+            name = rospy.get_name().lstrip('/')
+        provider = MockProvider()
+        launch_file = _nodelet_manager_launch_file
+        self.run_launch_file(launch_file, provider)
+
     def __start_communication_thread(self, proc):
         return threading.Thread(target=self.__monitor_process, args=(proc,))
 

src/capabilities/server.py

@@ -76,6 +76,7 @@
 from capabilities.discovery import spec_file_index_from_package_index
 from capabilities.discovery import spec_index_from_spec_file_index
 
+from capabilities.launch_manager import _special_nodelet_manager_capability
 from capabilities.launch_manager import LaunchManager
 
 from capabilities.msg import Capability
@@ -473,6 +474,14 @@ def _handle_capability_events(self, event):
         # Ignore the `server_ready` event
         if event.type == event.SERVER_READY:
             return
+        # Specially handle the nodelet manager
+        if event.capability == _special_nodelet_manager_capability:
+            if event.type == event.LAUNCHED:
+                return
+            elif event.type == event.TERMINATED:
+                if not rospy.is_shutdown():
+                    rospy.logerr("Capability server's nodelet manager terminated unexpectedly.")
+                    self.shutdown()
         # Update the capability
         capability = event.capability
         with self.__graph_lock:

test/rostest/test_launch_manager/test_launch_manager.py

@@ -77,10 +77,13 @@ def test_launch_manager_no_launch_file_provider(self):
 
     def test_process_monitoring(self):
         lm = launch_manager.LaunchManager()
-        with assert_raises_regex(RuntimeError, 'Unknown process id'):
-            proc = Mock()
-            proc.pid = -1
-            lm._LaunchManager__monitor_process(proc)
+        try:
+            with assert_raises_regex(RuntimeError, 'Unknown process id'):
+                proc = Mock()
+                proc.pid = -1
+                lm._LaunchManager__monitor_process(proc)
+        finally:
+            lm.stop()
 
 if __name__ == '__main__':
     import rospy

test/rostest/test_server/test_invalid_specs.py

@@ -28,6 +28,7 @@ def test_invalid_specs(self):
         capability_server._CapabilityServer__load_capabilities()
         capability_server._CapabilityServer__populate_default_providers()
         capability_server._CapabilityServer__stop_capability('not_a_running_capability')
+        capability_server.shutdown()
 
     def test_no_default_provider_pedantic(self):
         no_default_provider = os.path.join(TEST_DIR, 'rostest', 'test_server', 'no_default_provider')
@@ -37,6 +38,7 @@ def test_no_default_provider_pedantic(self):
         capability_server._CapabilityServer__load_capabilities()
         with assert_raises(SystemExit):
             capability_server._CapabilityServer__populate_default_providers()
+        capability_server.shutdown()
 
     def test_no_default_provider(self):
         no_default_provider = os.path.join(TEST_DIR, 'rostest', 'test_server', 'no_default_provider')
@@ -45,6 +47,7 @@ def test_no_default_provider(self):
         capability_server = server.CapabilityServer(ros_package_path)
         capability_server._CapabilityServer__load_capabilities()
         capability_server._CapabilityServer__populate_default_providers()
+        capability_server.shutdown()
 
     def test_invalid_default_provider(self):
         minimal_dir = os.path.join(TEST_DIR, 'unit', 'discovery_workspaces', 'minimal')
@@ -54,6 +57,7 @@ def test_invalid_default_provider(self):
         capability_server._CapabilityServer__load_capabilities()
         with assert_raises(SystemExit):
             capability_server._CapabilityServer__populate_default_providers()
+        capability_server.shutdown()
 
     def test_wrong_default_provider(self):
         dc_dir = os.path.join(TEST_DIR, 'unit', 'discovery_workspaces', 'dependent_capabilities')
@@ -64,6 +68,7 @@ def test_wrong_default_provider(self):
         capability_server._CapabilityServer__load_capabilities()
         with assert_raises(SystemExit):
             capability_server._CapabilityServer__populate_default_providers()
+        capability_server.shutdown()
 
     def test_event_handler(self):
         invalid_specs_dir = os.path.join(TEST_DIR, 'unit', 'discovery_workspaces', 'invalid_specs')
@@ -80,6 +85,7 @@ def test_event_handler(self):
         msg.type = 'doesnt matter'
         pub.publish(msg)
         rospy.sleep(1)  # Allow time for the publish to happen
+        capability_server.shutdown()
 
 if __name__ == '__main__':
     rospy.init_node(TEST_NAME, anonymous=True)