diff --git a/src/test_topic/include/test_topic/minHandler.h b/src/test_topic/include/test_topic/minHandler.h index 84a05bc..3d11cca 100644 --- a/src/test_topic/include/test_topic/minHandler.h +++ b/src/test_topic/include/test_topic/minHandler.h @@ -1,28 +1,147 @@ #ifndef __MIN_HANDLER_H__ #define __MIN_HANDLER_H__ +/** + * @file minHandler.h + * + * @brief ros 最小节点抽象模型, 使用方法参见 `testMinHandler` + * + * @date 2021-09-14 + * + * @version 0.1.0-dev + * + */ + #include #include #include #include #include +namespace xyfc_std +{ + using MsgType = std_msgs::String; using ThreadFuncType = boost::function; +/** + * @brief ros 节点最小抽象 + * + * 提供ros nod 初始化的基本逻辑, 以便于快捷地创建节点及后期移植 ros2 + * + * Usage: + * + * ```CXX + * using namespace xyfc_std; + * + * class MyNode : public MinHandler + * { + * public: + * MyNode(int argc, char **argv, char *node_name): + * { + * //..... + * } + * } + * + * + * int main(int argc, char **argv) + * { + * MyNode nh(argc, argv, "test_nde"); + * nh.run(4); + * } + * ``` + * + */ class MinHandler { public: + /** + * @brief 构造函数 + * + * @param argc main 函数参数 + * @param argv main 函数参数 + * @param node_name node 名称, 必须唯一, 推荐使用 ros param + */ MinHandler(int argc, char **argv, const char *node_name); virtual ~MinHandler(); + /** + * @brief 启动 ros 逻辑 + * + * @param thread_num 线程数 + * - thread_num <= 0, 不使用多线程 spin 逻辑, 此时 + * loop_handle() 函数有效。 + * - thread_num > 0 使用多线程 spin 逻辑, thread_num + * - 为 1时, 表示使用默认值 (cpu 核数) + */ void run(int thread_num); + /** + * @brief 主动停止当前 node节点 + * + */ void shutdown(void); - void init_shutdown_service(void); protected: - 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; + /** + * @brief 节点初始化后回调函数 + * + * 此时 `_node` 节点已完成初始化, + * 可以在此函数中初始化 topic publisher/ subscriber ... + * + * @return true 初始化成功 + * @return false 初始化失败, 程序将立即终止 + * + */ + virtual bool node_init(void); + /** + * @brief 周期回调函数 + * + * 仅当 tread_nums < 0 时, + * 会在 run() 中周期性执行 + * + */ + virtual void loop_handle(void); + /** + * @brief 添加线程 + * + * @param pth tread 函数`对象`, 无参数, 无返回值 + * 可以使用lambda 表达, 普通函数, boost::bind(...) + * + * + * @param thread_num 线程数, 默认为 1 + * + * @warning 为确保程序可以`优雅`的退出, 在线程函数中 + * 必须周期调用 ok() 函数, 当返回值为 false时 + * 应该立即退出当前线程例如: + * + * ```CXX + * void thread_entry(self *MinHandler) + * { + * // init ... + * while(self->ok()) + * { + * // do something ... + * } + * + * // exit + * } + * ``` + */ + void add_thread(ThreadFuncType &pth, int thread_num = 1); + /** + * @brief 当前 node 是否还在运行 + * + */ + bool ok(void); + /** + * @brief node handle, 用于创建 topic publisher/ subscriber ... + * + */ + ros::NodeHandle *_node; + /** + * @brief Get the node name object + * + * @return const char* _node_name + */ + const char *get_node_name(); + private: int _argc; char **_argv; @@ -32,4 +151,6 @@ private: ros::AsyncSpinner *_spinner; }; +} // namespace xyfc_std + #endif diff --git a/src/test_topic/src/minHandler.cpp b/src/test_topic/src/minHandler.cpp index 52f66fa..1ef33cf 100644 --- a/src/test_topic/src/minHandler.cpp +++ b/src/test_topic/src/minHandler.cpp @@ -1,5 +1,8 @@ #include "test_topic/minHandler.h" +namespace xyfc_std +{ + MinHandler::MinHandler(int argc, char **argv, const char *node_name) { _argc = argc; @@ -20,9 +23,9 @@ MinHandler::~MinHandler() { } -void MinHandler::node_init() +bool MinHandler::node_init() { - + return true; } void MinHandler::loop_handle() @@ -43,6 +46,11 @@ bool MinHandler::ok(void) return !_shutdown; } +const char *MinHandler::get_node_name() +{ + return _node_name; +} + void MinHandler::run(int thread_num) { if (_node != nullptr) @@ -53,7 +61,12 @@ void MinHandler::run(int thread_num) ROS_INFO("init '%s' node", _node_name); ros::init(_argc, _argv, _node_name); _node = new ros::NodeHandle; - node_init(); + + if (!node_init()) + { + free(_node); + return; + } if (thread_num <= 0) { @@ -110,3 +123,5 @@ void MinHandler::shutdown(void) ros::shutdown(); } + +} // namespace xyfc_std diff --git a/src/test_topic/src/testMinHandler.cpp b/src/test_topic/src/testMinHandler.cpp index ca9c6e9..2ce8679 100644 --- a/src/test_topic/src/testMinHandler.cpp +++ b/src/test_topic/src/testMinHandler.cpp @@ -1,5 +1,7 @@ #include "test_topic/minHandler.h" +using namespace xyfc_std; + class testMinHandler: public MinHandler { private: @@ -10,7 +12,7 @@ public: testMinHandler(int argc, char **argv); ~testMinHandler(); protected: - virtual void node_init(); + virtual bool node_init(); }; void testMinHandler::callback(const boost::shared_ptr &msg) @@ -32,7 +34,7 @@ testMinHandler::testMinHandler(int argc, char **argv) } -void testMinHandler::node_init() +bool testMinHandler::node_init() { _pub = _node->advertise("test_topic_2", 200); _sub = _node->subscribe("test_topic", 200, &testMinHandler::callback, this); @@ -46,6 +48,7 @@ void testMinHandler::node_init() printf("hello pass thread shutdown\n"); }; add_thread(fn); + return true; } testMinHandler::~testMinHandler()