多线程优化

Signed-off-by: a1012112796 <1012112796@qq.com>

src/test_topic/CMakeLists.txt

@@ -10,6 +10,7 @@ project(test_topic)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
+  std_srvs
 )
 
 ## System dependencies are found with CMake's conventions

src/test_topic/include/test_topic/minHandler.h

@@ -3,22 +3,33 @@
 
 #include <ros/ros.h>
 #include <std_msgs/String.h>
+#include <std_srvs/Empty.h>
+#include <vector>
+#include <thread>
 
 using MsgType = std_msgs::String;
+using ThreadFuncType = boost::function<void (void)>;
 
 class MinHandler {
 public:
     MinHandler(int argc, char **argv, const char *node_name);
     virtual ~MinHandler();
     void run(int thread_num);
+    void shutdown(void);
+    void init_shutdown_service(void);
 protected:
-   virtual void node_init();
-   virtual void loop_handle();
+   virtual void node_init(void);
+   virtual void loop_handle(void);
+   void add_thread(ThreadFuncType &pth, int thread_num = 1);
+   bool ok(void);
    ros::NodeHandle *_node;
 private:
     int _argc;
     char **_argv;
     const char *_node_name;
+    std::vector<std::thread> _threads;
+    bool _shutdown;
+    ros::AsyncSpinner *_spinner;
 };
 
 #endif

src/test_topic/package.xml

@@ -51,10 +51,12 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/test_topic/src/minHandler.cpp

@@ -6,21 +6,18 @@ MinHandler::MinHandler(int argc, char **argv, const char *node_name)
     _argv = argv;
     _node_name = node_name;
     _node = nullptr;
+    _shutdown = false;
+    _spinner = nullptr;
 
     if (_node_name == nullptr)
     {
-        ROS_ERROR("_node_name is request");
+        printf("_node_name is request\n");
         return;
     }
 }
 
 MinHandler::~MinHandler()
 {
-    ROS_INFO("MinHandler::~MinHandler() called");
-    if (_node != nullptr)
-    {
-        free(_node);
-    }
 }
 
 void MinHandler::node_init()
@@ -33,6 +30,19 @@ void MinHandler::loop_handle()
 
 }
 
+void MinHandler::add_thread(ThreadFuncType &pth, int thread_num)
+{
+    while (thread_num-- > 0)
+    {
+        _threads.push_back(std::thread(pth));
+    }
+}
+
+bool MinHandler::ok(void)
+{
+    return !_shutdown;
+}
+
 void MinHandler::run(int thread_num)
 {
     if (_node != nullptr)
@@ -52,17 +62,51 @@ void MinHandler::run(int thread_num)
             ros::spinOnce();
             loop_handle();
         }
-    } 
+    }
     else if (thread_num == 1)
     {
-        ros::MultiThreadedSpinner s;
-        ros::spin(s);
+        ros::spin();
     }
     else
     {
-        ros::MultiThreadedSpinner s(thread_num);
-        ros::spin(s);
+        ros::AsyncSpinner spinner(thread_num);
+        spinner.start();
+        ros::waitForShutdown();
     }
 
-    ROS_INFO("stoped '%s' node", _node_name);
+    printf("stopping '%s' node ...\n", _node_name);
+
+    _shutdown = true;
+
+    if (_node != nullptr)
+    {
+        free(_node);
+    }
+
+    for (int i = 0; i < _threads.size(); i++)
+    {
+        _threads[i].join();
+    }
+}
+
+void MinHandler::shutdown(void)
+{
+    printf("[MinHandler::shutdown] try shutdown '%s' node\n", _node_name);
+    if (_shutdown)
+    {
+        return;
+    }
+
+    if (_spinner != nullptr)
+    {
+        _spinner->stop();
+        return;
+    }
+
+    if (_node == nullptr)
+    {
+        return;
+    }
+
+    ros::shutdown();
 }

src/test_topic/src/testMinHandler.cpp

@@ -19,7 +19,7 @@ void testMinHandler::callback(const boost::shared_ptr<MsgType const> &msg)
 
     ss << "handled data: " << msg->data;
     ROS_INFO("handle :%s", msg->data.c_str());
-    sleep(2);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
 
     MsgType send;
     send.data = ss.str();
@@ -36,6 +36,16 @@ void testMinHandler::node_init()
 {
     _pub = _node->advertise<MsgType>("test_topic_2", 200);
     _sub = _node->subscribe<MsgType>("test_topic", 200, &testMinHandler::callback, this);
+    ThreadFuncType fn = [this](){
+        while(this->ok())
+        {
+            ROS_INFO("hello pass thread");
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        printf("hello pass thread shutdown\n");
+    };
+    add_thread(fn);
 }
 
 testMinHandler::~testMinHandler()