三节点测试代码

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

src/test_topic/CMakeLists.txt

@@ -155,6 +155,9 @@ target_link_libraries(listener ${catkin_LIBRARIES})
 add_executable(time_tacker src/timerPublisher.cpp)
 target_link_libraries(time_tacker ${catkin_LIBRARIES})
 
+add_executable(test_node src/testMinHandler.cpp src/minHandler.cpp)
+target_link_libraries(test_node ${catkin_LIBRARIES})
+
 #############
 ## Install ##
 #############
@@ -195,6 +198,7 @@ install(FILES
   talker
   listener
   time_tacker
+  test_node
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 

src/test_topic/include/test_topic/minHandler.h

@@ -0,0 +1,24 @@
+#ifndef __MIN_HANDLER_H__
+#define __MIN_HANDLER_H__
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+using MsgType = std_msgs::String;
+
+class MinHandler {
+public:
+    MinHandler(int argc, char **argv, const char *node_name);
+    virtual ~MinHandler();
+    void run(int thread_num);
+protected:
+   virtual void node_init();
+   virtual void loop_handle();
+   ros::NodeHandle *_node;
+private:
+    int _argc;
+    char **_argv;
+    const char *_node_name;
+};
+
+#endif

src/test_topic/src/minHandler.cpp

@@ -0,0 +1,68 @@
+#include "test_topic/minHandler.h"
+
+MinHandler::MinHandler(int argc, char **argv, const char *node_name)
+{
+    _argc = argc;
+    _argv = argv;
+    _node_name = node_name;
+    _node = nullptr;
+
+    if (_node_name == nullptr)
+    {
+        ROS_ERROR("_node_name is request");
+        return;
+    }
+}
+
+MinHandler::~MinHandler()
+{
+    ROS_INFO("MinHandler::~MinHandler() called");
+    if (_node != nullptr)
+    {
+        free(_node);
+    }
+}
+
+void MinHandler::node_init()
+{
+
+}
+
+void MinHandler::loop_handle()
+{
+
+}
+
+void MinHandler::run(int thread_num)
+{
+    if (_node != nullptr)
+    {
+        return;
+    }
+
+    ROS_INFO("init '%s' node", _node_name);
+    ros::init(_argc, _argv, _node_name);
+    _node = new ros::NodeHandle;
+    node_init();
+
+    if (thread_num <= 0)
+    {
+        while (ros::ok())
+        {
+            ros::spinOnce();
+            loop_handle();
+        }
+    } 
+    else if (thread_num == 1)
+    {
+        ros::MultiThreadedSpinner s;
+        ros::spin(s);
+    }
+    else
+    {
+        ros::MultiThreadedSpinner s(thread_num);
+        ros::spin(s);
+    }
+
+    ROS_INFO("stoped '%s' node", _node_name);
+}

src/test_topic/src/minPublisher.cpp

@@ -43,7 +43,7 @@ int main(int argc, char *argv[])
     ros::init(argc, argv, "test_pub_node");
     MinPublisher nh;
 
-    nh.run("/dev/ttyUSB0");
+    nh.run("/dev/ttyUSB1");
 
     return 0;
 }

src/test_topic/src/minSubscriber.cpp

@@ -2,7 +2,7 @@
 
 MinSubscriber::MinSubscriber()
 {
-    _sub = this->subscribe<MsgType>("test_topic", 200, boost::bind(&MinSubscriber::callback, this, _1));
+    _sub = this->subscribe<MsgType>("test_topic_2", 200, boost::bind(&MinSubscriber::callback, this, _1));
 }
 
 MinSubscriber::~MinSubscriber()

src/test_topic/src/testMinHandler.cpp

@@ -0,0 +1,52 @@
+#include "test_topic/minHandler.h"
+
+class testMinHandler: public MinHandler
+{
+private:
+    ros::Publisher _pub;
+    ros::Subscriber _sub;
+    void callback(const boost::shared_ptr<MsgType const> &msg);
+public:
+    testMinHandler(int argc, char **argv);
+    ~testMinHandler();
+protected:
+    virtual void node_init();
+};
+
+void testMinHandler::callback(const boost::shared_ptr<MsgType const> &msg)
+{
+    std::stringstream ss;
+
+    ss << "handled data: " << msg->data;
+    ROS_INFO("handle :%s", msg->data.c_str());
+    sleep(2);
+
+    MsgType send;
+    send.data = ss.str();
+    _pub.publish(send);
+}
+
+testMinHandler::testMinHandler(int argc, char **argv)
+    :MinHandler(argc, argv, "test_handle")
+{
+
+}
+
+void testMinHandler::node_init()
+{
+    _pub = _node->advertise<MsgType>("test_topic_2", 200);
+    _sub = _node->subscribe<MsgType>("test_topic", 200, &testMinHandler::callback, this);
+}
+
+testMinHandler::~testMinHandler()
+{
+}
+
+int main(int argc, char *argv[])
+{
+    testMinHandler *nb = new testMinHandler(argc, argv);
+    nb->run(8);
+    free(nb);
+
+    return 0;
+}

src/test_topic/src/timerPublisher.cpp

@@ -32,7 +32,7 @@ void TimerPublisher::run()
         std_msgs::String msg;
 
         std::stringstream ss;
-        ss << "hello world " << _count++;
+        ss << "hello world " << _count++ << "\n";
         msg.data = ss.str();
 
         ROS_INFO("%s", msg.data.c_str());