Fix bond handling during nodelet unloading (#51)

* add test whether bond breaking on unload works (tests #50)

* disable callback for broken bond when we are breaking it

This avoids the nodelet::LoaderROS::unload() method to be called
twice for the same nodelet, causing an error output.

* use AsyncSpinner for nodelet load in order for the shutdown procedure to work

During shutdown, the bonds still need to communicate their status in order
for the nodelet to properly/cleanly/quickly unload. This requires the node
to spin.

* add test whether LoaderROS::unload() is called twice (tests #50)

nodelet/src/loader.cpp

@@ -89,6 +89,8 @@ class LoaderROS
   bool serviceLoad(nodelet::NodeletLoad::Request &req,
                    nodelet::NodeletLoad::Response &res)
   {
+    boost::mutex::scoped_lock lock(lock_);
+
     // build map
     M_string remappings;
     if (req.remap_source_args.size() != req.remap_target_args.size())
@@ -127,15 +129,23 @@ class LoaderROS
 
   bool unload(const std::string& name)
   {
+    boost::mutex::scoped_lock lock(lock_);
+
     bool success = parent_->unload(name);
     if (!success)
     {
       ROS_ERROR("Failed to find nodelet with name '%s' to unload.", name.c_str());
       return success;
     }
 
-    // Break the bond, if there is one
-    bond_map_.erase(name);
+    // break the bond, if there is one
+    M_stringToBond::iterator it = bond_map_.find(name);
+    if (it != bond_map_.end()) {
+        // disable callback for broken bond, as we are breaking it intentially now
+        it->second->setBrokenCallback(boost::function<void(void)>());
+        // erase (and break) bond
+        bond_map_.erase(name);
+    }
 
     return success;
   }
@@ -152,7 +162,9 @@ class LoaderROS
   ros::ServiceServer load_server_;
   ros::ServiceServer unload_server_;
   ros::ServiceServer list_server_;
-
+
+  boost::mutex lock_;
+
   ros::CallbackQueue bond_callback_queue_;
   ros::AsyncSpinner bond_spinner_;
   typedef boost::ptr_map<std::string, bond::Bond> M_stringToBond;

nodelet/src/nodelet.cpp

@@ -331,9 +331,10 @@ int
     if (arg_parser.isBondEnabled())
       bond.start();
     // Spin our own loop
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
     while (!request_shutdown)
     {
-      ros::spinOnce();
       if (arg_parser.isBondEnabled() && bond.isBroken())
       {
         ROS_INFO("Bond broken, exiting");

test_nodelet/CMakeLists.txt

@@ -29,6 +29,8 @@ if(CATKIN_ENABLE_TESTING)
   add_dependencies(tests test_console)
 
   add_rostest(test/test_console.launch)
+  add_rostest(test/test_bond_break_on_shutdown.launch)
+  add_rostest(test/test_unload_called_twice.launch)
 
   # Not a real test. Tries to measure overhead of CallbackQueueManager.
   add_executable(benchmark src/benchmark.cpp)

test_nodelet/debug_logging.conf

@@ -0,0 +1 @@
+log4j.logger.ros=DEBUG

test_nodelet/test/test_bond_break_on_shutdown.launch

@@ -0,0 +1,4 @@
+<launch>
+  <env name="ROSCONSOLE_CONFIG_FILE" value="$(find test_nodelet)/debug_logging.conf"/>
+  <test test-name="test_bond_break_on_shutdown" pkg="test_nodelet" type="test_bond_break_on_shutdown.py" />
+</launch>

test_nodelet/test/test_bond_break_on_shutdown.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+
+import roslib; roslib.load_manifest('test_nodelet')
+import rospy
+import unittest
+import rostest
+import signal
+import subprocess
+import time
+
+from nodelet.srv import *
+
+class TestBondBreakOnShutdown(unittest.TestCase):
+    def test_bond_break_on_shutdown(self):
+        '''
+        Test that the bond is broken cleanly when closing a nodelet loader (#50).
+        This relies on a debug message printed by the bondcpp package in case
+        of error.
+        '''
+
+        # start nodelet manager
+        proc_manager = subprocess.Popen(["rosrun", "nodelet", "nodelet", "manager", "__name:=nodelet_manager"],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE,
+            bufsize=-1
+        )
+
+        # wait for nodelet manager to be ready
+        try:
+            rospy.wait_for_service('/nodelet_manager/load_nodelet', timeout=2)
+        except:
+            self.fail("Could not determine that nodelet manager has started")
+
+        # load a nodelet
+        proc_nodelet = subprocess.Popen(["rosrun", "nodelet", "nodelet", "load", "test_nodelet/Plus", "nodelet_manager", "__name:=test"],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE,
+            bufsize=-1
+        )
+
+        # wait for it to be ready
+        try:
+            rospy.wait_for_service('test/get_loggers', timeout=2)
+        except:
+            self.fail("Could not determine that nodelet has started")
+        time.sleep(1)
+
+        # stop the nodelet loader via signal (similar to roslaunch killing it)
+        proc_nodelet.send_signal(signal.SIGINT)
+        (n_out, n_err) = proc_nodelet.communicate()
+
+        # stop the nodelet manager, too
+        proc_manager.send_signal(signal.SIGINT)
+        (m_out, m_err) = proc_manager.communicate()
+
+        # check that nodelet unloaded and there was no error with bond breaking
+        self.assertIn('Unloading nodelet /test from manager nodelet_manager', n_out)
+        self.assertNotIn('Bond failed to break on destruction', m_out)
+        self.assertNotIn('Bond failed to break on destruction', n_out)
+
+if __name__ == '__main__':
+    rospy.init_node('test_bond_break_on_shutdown')
+    rostest.unitrun('test_bond_break_on_shutdown', 'test_bond_break_on_shutdown', TestBondBreakOnShutdown)
+

test_nodelet/test/test_unload_called_twice.launch

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="unload_called_twice" pkg="test_nodelet" type="test_unload_called_twice.py" />
+</launch>

test_nodelet/test/test_unload_called_twice.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python
+
+import roslib; roslib.load_manifest('test_nodelet')
+import rospy
+import unittest
+import rostest
+import signal
+import subprocess
+import time
+
+from nodelet.srv import *
+
+class TestUnloadCalledTwice(unittest.TestCase):
+    def test_unload_called_twice(self):
+        '''
+        Test that when a nodelet loader is stopped and requests unloading,
+        the unload() call in LoaderROS is not run twice (#50).
+        '''
+
+        # start nodelet manager
+        proc_manager = subprocess.Popen(["rosrun", "nodelet", "nodelet", "manager", "__name:=nodelet_manager"],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE,
+            bufsize=-1
+        )
+
+        # wait for nodelet manager to be ready
+        try:
+            rospy.wait_for_service('/nodelet_manager/load_nodelet', timeout=2)
+        except:
+            self.fail("Could not determine that nodelet manager has started")
+
+        # load nodelet
+        proc_nodelet = subprocess.Popen(["rosrun", "nodelet", "nodelet", "load", "test_nodelet/Plus", "nodelet_manager", "__name:=test"],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE
+        )
+
+        # wait for nodelet to be ready
+        try:
+            rospy.wait_for_service('test/get_loggers', timeout=2)
+        except:
+            self.fail("Could not determine that nodelet has started")
+        time.sleep(1)
+
+        # stop the nodelet loader via signal (similar to roslaunch killing it)
+        proc_nodelet.send_signal(signal.SIGINT)
+        (n_out, n_err) = proc_nodelet.communicate()
+
+        # stop the nodelet manager, too
+        proc_manager.send_signal(signal.SIGINT)
+        (m_out, m_err) = proc_manager.communicate()
+
+        # check that nodelet unloaded and that LoaderROS::unload() does not
+        # complain about nodelet not being found (an indication that it was called
+        # again after the nodelet was already unloaded)
+        self.assertIn('Unloading nodelet /test from manager nodelet_manager', n_out)
+        self.assertNotIn('Failed to find nodelet with name', m_err)
+
+if __name__ == '__main__':
+    rospy.init_node('test_unload_called_twice')
+    rostest.unitrun('test_unload_called_twice', 'test_unload_called_twice', TestUnloadCalledTwice)