From 18d5a0f4f3aea9a782dabd1f31684862a5f075ea Mon Sep 17 00:00:00 2001
From: a1012112796 <1012112796@qq.com>
Date: Tue, 7 Sep 2021 20:45:14 +0800
Subject: [PATCH] =?UTF-8?q?=E7=A7=BB=E9=99=A4=E4=B8=B2=E5=8F=A3=E5=BC=82?=
 =?UTF-8?q?=E6=AD=A5=E6=8E=A5=E6=94=B6=E9=80=BB=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: a1012112796 <1012112796@qq.com>
---
 .../include/test_topic/WzSerialportPlus.h     |  2 ++
 .../include/test_topic/minPublisher.h         |  2 --
 src/test_topic/src/WzSerialportPlus.cpp       | 20 ++++++++---
 src/test_topic/src/minPublisher.cpp           | 35 +++++++++----------
 src/test_topic/src/minSubscriber.cpp          |  4 +--
 5 files changed, 36 insertions(+), 27 deletions(-)

diff --git a/src/test_topic/include/test_topic/WzSerialportPlus.h b/src/test_topic/include/test_topic/WzSerialportPlus.h
index 5b49e16..8ef2519 100755
--- a/src/test_topic/include/test_topic/WzSerialportPlus.h
+++ b/src/test_topic/include/test_topic/WzSerialportPlus.h
@@ -51,6 +51,8 @@ public:
     int send(const char* data,int length);
 
     void setReceiveCalback(ReceiveCallback receiveCallback);
+    int read_data(char* receive_buf, size_t receive_buf_size);
+    void sync_read(void);
 
 protected:
     virtual void onReceive(char* data,int length);
diff --git a/src/test_topic/include/test_topic/minPublisher.h b/src/test_topic/include/test_topic/minPublisher.h
index e9a6e8a..b2b61b4 100644
--- a/src/test_topic/include/test_topic/minPublisher.h
+++ b/src/test_topic/include/test_topic/minPublisher.h
@@ -3,7 +3,6 @@
 
 #include <ros/ros.h>
 #include <std_msgs/String.h>
-#include <queue>
 #include "test_topic/WzSerialportPlus.h"
 
 using MsgType = std_msgs::String;
@@ -14,7 +13,6 @@ public:
     void run(const char *device);
 private:
     ros::Publisher _sender;
-    std::queue<MsgType> _queue;
     WzSerialportPlus _serial;
 };
 
diff --git a/src/test_topic/src/WzSerialportPlus.cpp b/src/test_topic/src/WzSerialportPlus.cpp
index 0570dec..2653b38 100644
--- a/src/test_topic/src/WzSerialportPlus.cpp
+++ b/src/test_topic/src/WzSerialportPlus.cpp
@@ -195,10 +195,25 @@ bool WzSerialportPlus::open()
 
     receivable = true;
 
+    printf("[WzSerialportPlus::open()]: open success.\n");
+    return true;
+}
+
+int WzSerialportPlus::read_data(char* receive_buf, size_t receive_buf_size)
+{
+    if (!receivable)
+    {
+        return -1;
+    }
+
+    return read(serialportFd, receive_buf, receive_buf_size);
+}
+
+void WzSerialportPlus::sync_read(void)
+{
     std::thread([&]{
         char* receiveData = new char[receiveMaxlength];
         int receivedLength = 0;
-        int selectResult = -1;
 
         while (receivable)
         {
@@ -218,9 +233,6 @@ bool WzSerialportPlus::open()
         delete[] receiveData;
         receiveData = nullptr; 
     }).detach();
-
-    printf("[WzSerialportPlus::open()]: open success.\n");
-    return true;
 }
 
 bool WzSerialportPlus::open(const std::string& name,
diff --git a/src/test_topic/src/minPublisher.cpp b/src/test_topic/src/minPublisher.cpp
index 34b5ee2..316ce44 100644
--- a/src/test_topic/src/minPublisher.cpp
+++ b/src/test_topic/src/minPublisher.cpp
@@ -12,29 +12,26 @@ MinPublisher::~MinPublisher()
 
 void MinPublisher::run(const char *device)
 {
-    _serial.setReceiveCalback([this](char* str,int length){
-        MsgType msg;
-        std::stringstream ss;
-        ss << str;
-
-        msg.data = ss.str();
-
-        this->_queue.push(msg);
-    });
-
     _serial.open(device, 115200, 1, 8, 'N');
+    char *read_buf = new char[500];
 
     while (ros::ok())
     {
-        while(_queue.size() > 0)
-        {
-            MsgType msg = _queue.front();
-            ROS_INFO("send: '%s'", msg.data.c_str());
-            _sender.publish(msg);
-            _queue.pop();
-        }
+        std_msgs::String msg;
+        std::stringstream ss;
 
-        usleep(50);
+        memset(read_buf, 0, 500);
+
+        int len = _serial.read_data(read_buf, 500);
+        if (len <= 0)
+        {
+            continue;
+        }
+        ss << read_buf;
+        msg.data = ss.str();
+
+        ROS_INFO("send: '%s'", msg.data.c_str());
+        _sender.publish(msg);
     }
 }
 
@@ -43,7 +40,7 @@ int main(int argc, char *argv[])
     ros::init(argc, argv, "test_pub_node");
     MinPublisher nh;
 
-    nh.run("/dev/ttyUSB1");
+    nh.run("/dev/ttyUSB0");
 
     return 0;
 }
diff --git a/src/test_topic/src/minSubscriber.cpp b/src/test_topic/src/minSubscriber.cpp
index ee1d15e..d42a079 100644
--- a/src/test_topic/src/minSubscriber.cpp
+++ b/src/test_topic/src/minSubscriber.cpp
@@ -2,7 +2,7 @@
 
 MinSubscriber::MinSubscriber()
 {
-    _sub = this->subscribe<MsgType>("test_topic_2", 200, boost::bind(&MinSubscriber::callback, this, _1));
+    _sub = this->subscribe<MsgType>("test_topic", 200, boost::bind(&MinSubscriber::callback, this, _1));
 }
 
 MinSubscriber::~MinSubscriber()
@@ -27,7 +27,7 @@ int main(int argc, char *argv[])
     ros::init(argc, argv, "test_sub_node");
     MinSubscriber nh;
 
-    nh.run("/dev/ttyUSB0");
+    nh.run("/dev/ttyUSB1");
 
     return 0;
 }