Added more tests for Nodelet::ok() and Nodelet::requestStop().

ros · Apr 11, 2023 · 165b9ef · 165b9ef
1 parent a6a95a1
commit 165b9ef
Show file tree

Hide file tree

Showing 11 changed files with 274 additions and 9 deletions.
diff --git a/nodelet/include/nodelet/nodelet.h b/nodelet/include/nodelet/nodelet.h
@@ -236,16 +236,15 @@ class NODELETLIB_DECL Nodelet
 
   /**\brief Whether it is OK to continue working with this nodelet. This function starts returning true right
    *        before onInit() is called and starts returning false when the nodelet is requested to stop via
-   *        requestStop() or execution of its destructor.
+   *        requestStop().
    * \return Status of the nodelet.
-   * \note You have to make sure the destructor does not finish while you are querying this->ok()! So it is best to
-   *       put a synchronization primitive in the child class destructor that will make sure all callbacks querying
-   *       this->ok() will finish prior to proceeding with the object destruction. But be aware that any deadlock
-   *       in such a destructor would prevent any other nodelets to be loaded/unloaded into the nodelet manager.
+   * \note This does not take into account `ros::ok()`. This should be queried separately.
    */
   bool ok() const;
 
   /**\brief Request this nodelet to stop. This function returns immediately. Afterwards, ok() returns false.
+   *        This function is automatically called by the nodelet manager when the nodelet is requested to unload
+   *        or is killed.
    */
   void requestStop();
 

diff --git a/nodelet/src/nodelet_class.cpp b/nodelet/src/nodelet_class.cpp
@@ -45,7 +45,11 @@ Nodelet::Nodelet ()
 
 Nodelet::~Nodelet()
 {
-  requestStop();
+  // Calling requestStop() would not make sense here. Any downstream class that
+  // would like to use ok() to check for validity of itself would have already
+  // been destroyed by its own destructor before getting here.
+  // requestStop() has to be called by an external manager before actually
+  // starting the destruction sequence.
 }
 
 ros::CallbackQueueInterface& Nodelet::getSTCallbackQueue () const

diff --git a/test_nodelet/CMakeLists.txt b/test_nodelet/CMakeLists.txt
@@ -7,7 +7,7 @@ catkin_package()
 
 if(CATKIN_ENABLE_TESTING)
   find_package(Boost REQUIRED thread)
-  find_package(catkin REQUIRED nodelet pluginlib rostest)
+  find_package(catkin REQUIRED bond nodelet pluginlib rostest)
   include_directories(SYSTEM ${BOOST_INCLUDE_DIRS}
                              ${catkin_INCLUDE_DIRS}
   )
@@ -17,7 +17,7 @@ if(CATKIN_ENABLE_TESTING)
   find_package(Threads REQUIRED)
 
   #common commands for building c++ executables and libraries
-  add_library(${PROJECT_NAME} src/plus.cpp src/console_tests.cpp src/failing_nodelet.cpp src/nodehandles.cpp)
+  add_library(${PROJECT_NAME} src/plus.cpp src/console_tests.cpp src/failing_nodelet.cpp src/nodehandles.cpp src/request_stop.cpp)
   target_link_libraries(${PROJECT_NAME} ${BOOST_LIBRARIES}
                                         ${catkin_LIBRARIES}
   )
@@ -65,4 +65,8 @@ if(CATKIN_ENABLE_TESTING)
 
   add_rostest_gtest(test_nodelet_request_stop test/test_nodelet_request_stop.test test/test_nodelet_request_stop.cpp)
   target_link_libraries(test_nodelet_request_stop ${BOOST_LIBRARIES} ${catkin_LIBRARIES} Threads::Threads)
+
+  add_rostest(test/test_request_stop_unload.test DEPENDENCIES ${PROJECT_NAME})
+  add_rostest(test/test_request_stop_unload_standalone.test DEPENDENCIES ${PROJECT_NAME})
+  add_rostest(test/test_request_stop_unload_kill_manager.test DEPENDENCIES ${PROJECT_NAME})
 endif()
diff --git a/test_nodelet/package.xml b/test_nodelet/package.xml
@@ -21,7 +21,9 @@
   <depend>rostest</depend>
   <depend>std_msgs</depend>
 
+  <test_depend>bond</test_depend>
   <test_depend>rosbash</test_depend>
+  <test_depend>rospy</test_depend>
 
   <export>
     <nodelet plugin="${prefix}/test_nodelet.xml"/>

diff --git a/test_nodelet/src/request_stop.cpp b/test_nodelet/src/request_stop.cpp
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2023, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <bond/Status.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+namespace test_nodelet
+{
+
+class RequestStop : public nodelet::Nodelet
+{
+private:
+  void onInit() override
+  {
+    ros::NodeHandle pnh = getPrivateNodeHandle();
+    std::string managerName {};
+    managerName = pnh.param("manager_name", managerName);
+    waitForBond_ = pnh.param("wait_for_bond", waitForBond_);
+
+    ros::NodeHandle nh = getNodeHandle();
+    pub_ = nh.advertise<std_msgs::String>("ready", 1, true);
+    timer_ = nh.createSteadyTimer(ros::WallDuration(0.1), &RequestStop::callback, this, false);
+    if (waitForBond_)
+      sub_ = nh.subscribe(ros::names::append(managerName, "bond"), 4, &RequestStop::bond, this);
+  }
+
+  void bond(const bond::Status&)
+  {
+    numBondMsgs_ += 1;
+  }
+
+  void callback(const ros::SteadyTimerEvent&)
+  {
+    // If running non-standalone, wait for the bonds to establish; otherwise, killing
+    // this nodelet too early would mean waiting 10 seconds for bond connect timeout, instead
+    // of waiting just 4 seconds for bond heartbeat timeout. Each end of the bond sends
+    // one message per second, so we wait for 2 messages to be sent from each end.
+
+    if (waitForBond_ && numBondMsgs_ < 4)
+      return;
+
+    timer_.stop();
+
+    std_msgs::String ready;
+    ready.data = this->getPrivateNodeHandle().getNamespace();
+    NODELET_DEBUG("Ready");
+    pub_.publish(ready);
+
+    // Run an infinite loop which can only be ended by an async call to requestShutdown().
+    while (ros::ok() && this->ok())
+      ros::WallDuration(0.1).sleep();
+
+    ready.data = "interrupted";
+    NODELET_INFO("Interrupted");
+    std::cout << "Interrupted" << std::endl;
+    pub_.publish(ready);
+  }
+
+  bool waitForBond_ {false};
+  size_t numBondMsgs_ {0u};
+  ros::Publisher pub_;
+  ros::Subscriber sub_;
+  ros::SteadyTimer timer_;
+};
+
+PLUGINLIB_EXPORT_CLASS(test_nodelet::RequestStop, nodelet::Nodelet)
+}
diff --git a/test_nodelet/test/test_nodelet_request_stop.cpp b/test_nodelet/test/test_nodelet_request_stop.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Open Source Robotics Foundation
+ * Copyright (c) 2023, Open Source Robotics Foundation
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without

diff --git a/test_nodelet/test/test_request_stop.py b/test_nodelet/test/test_request_stop.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2023, Open Source Robotics Foundation
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+import rosnode
+import rospy
+import rostest
+import sys
+from time import sleep
+import unittest
+
+from bond.msg import Status
+from nodelet.srv import NodeletUnload, NodeletUnloadRequest
+from std_msgs.msg import String
+
+
+class TestRequestStop(unittest.TestCase):
+    def __init__(self, methodName='runTest'):
+        super(TestRequestStop, self).__init__(methodName)
+        self._last_ready_msg = None
+        self._num_received = 0
+        self._stop_method = rospy.get_param("~stop_method", "unload")
+        self._manager_name = rospy.get_param("~manager_name", "")
+        self._wait_for_bond = rospy.get_param("~wait_for_bond", False)
+        if self._wait_for_bond:
+            self._num_bond_msgs = 0
+            self._bond_sub = rospy.Subscriber(
+                rospy.names.ns_join(self._manager_name, "bond"), Status, self.bond_cb, queue_size=4)
+
+    def bond_cb(self, msg):
+        self._num_bond_msgs += 1
+
+    def ready_cb(self, msg):
+        self._last_ready_msg = msg
+        self._num_received += 1
+
+    def test_unload(self):
+        # The nodelet runs an infinite loop that can only be broken by calling `requestStop()` on the nodelet.
+        # Test this by unloading the nodelet, which should automatically call `requestStop()`.
+        # The nodelet publishes one message on /ready topic before entering the loop and one message after it is broken.
+        # However, if the nodelet is killed instead of unloaded, the `ros::shutdown()` call that happens during the kill
+        # will make it impossible to send the second message. So the kill tests just tests whether the node has stopped
+        # responding.
+
+        if self._stop_method == "unload":
+            unload_srv = rospy.names.ns_join(self._manager_name, "unload_nodelet")
+            unload = rospy.ServiceProxy(unload_srv, NodeletUnload)
+            unload.wait_for_service()
+
+        self._last_ready_msg = None
+        self._num_received = 0
+
+        sub = rospy.Subscriber("ready", String, self.ready_cb, queue_size=2)
+
+        while not rospy.is_shutdown() and self._num_received < 1:
+            rospy.logdebug("Waiting for ready message")
+            sleep(0.1)
+
+        self.assertGreaterEqual(self._num_received, 1)
+        node_name = self._last_ready_msg.data
+
+        # Give the bonds time to start. If we let one bond end to send exactly one heartbeat,
+        # we run into https://github.com/ros/bond_core/pull/93 which puts the other end into
+        # infinite wait. We want to make sure at least two heartbeats are sent. Each end sends
+        # one message per second to the bond topic, so we expect to see at least 4 messages.
+        while self._wait_for_bond and not rospy.is_shutdown() and self._num_bond_msgs < 4:
+            rospy.loginfo("Waiting for bonds. Have %i messages so far." % (self._num_bond_msgs,))
+            sleep(0.1)
+
+        if self._stop_method == "unload":
+            req = NodeletUnloadRequest()
+            req.name = node_name
+            res = unload.call(req)
+            self.assertTrue(res.success)
+
+            while not rospy.is_shutdown() and self._num_received < 2:
+                rospy.loginfo("Waiting for interrupted message")
+                sleep(0.1)
+
+            self.assertEqual(self._num_received, 2)
+            self.assertEqual(self._last_ready_msg.data, "interrupted")
+
+        elif self._stop_method.startswith("kill"):
+            name_to_kill = node_name if self._stop_method == "kill" else self._manager_name
+            success, fail = rosnode.kill_nodes((name_to_kill,))
+            self.assertEqual(success, [name_to_kill])
+            self.assertEqual(fail, [])
+
+            while rosnode.rosnode_ping(node_name, max_count=1):
+                sleep(0.1)
+            self.assertFalse(rosnode.rosnode_ping(node_name, max_count=1))
+
+
+if __name__ == '__main__':
+    rospy.init_node('test_request_stop')
+    rostest.rosrun('test_request_stop', 'test_request_stop', TestRequestStop)
diff --git a/test_nodelet/test/test_request_stop_unload.test b/test_nodelet/test/test_request_stop_unload.test
@@ -0,0 +1,12 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="manager"  args="manager" output="screen"/>
+    <node pkg="nodelet" type="nodelet" name="nodelet" args="load test_nodelet/RequestStop manager">
+        <param name="manager_name" value="manager" />
+        <param name="wait_for_bond" value="true" />
+    </node>
+
+    <test test-name="request_stop_test" pkg="test_nodelet" type="test_request_stop.py" time-limit="20">
+        <param name="manager_name" value="manager" />
+        <param name="wait_for_bond" value="true" />
+    </test>
+</launch>
diff --git a/test_nodelet/test/test_request_stop_unload_kill_manager.test b/test_nodelet/test/test_request_stop_unload_kill_manager.test
@@ -0,0 +1,14 @@
+<launch>
+<!--    <node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen" launch-prefix="xterm -e gdb -ex run &#45;&#45;args"/>-->
+    <node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen"/>
+    <node pkg="nodelet" type="nodelet" name="nodelet" args="load test_nodelet/RequestStop manager">
+        <param name="manager_name" value="manager" />
+        <param name="wait_for_bond" value="true" />
+    </node>
+
+    <test test-name="request_stop_test" pkg="test_nodelet" type="test_request_stop.py" time-limit="20">
+        <param name="stop_method" value="kill_manager" />
+        <param name="manager_name" value="manager" />
+        <param name="wait_for_bond" value="true" />
+    </test>
+</launch>
diff --git a/test_nodelet/test/test_request_stop_unload_standalone.test b/test_nodelet/test/test_request_stop_unload_standalone.test
@@ -0,0 +1,7 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet" args="standalone test_nodelet/RequestStop" />
+
+    <test test-name="request_stop_test" pkg="test_nodelet" type="test_request_stop.py" time-limit="20">
+        <param name="stop_method" value="kill" />
+    </test>
+</launch>
diff --git a/test_nodelet/test_nodelet.xml b/test_nodelet/test_nodelet.xml
@@ -19,4 +19,9 @@
       A node that creates public and private nodehandles.
     </description>
   </class>
+  <class name="test_nodelet/RequestStop" type="test_nodelet::RequestStop" base_class_type="nodelet::Nodelet">
+    <description>
+      A node that indefinitely waits until requested to stop.
+    </description>
+  </class>
 </library>