Added functions Nodelet::ok() and Nodelet::requestStop()

nodelet/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.1)
 project(nodelet)
 
 ## Find catkin dependencies
@@ -74,3 +74,4 @@ install(TARGETS nodeletlib
 install(TARGETS nodelet
         DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+

nodelet/include/nodelet/nodelet.h

@@ -234,6 +234,20 @@ class NODELETLIB_DECL Nodelet
             ros::CallbackQueueInterface* st_queue = NULL,
             ros::CallbackQueueInterface* mt_queue = NULL);
 
+  /**\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().
+   * \return Status of the nodelet.
+   * \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();
+
   virtual ~Nodelet();
 };
 

nodelet/src/loader.cpp

@@ -85,6 +85,28 @@ class LoaderROS
     bond_spinner_.start();
   }
 
+  ~LoaderROS()
+  {
+    // Clean up the bond map with the mutex locked; if we left that just on the destructor,
+    // the bond map would be destroyed unlocked allowing async callbacks to access it
+    // meanwhile, which could lead to memory corruption and segfaults.
+
+    boost::mutex::scoped_lock lock(lock_);
+    for (auto nameAndBond : bond_map_)
+    {
+      const auto& name = nameAndBond.first;
+      auto bond = nameAndBond.second;
+
+      // Set the broken callback to just unload the nodelet and do not touch the bond map.
+      // It is being cleared explicitly in the next lines. We do not want to erase elements
+      // from it during its own .clear() call. The NoLock variant can be called because we
+      // already have locked the mutex above.
+      bond->setBrokenCallback(boost::bind(&LoaderROS::unloadNoLock, this, name));
+    }
+
+    bond_map_.clear();
+  }
+
 private:
   bool serviceLoad(nodelet::NodeletLoad::Request &req,
                    nodelet::NodeletLoad::Response &res)
@@ -95,7 +117,7 @@ class LoaderROS
     M_string remappings;
     if (req.remap_source_args.size() != req.remap_target_args.size())
     {
-      ROS_ERROR("Bad remapppings provided, target and source of different length");
+      ROS_ERROR("Bad remappings provided, target and source of different length");
     }
     else
     {
@@ -114,7 +136,7 @@ class LoaderROS
       bond::Bond* bond = new bond::Bond(nh_.getNamespace() + "/bond", req.bond_id);
       bond_map_.insert(req.name, bond);
       bond->setCallbackQueue(&bond_callback_queue_);
-      bond->setBrokenCallback(boost::bind(&LoaderROS::unload, this, req.name));
+      bond->setBrokenCallback(boost::bind(&LoaderROS::unloadAndEraseBond, this, req.name));
       bond->start();
     }
     return res.success;
@@ -123,25 +145,35 @@ class LoaderROS
   bool serviceUnload(nodelet::NodeletUnload::Request &req,
                      nodelet::NodeletUnload::Response &res)
   {
-    res.success = unload(req.name);
+    res.success = unloadAndEraseBond(req.name);
     return res.success;
   }
 
-  bool unload(const std::string& name)
+  bool unloadNoLock(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;
     }
 
+    return success;
+  }
+
+  bool unloadAndEraseBond(const std::string& name)
+  {
+    boost::mutex::scoped_lock lock(lock_);
+
+    bool success = unloadNoLock(name);
+    if (!success)
+      return success;
+
     // 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
+        // disable callback for broken bond, as we are breaking it intentionally now
         it->second->setBrokenCallback(boost::function<void(void)>());
         // erase (and break) bond
         bond_map_.erase(name);
@@ -194,22 +226,23 @@ struct ManagedNodelet : boost::noncopyable
 
   ~ManagedNodelet()
   {
+    nodelet->requestStop();
     callback_manager->removeQueue(st_queue);
     callback_manager->removeQueue(mt_queue);
   }
 };
 
 struct Loader::Impl
 {
-  boost::shared_ptr<LoaderROS> services_;
-
   boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)> create_instance_;
   boost::function<void ()> refresh_classes_;
   boost::shared_ptr<detail::CallbackQueueManager> callback_manager_; // Must outlive nodelets_
 
   typedef boost::ptr_map<std::string, ManagedNodelet> M_stringToNodelet;
   M_stringToNodelet nodelets_; ///<! A map of name to currently constructed nodelets
 
+  boost::shared_ptr<LoaderROS> services_;  // Must be destroyed before nodelets_
+
   Impl()
   {
     // Under normal circumstances, we use pluginlib to load any registered nodelet

nodelet/src/nodelet_class.cpp

@@ -45,6 +45,11 @@ Nodelet::Nodelet ()
 
 Nodelet::~Nodelet()
 {
+  // 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
@@ -134,4 +139,14 @@ void Nodelet::init(const std::string& name, const M_string& remapping_args, cons
   this->onInit ();
 }
 
+bool Nodelet::ok() const
+{
+  return inited_;
+}
+
+void Nodelet::requestStop()
+{
+  inited_ = false;
+}
+
 } // namespace nodelet

test_nodelet/CMakeLists.txt

@@ -7,13 +7,17 @@ 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}
   )
 
+  # find pthread and provide it as Threads::Threads imported target
+  set(THREADS_PREFER_PTHREAD_FLAG ON)
+  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}
   )
@@ -58,4 +62,11 @@ if(CATKIN_ENABLE_TESTING)
 
   add_executable(create_instance_cb_error src/create_instance_cb_error.cpp)
   target_link_libraries(create_instance_cb_error ${catkin_LIBRARIES})
+
+  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()

test_nodelet/package.xml

@@ -21,7 +21,10 @@
   <depend>rostest</depend>
   <depend>std_msgs</depend>
 
+  <test_depend>bond</test_depend>
   <test_depend>rosbash</test_depend>
+  <test_depend>rosnode</test_depend>
+  <test_depend>rospy</test_depend>
 
   <export>
     <nodelet plugin="${prefix}/test_nodelet.xml"/>

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)
+}