diff --git a/src/test_topic/CMakeLists.txt b/src/test_topic/CMakeLists.txt index effb208..1153c90 100644 --- a/src/test_topic/CMakeLists.txt +++ b/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 diff --git a/src/test_topic/include/test_topic/minHandler.h b/src/test_topic/include/test_topic/minHandler.h index 25bb585..84a05bc 100644 --- a/src/test_topic/include/test_topic/minHandler.h +++ b/src/test_topic/include/test_topic/minHandler.h @@ -3,22 +3,33 @@ #include #include +#include +#include +#include using MsgType = std_msgs::String; +using ThreadFuncType = boost::function; 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 _threads; + bool _shutdown; + ros::AsyncSpinner *_spinner; }; #endif diff --git a/src/test_topic/package.xml b/src/test_topic/package.xml index 15f067c..63a5dbe 100644 --- a/src/test_topic/package.xml +++ b/src/test_topic/package.xml @@ -51,10 +51,12 @@ catkin roscpp std_msgs + std_srvs roscpp std_msgs roscpp std_msgs + std_srvs diff --git a/src/test_topic/src/minHandler.cpp b/src/test_topic/src/minHandler.cpp index 6003cd9..52f66fa 100644 --- a/src/test_topic/src/minHandler.cpp +++ b/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(); } diff --git a/src/test_topic/src/testMinHandler.cpp b/src/test_topic/src/testMinHandler.cpp index 6d8f349..ca9c6e9 100644 --- a/src/test_topic/src/testMinHandler.cpp +++ b/src/test_topic/src/testMinHandler.cpp @@ -19,7 +19,7 @@ void testMinHandler::callback(const boost::shared_ptr &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("test_topic_2", 200); _sub = _node->subscribe("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()