我的AI之路(29)--使用Boost Asio和Rapidjson实现机器人与机器人后端系统之间的自定义协议的通讯

       如果机器人与后端系统或其他系统之间根据实际情况需要使用自定义协议通讯，例如第三方系统使用的是自定义协议或者传输大文件之类的，那么多半需要回到使用TCP Socket编程，传输的数据既有二进制数据又有控制命令的文本数据时，直接使用C++的API进行网络编程和进行json格式数据的构建和解析的话需要费不少劲进行编码和调试，如果使用某种框架来进行开发就省事不少，Boost是使用较多的C++框架，我使用其中的Asio网络编程的框架，使用Thread来做线程编程，使用Chrono提供时间的实现，使用uuid来生成传输消息数据的唯一ID，另外使用到算法库algorithm来操作字符串；另外使用RapidJSON来做json数据的解析和构建。

    (1)使用Boost

要使用Boost的版本需要跟操作系统默认安装的boost库文件版本保持一致，不然可能会导致严重问题(程序可以编译过但是执行时会crash掉，后面再说)，Ubuntu 16.04 LTS默认安装的Boost是1.58.0的so，所以需要下载1.58.0的源码，在https://www.boost.org/users/download/页面里点击OLD BOOST RELEASES下面的version history 进入页面https://www.boost.org/users/history/，点击Version 1.58.0 下面的Download去sourceforge.net网站页面https://sourceforge.net/projects/boost/files/boost/1.58.0/下载源码包，加压后进行编译，按照https://www.boost.org/doc/libs/1_58_0/doc/html/boost_asio/using.html中的提示执行：

bjam --with-system --with-thread --with-date_time --with-regex --with-serialization stage

(对于boost 1.70.0新版本，需要先执行./bootstrap生成bjam和b2，然后执行bz2 --with-system --with-thread --with-date_time --with-regex --with-serialization stage) 在stage目录下生成lib目录及编译生成使用ASIO所需的so文件。

通讯程序实现为一个ROS节点，包路径为catkin_ws/src/platform_robot_telecom，为了方便节点的部署而不用在新环境中又编译安装boost，在此路径下mkdir -p include/boost lib/boost，下面需要把boost的头文件拷贝到包下面的include/boost目录里，把编译生成在stage/lib下的so文件拷贝到包下的lib/boost目录下:

      mkdir -p ~/catkin_ws/src/platform_robot_telecom/lib/boost
      cd ~/boost_1_58_0/stage/lib
      cp -d * ~/catkin_ws/src/platform_robot_telecom/lib/boost/

在CMakeLists.txt中增加相关头文件的include和库文件的link路径:

 include_directories(
 include
 ${catkin_INCLUDE_DIRS}
)

set(BOOST_LINK_LIBRARIES
  boost_context
  boost_system
  boost_thread
  boost_date_time
  boost_regex
  boost_serialization
  pthread
)
target_link_libraries(${PROJECT_NAME}_node
   ${BOOST_LINK_LIBRARIES}
   ${catkin_LIBRARIES}
 )

在源码中包含所用到的相关头文件:

#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/algorithm/string.hpp>

然后是编码实现网络通讯，这里只列出主要步骤API，为保证TCP真正工作在双向双工状态，对数据的收发分别使用了不同的线程来负责数据的发送和接收，待发送数据和接收到的待处理数据都存放入deque类型消息队列，处理接收到的数据也由专门的线程负责，再加上ROS Message Topic/Service，实现数据的双向传输与机器人的控制解耦合；数据的发送和接收可以根据实际情况需要使用同步的boost::asio::read(...,) / boost::asio::write(socket,...)或者异步基于事件的socket.read_some(...)/socket.write_some(...)，同步传输适合传输长度可知且经常固定的数据并且并发连接少的情况，异步传输则适合传输长度经常不定的数据并且并发连接多要求吞吐量大的情况。

这里列出的代码基本可以体现出设计和实现思路供参考，对于涉及到商业秘密的代码省略了:

class PRTMsg

{

    ...

}

typedef std::deque<PRTMsg*> Send_Message_Queue;
typedef std::deque<PRTMsg*> Recv_Message_Queue;

io_service service;

class PRTClient {

   public:
    PRTClient(const std::string & username) : sock_(service), username_(username) 
    {

       ...

     }

   int connect(ip::tcp::endpoint ep) {

         sock_.connect(ep);

         sock_.set_option(boost::asio::ip::tcp::no_delay(true));
         sock_.set_option(boost::asio::socket_base::keep_alive(true));

    }

   void disconnect() 
        {
      sock_.close();
    }

   ...

void recv()
    {
          ...
          int result=0;
          try{
              while(!bStopRecvThread)
                  {    
                boost::this_thread::interruption_point();
                if(result=putDataToRecvQueue())break;
                boost::this_thread::interruption_point();
              }
           } catch(boost::system::system_error & err) {
               std::cout << "recv() terminated,error: "<<err.what() << std::endl;
           } catch(boost::thread_interrupted & err) {
               std::cout << "recv() interrupted! "<< std::endl;
               result=-99;
            }
              ...
           }

          void send(){
             int result=0;
             try{
         
                 ...
                while(!bStopSendThread)
                 {   
                     if(!sendMsgQueue.empty())
                     {  boost::this_thread::interruption_point();
                         PRTMsg* prtMsg = (sendMsgQueue.front());

                          //send data

                          

                          sendMsgQueue.pop_front();  //remove and destroy prtMsg
               
                           boost::this_thread::interruption_point();
                    
                    }else
                   {  
                          boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
                   }
                }

             } catch(boost::system::system_error & err) {

                 ...

              }

             

void recvHandler(){
        int result=0;
        try{
              ...

               while(!bStopRecvHandlerThread)
               {
                 if(!recvMsgQueue.empty())
                {
                   boost::this_thread::interruption_point();
                    
                   PRTMsg* prtMsg = (recvMsgQueue.front());
                   if((int)prtMsg->GetMsgType() ==1) {//response, don't need to pass it to CCM,just handle and drop it! 
                       //stop the corresponding send-timer and delete the PRTMsg data according to msgID!

                        ...
                      }else{    
                       if((int)prtMsg->GetBodyDataType()){  //binary data
                        handleBinData(prtMsg->GetMsgID(),prtMsg->GetBodySize(), prtMsg->GetBodyData());
                       }else{
                        handleTextData(prtMsg->GetMsgID(),prtMsg->GetBodySize(), prtMsg->GetBodyData());
                       }    
                    }
                    recvMsgQueue.pop_front();  //remove and destroy prtMsg
                    boost::this_thread::interruption_point();
                    
                }else{
                    boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
                }
                   }
           } catch(boost::system::system_error & err) {
               ...
            }
           catch(boost::thread_interrupted & err) {
               ...
            }
              ...
      }

  ...

 std::shared_ptr<boost::thread> send_thread; //发送数据的线程，从发送消息列队里获取消息拆包数据并write到socket里
 std::shared_ptr<boost::thread> recv_thread;  //接收数据的线程，从socket里读取数据并封装成消息放入接收消息的队列
 std::shared_ptr<boost::thread> recvHander_thread;  //处理接收数据的线程，从接收消息队列读取消息并拆包然后根据msg ID调用对应的ROS Service或者发布数据到对应的message Topic以与机器人下层的中控系统交互实现控制机器人

private:
    
    ip::tcp::socket sock_;

     ...

}

PRTClient* run_client(const std::string & client_name,std::string ip,int port) 
{
        PRTClient* client = new PRTClient(client_name);
        ip::tcp::endpoint ep( ip::address::from_string(ip), port);
        bool isThreadsCreated = false;
        try {
        std::cout << "connecting..."<< std::endl;
        client->connect(ep); 
        std::cout << "connected"<< std::endl;
        client->send_thread.reset(new boost::thread(boost::bind(&PRTClient::send,client)));
        client->recv_thread.reset(new boost::thread(boost::bind(&PRTClient::recv,client)));
        client->recvHander_thread.reset(new boost::thread(boost::bind(&PRTClient::recvHandler,client)));
        isThreadsCreated = true ;
        std::cout << "all threads created!"<< std::endl;
        
        } catch(boost::system::system_error & err) {
             std::cout << "run_client terminated,error: "<<err.what() <<", rerun again..."<< std::endl;
             client->disconnect();

          ...

         }

        

int main(int argc, char ** argv) {

  ros::init(argc, argv, "platform_robot_telecom");
  ros::NodeHandle node;
  ros::Rate rate(100.0);
  int port=9000;

  std::string ip="192.168.80.222";
  std::string user;
  std::string pwd;
  node.getParam("/platform_robot_telecom/ip",ip);
  node.getParam("/platform_robot_telecom/port",port);
  node.getParam("/platform_robot_telecom/user",user);
  node.getParam("/platform_robot_telecom/pwd",pwd);
   ...

  PRTClient* client = run_client(user,ip,port);

  ...

   

while(ros::ok()){
    if(client->isSendThreadExited() || client->isRecvThreadExited())  //socket error happened!
    { 
            client->disconnect();
            boost::this_thread::sleep_for(boost::chrono::milliseconds(100));

        if(client->isSendThreadStarted() && !client->isSendThreadExited())
        {
           client->stopSendThread();
           client->send_thread->interrupt();
        }
        if(client->isRecvThreadStarted() && !client->isRecvThreadExited())
         {
           client->stopRecvThread();
           client->recv_thread->interrupt();
         }
        if(client->isRecvHandlerThreadStarted() && !client->isRecvHandlerThreadExited())
        {
           client->stopRecvHandlerThread();
           client->recvHander_thread->interrupt();
        }
        
        while(client->isSendThreadStarted() && !client->isSendThreadExited()
            ||client->isRecvThreadStarted() && !client->isRecvThreadExited()
            ||client->isRecvHandlerThreadStarted() && !client->isRecvHandlerThreadExited())
             boost::this_thread::sleep_for(boost::chrono::seconds(1));
        delete client;
        boost::this_thread::sleep_for(boost::chrono::seconds(3));  //3+ seconds
        client =  run_client(user,ip,port);  //try to reconnect to CCP
        ...
    }else{
        ros::spinOnce();
        boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
    }
  }

  ...
 ros::shutdown();
}

可能出现的错误：

/usr/bin/ld: /tmp/cc0EorPk.o: undefined reference to symbol 'pthread_create@@GLIBC_2.2.5'
/lib/x86_64-linux-gnu/libpthread.so.0: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status

这是因为没有把libpthread.so加入到link路径中。

In function `boost::thread::do_try_join_until(boost::detail::mono_platform_timepoint const&)':
/home/fychen/catkin_ws/src/platform_robot_telecom/include/boost/thread/detail/thread.hpp:750: undefined reference to `boost::thread::do_try_join_until_noexcept(boost::detail::mono_platform_timepoint const&, bool&)'
collect2: error: ld returned 1 exit status

这个错误一般是因为你编译时没有把正确版本的boost的libthread.so库加入到link路径中。

/usr/bin/ld: warning: libboost_thread.so.1.58.0, needed by /opt/ros/kinetic/lib/libroscpp.so, may conflict with libboost_thread.so.1.70.0
/usr/bin/ld: warning: libboost_chrono.so.1.58.0, needed by /opt/ros/kinetic/lib/libroscpp.so, may conflict with libboost_chrono.so.1.70.0
/usr/bin/ld: warning: libboost_regex.so.1.58.0, needed by /opt/ros/kinetic/lib/librosconsole.so, may conflict with libboost_regex.so.1.70.0

这是因为程序编译时使用的是1.70.0版的boost，而roscpp使用的操作系统默认安装的1.58.0版本，这里只是警告不会编译不过，不过即使编译过了，因为版本不一致，会导致程序运行时roscpp崩溃，通过分析core dump(在CMakeLists.txt里注意加上-g参数编译)可知：

ulimit -c unlimited
./platform_robot_telecom
Segmentation fault (core dumped)

ThinkStation-P318:~/catkin_ws/devel/lib/platform_robot_telecom$ gdb ./platform_robot_telecom_node core
GNU gdb (Ubuntu 7.11.1-0ubuntu1~16.5) 7.11.1
Copyright (C) 2016 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.  Type "show copying"
and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
<http://www.gnu.org/software/gdb/documentation/>.
For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from ./platform_robot_telecom_node...done.

warning: core file may not match specified executable file.
[New LWP 21381]
[New LWP 21390]
[New LWP 21411]
[New LWP 21385]
[New LWP 21412]
[New LWP 21383]
[New LWP 21413]
[New LWP 21384]
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
Core was generated by `./platform_robot_telecom_node'.
Program terminated with signal SIGSEGV, Segmentation fault.
#0  0x00007fcd91d2adc3 in ros::CallbackQueue::callAvailable(ros::WallDuration)
    () from /opt/ros/kinetic/lib/libroscpp.so
[Current thread is 1 (Thread 0x7fcd925e8780 (LWP 21381))]
(gdb) bt
#0  0x00007fcd91d2adc3 in ros::CallbackQueue::callAvailable(ros::WallDuration)
    () from /opt/ros/kinetic/lib/libroscpp.so
#1  0x00007fcd91d6cef5 in ros::spinOnce() ()
   from /opt/ros/kinetic/lib/libroscpp.so
#2  0x0000000000492af5 in main (argc=1, argv=0x7fff77ee6d58)
    at /home/fychen/catkin_ws/src/platform_robot_telecom/src/platform_robot_telecom.cpp:1039
(gdb) quit

上面就是崩溃在roscpp的spinOnce()里，把程序编译链接使用的boost源码和库换成Ubuntu 16.04 LTS默认安装的1.58.0这个版本后这个SIGSEGV崩溃问题就消失了。

 

 

(2)使用RapidJSON

    RapidJSON是个性能很高的json数据构建和解析库，支持SAX和DOM，主页http://rapidjson.org/zh-cn/, 源码github地址：https://github.com/Tencent/rapidjson/，如果以前做过XML的开发，清楚SAX和DOM，非常容易上手，按照示例文档或者示例代码写json数据的解析和构建例子几下就能写好。而且它是只有头文件的C++库，只需把https://github.com/Tencent/rapidjson/tree/master/include/rapidjson里面的所有.h文件(包括子目录)全部拷贝到项目的include目录中即可使用，例如 cp -r rapidjson_master/include   ~/catkin_ws/src/platform_robot_telecom/include，然后在源码中包含相关头文件即可：

    #include "rapidjson/document.h"
    #include "rapidjson/writer.h"
    #include "rapidjson/stringbuffer.h"
    #include "rapidjson/prettywriter.h" 

  i)解析json数据:

   void handleTextData(std::string msgID,unsigned char bodyDataSize, char* bodyData)
      {
        std::cout << "[handleTextData]msgID:"<<msgID<<", bodyData: "<<bodyData<< std::endl;
         //1.parse the jason data to get the items: msgCode and data
         const char* json = bodyData;
         Document d;
         d.Parse(json);
         Value& v = d["msgCode"];
         const char* msgCode = "";
        if(v.IsString()) msgCode = d["msgCode"].GetString();
         std::cout << "[handleTextData] msgCode: "<<msgCode<< std::endl;
         //2.call business operation according to msgCode
         if(strcmp(msgCode,"powerOnOff")==0)  //
         {    
             Value& data = d["data"];  //json
            const char* strOnOff = data["onOff"].GetString();
            platform_robot_telecom::PowerControl pwrctrlMsg;
            pwrctrlMsg.msgCode = std::string(msgCode);
            pwrctrlMsg.onOff = std::string(strOnOff);
            Pub_PowerControl.publish(pwrctrlMsg);
         }

         ...

      }

     ii)构建数据：    

   void currentPosCallback(const geometry_msgs::Point32::ConstPtr& msg){//监听来自下层控制系统的传感数据
        
        float x = msg->x;
        float y = msg->y;
        float z= msg->z;
        Document doc;
        doc.SetObject();
        Document::AllocatorType & allocator = doc.GetAllocator();
        doc.AddMember("msgCode","robotCurrentPostion",allocator);
        Value data(kObjectType);
        data.AddMember("x",x,allocator);
        data.AddMember("y",y,allocator);
        data.AddMember("z",yaw,allocator);
        data.AddMember("Desc","当前位置",allocator);
        doc.AddMember("data",data,allocator);

        StringBuffer buffer;
        Writer<StringBuffer> writer(buffer);
        doc.Accept(writer);
        const char* jsonData = buffer.GetString();
        unsigned int dataSize = strlen(jsonData);
        putTextDataToSendQueue(dataSize,(char*)jsonData);    //存入发送数据消息队列
     }

出现下面这种IsObject() assertion fail错误一般都是因为json文件里有乱码或者json数据不完整导致格式解析出错了，这常见与在自定义协议按制定长度传输数据时，发送数据方因为代码bug构建json数据后指定的数据长度不是json数据的实际长度或者发送方与接收方使用了不一致的字符编码(注意rapidjson默认使用的utf-8字符集)：

ThinkStation-P318:~/catkin_ws/devel/lib/platform_robot_telecom$ gdb ./platform_robot_telecom_node  core
GNU gdb (Ubuntu 7.11.1-0ubuntu1~16.5) 7.11.1
Copyright (C) 2016 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.  Type "show copying"
and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
<http://www.gnu.org/software/gdb/documentation/>.
For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from ./platform_robot_telecom_node...done.

warning: core file may not match specified executable file.
[New LWP 9921]
[New LWP 9895]
[New LWP 9904]
[New LWP 9897]
[New LWP 9919]
[New LWP 9899]
[New LWP 9920]
[New LWP 9898]
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
Core was generated by `./platform_robot_telecom_node'.
Program terminated with signal SIGABRT, Aborted.
#0  0x00007f2f206d8428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
54    ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
[Current thread is 1 (Thread 0x7f2f18abc700 (LWP 9921))]
(gdb) bt
#0  0x00007f2f206d8428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007f2f206da02a in __GI_abort () at abort.c:89
#2  0x00007f2f206d0bd7 in __assert_fail_base (fmt=<optimized out>, assertion=assertion@entry=0x4c4ce7 "IsObject()", 
    file=file@entry=0x4c4968 "/home/fychen/catkin_ws/src/platform_robot_telecom/include/rapidjson/document.h", line=line@entry=1255, 
    function=function@entry=0x4c7700 <rapidjson::GenericMemberIterator<false, rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> > rapidjson::GenericValue<rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> >::FindMember<rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> >(rapidjson::GenericValue<rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> > const&)::__PRETTY_FUNCTION__> "rapidjson::GenericValue<Encoding, Allocator>::MemberIterator rapidjson::GenericValue<Encoding, Allocator>::FindMember(const rapidjson::GenericValue<Encoding, SourceAllocator>&) [with SourceAllocator ="...) at assert.c:92
#3  0x00007f2f206d0c82 in __GI___assert_fail (assertion=0x4c4ce7 "IsObject()", file=0x4c4968 "/home/fychen/catkin_ws/src/platform_robot_telecom/include/rapidjson/document.h", 
    line=1255, 
    function=0x4c7700 <rapidjson::GenericMemberIterator<false, rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> > rapidjson::GenericValue<rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> >::FindMember<rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> >(rapidjson::GenericValue<rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> > const&)::__PRETTY_FUNCTION__> "rapidjson::GenericValue<Encoding, Allocator>::MemberIterator rapidjson::GenericValue<Encoding, Allocator>::FindMember(const rapidjson::GenericValue<Encoding, SourceAllocator>&) [with SourceAllocator ="...) at assert.c:101
#4  0x00000000004a31fc in rapidjson::GenericValue<rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> >::FindMember<rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> > (this=0x7f2f18abbc90, name=...) at /home/fychen/catkin_ws/src/platform_robot_telecom/include/rapidjson/document.h:1255
#5  0x000000000049dc3e in rapidjson::GenericValue<rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> >::operator[]<rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> > (this=0x7f2f18abbc90, name=...) at /home/fychen/catkin_ws/src/platform_robot_telecom/include/rapidjson/document.h:1135
#6  0x0000000000493ca3 in rapidjson::GenericValue<rapidjson::UTF8<char>, rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> >::operator[]<char const> (this=0x7f2f18abbc90, 
    name=0x4c4329 "msgCode") at /home/fychen/catkin_ws/src/platform_robot_telecom/include/rapidjson/document.h:1119

例如，用Java开发的Server端发送含中文的字符串前一定要先: byte[] data = str.getBytes("UTF-8")，然后传输data时的size一定要使用data.length而不是str.length() !!! 否则接收端收到数据后用RapidJson解析时可能会报下上面的IsObject() assertion failed错误!!! 这是java网络编程新手容易犯的错误。