在minPublisher中使用自定义数据类型

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

src/test_topic/CMakeLists.txt

@@ -148,7 +148,7 @@ include_directories( include ${catkin_INCLUDE_DIRS})
 #   ${catkin_LIBRARIES}
 # )
 
-add_executable(talker src/minPublisher.cpp src/WzSerialportPlus.cpp)
+add_executable(talker src/minPublisher.cpp src/WzSerialportPlus.cpp src/minHandler.cpp)
 target_link_libraries(talker ${catkin_LIBRARIES})
 
 add_executable(listener src/minSubscriber.cpp src/WzSerialportPlus.cpp src/minHandler.cpp)

src/test_topic/include/test_topic/minPublisher.h

@@ -4,16 +4,19 @@
 #include <ros/ros.h>
 #include <std_msgs/String.h>
 #include "test_topic/WzSerialportPlus.h"
+#include "test_topic/minHandler.h"
 
-using MsgType = std_msgs::String;
-class MinPublisher: ros::NodeHandle {
+using namespace xyfc_std;
+class MinPublisher: public MinHandler {
 public:
-    MinPublisher();
+    MinPublisher(int argc, char **argv);
     ~MinPublisher();
-    void run(const char *device);
+protected:
+    virtual bool node_init(void);
 private:
     ros::Publisher _sender;
     WzSerialportPlus _serial;
+    void handle(void);
 };
 
 #endif

src/test_topic/launch/test1.launch

@@ -1,6 +1,10 @@
 <launch>
-    <arg name="port" default="/dev/ttyUSB0" />
-    <node pkg="test_topic" type="listener" name="serial_listen">
-      <param name="port" value="$(arg port)" />
+    <arg name="port_in" default="/dev/ttyUSB0" />
+    <arg name="port_out" default="/dev/ttyUSB1" />
+    <node pkg="test_topic" type="talker" name="serial_talker">
+      <param name="port" value="$(arg port_in)" />
+    </node>
+    <node pkg="test_topic" type="listener" name="serial_listen" output="screen">
+      <param name="port" value="$(arg port_out)" />
     </node>
   </launch>

src/test_topic/src/minPublisher.cpp

@@ -1,23 +1,44 @@
 #include "test_topic/minPublisher.h"
 
-MinPublisher::MinPublisher()
+MinPublisher::MinPublisher(int argc, char **argv):
+    MinHandler(argc, argv, "test_pub")
 {
-    _sender = this->advertise<MsgType>("test_topic", 200);
+}
+
+bool MinPublisher::node_init(void)
+{
+    std::string port, topic;
+    int baud = 115200;
+    ros::param::param<std::string>("~port", port, "/dev/ttyUSB0");
+    ros::param::param<int>("~baud", baud, 115200);
+    ros::param::param<std::string>("~topic", topic, "test_topic");
+
+    ROS_INFO("will send '%s' tiopic", topic.c_str());
+    _sender = _node->advertise<MsgType>("test_topic", 200);
+
+    if (!_serial.open(port, baud, 1, 8, 'N'))
+    {
+        ROS_ERROR("open serial '%s' failed!", port.c_str());
+        return false;
+    }
+
+    ThreadFuncType thd = boost::bind(&MinPublisher::handle, this);
+    add_thread(thd);
+
+    return true;
 }
 
 MinPublisher::~MinPublisher()
 {
-    _serial.close();
 }
 
-void MinPublisher::run(const char *device)
+void MinPublisher::handle()
 {
-    _serial.open(device, 115200, 1, 8, 'N');
     char *read_buf = new char[500];
 
-    while (ros::ok())
+    while (ok())
     {
-        std_msgs::String msg;
+        MsgType msg;
         std::stringstream ss;
 
         memset(read_buf, 0, 500);
@@ -27,20 +48,27 @@ void MinPublisher::run(const char *device)
         {
             continue;
         }
-        ss << read_buf;
-        msg.data = ss.str();
 
-        ROS_INFO("send: '%s'", msg.data.c_str());
+        for (int i =0; i < len; i++)
+        {
+            msg.data.push_back(read_buf[i]);
+        }
+        msg.length = len;
+        msg.tick = ros::Time::now();
+
+        ROS_INFO("send: '%s'", &msg.data[0]);
         _sender.publish(msg);
     }
+
+    printf("MinPublisher: exit handle thread\n");
+    _serial.close();
 }
 
 int main(int argc, char *argv[])
 {
-    ros::init(argc, argv, "test_pub_node");
-    MinPublisher nh;
-
-    nh.run("/dev/ttyUSB0");
+    MinPublisher *nh = new MinPublisher(argc, argv);
+    nh->run(4);
+    free(nh);
 
     return 0;
 }