增加测量通讯模块,未测试

CMakeLists.txt

@@ -66,11 +66,40 @@ target_link_libraries(terminal
         ${PROTOBUF_LIBRARIES}
         /usr/local/lib/libglog.a
         /usr/local/lib/libgflags.a
-
-#        ${OpenCV_LIBS}
         ${GLOG_LIBRARIES}
-#        ${PCL_LIBRARIES}
-        ${PROTOBUF_LIBRARIES}
 
         )
 
+
+add_executable(locate_client
+        ./test/Locate_client.cpp
+        ./lidar_locate/Locate_communicator.cpp
+        ./error_code/error_code.cpp
+        ${message_src}
+        ${TOOL_SRC}
+        ${COMMUNICATION_SRC}
+        )
+target_link_libraries(locate_client
+        nnxx
+        nanomsg
+        ${PROTOBUF_LIBRARIES}
+        /usr/local/lib/libglog.a
+        /usr/local/lib/libgflags.a
+        )
+
+
+add_executable(locate_test
+        ./test/test_locate_sample.cpp
+        ./lidar_locate/Locate_communicator.cpp
+        ./error_code/error_code.cpp
+        ${message_src}
+        ${TOOL_SRC}
+        ${COMMUNICATION_SRC}
+        )
+target_link_libraries(locate_test
+        nnxx
+        nanomsg
+        ${PROTOBUF_LIBRARIES}
+        /usr/local/lib/libglog.a
+        /usr/local/lib/libgflags.a
+        )

communication/communication_socket_base.cpp

@@ -487,6 +487,7 @@ void Communication_socket_base::send_data_thread()
 				{
 					std::unique_lock<std::mutex> lk(m_mutex);
 					int send_size = m_socket.send(tp_msg->get_message_buf());
+
 					delete(tp_msg);
 					tp_msg = NULL;
 				}
@@ -581,7 +582,8 @@ Error_manager Communication_socket_base::encapsulate_msg(std::string message)
 //封装消息, 需要子类重载
 Error_manager Communication_socket_base::encapsulate_msg(Communication_message* p_msg)
 {
-	Communication_message* tp_msg = new Communication_message(*p_msg);
+	Communication_message* tp_msg = new Communication_message(p_msg->get_message_buf());
+
 	bool is_push = m_send_data_list.push(tp_msg);
 	if ( is_push == false )
 	{

error_code/error_code.h

@@ -55,6 +55,7 @@ enum Error_code
 
     NO_DATA                         = 0x00000010,//没有数据，传入参数容器内部没有数据时，
 	INVALID_MESSAGE					= 0x00000011, //无效的消息,
+    RESPONSE_TIMEOUT                = 0x00000012,
 
     POINTER_IS_NULL                 = 0x00000101,//空指针
     PARAMETER_ERROR                 = 0x00000102,//参数错误，传入参数不符合规范时，
@@ -122,84 +123,16 @@ enum Error_code
 
 
 
-     //PLC error code  ...
-    PLC_ERROR_BASE                  = 0x02010000,
-    PLC_UNKNOWN_ERROR,
-    PLC_EMPTY_TASK,
-    PLC_IP_PORT_ERROR,
-    PLC_SLAVE_ID_ERROR,
-    PLC_CONNECTION_FAILED,
-    PLC_READ_FAILED,
-    PLC_WRITE_FAILED,
-    PLC_NOT_ENOUGH_DATA_ERROR,
-
-
-
     //locate 定位模块,
 	LOCATER_ERROR_BASE                				= 0x03000000,
 
 	//LASER_MANAGER 定位管理模块
 	LOCATER_MANAGER_ERROR_BASE                		= 0x03010000,
-	LOCATER_MANAGER_READ_PROTOBUF_ERROR,				//定位管理模块，读取参数错误
-	LOCATER_MANAGER_STATUS_BUSY,						//定位管理模块，状态正忙
-	LOCATER_MANAGER_STATUS_ERROR,						//定位管理模块，状态错误
-	LOCATER_MANAGER_TASK_TYPE_ERROR,					//定位管理模块，任务类型错误
-	LOCATER_MANAGER_IS_NOT_READY,						//定位管理模块，不在准备状态
-	LOCATER_MANAGER_CLOUD_MAP_ERROR,					//定位管理模块，任务输入点云map的error
-	LOCATE_MANAGER_TASK_OVER_TIME,
-
-
-	//Locater.cpp error from 0x0301000-0x030100FF
-	LOCATER_TASK_INIT_CLOUD_EMPTY ,
-    LOCATER_TASK_ERROR,
-    LOCATER_TASK_INPUT_CLOUD_UNINIT,
-    LOCATER_INPUT_CLOUD_EMPTY,
-    LOCATER_YOLO_UNINIT,
-    LOCATER_POINTSIFT_UNINIT,
-    LOCATER_3DCNN_UNINIT,
-    LOCATER_INPUT_YOLO_CLOUD_EMPTY,
-    LOCATER_Y_OUT_RANGE_BY_PLC,
-    LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,
-    LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,
-    LOCATER_INPUT_CLOUD_UNINIT,
-
-
-    //point sift from 0x03010100-0x030101FF
-    LOCATER_SIFT_INIT_FAILED=0x03010100,
-    LOCATER_SIFT_INPUT_CLOUD_UNINIT,
-	LOCATER_SIFT_INPUT_CLOUD_EMPTY,
-	LOCATER_SIFT_GRID_ERROR,
-	LOCATER_SIFT_SELECT_ERROR,
-	LOCATER_SIFT_CLOUD_VERY_LITTLE,
-	LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
-	LOCATER_SIFT_PREDICT_FAILED,
-	LOCATER_SIFT_PREDICT_NO_WHEEL_POINT,
-	LOCATER_SIFT_PREDICT_NO_CAR_POINT,
-
-    LOCATER_SIFT_FILTE_OBS_FAILED,
-    LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,
-
-//    //yolo from 0x03010200-0x030102FF
-//        LOCATER_YOLO_DETECT_FAILED=0x03010200,
-//    LOCATER_YOLO_DETECT_NO_TARGET,
-//    LOCATER_YOLO_PARAMETER_INVALID,
-//    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
-
-    //3dcnn from 0x03010300-0x030103FF
-    LOCATER_3DCNN_INIT_FAILED=0x03010300,
-    LOCATER_3DCNN_INPUT_CLOUD_UNINIT,
-	LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
-	LOCATER_3DCNN_INPUT_CLOUD_MAP_ERROR,
-	LOCATER_3DCNN_PCA_OUT_ERROR,
-	LOCATER_3DCNN_EXTRACT_RECT_ERROR,
-	LOCATER_3DCNN_RECT_SIZE_ERROR,
-
-    LOCATER_3DCNN_PREDICT_FAILED,
-    LOCATER_3DCNN_VERIFY_RECT_FAILED_3,
-    LOCATER_3DCNN_VERIFY_RECT_FAILED_4,
-    LOCATER_3DCNN_KMEANS_FAILED,
-    LOCATER_3DCNN_IIU_FAILED,
-    LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,
+    LOCATER_MSG_TABLE_NOT_EXIST ,
+    LOCATER_MSG_RESPONSE_TYPE_ERROR,
+    LOCATER_MSG_RESPONSE_INFO_ERROR,
+    LOCATER_MSG_REQUEST_INVALID,
+    LOCATER_MSG_RESPONSE_HAS_NO_REQUEST,
 
     //System_manager error from 0x04010000-0x0401FFFF
     SYSTEM_READ_PARAMETER_ERROR=0x04010100,

lidar_locate/Locate_communicator.cpp

@@ -6,8 +6,6 @@
 //#include "locate_message.pb.h"
 
 
-Locate_communicator* Locate_communicator::mp_locate_communicator=NULL;
-
 Locate_communicator::Locate_communicator()
 {
 
@@ -16,67 +14,106 @@ Locate_communicator::~Locate_communicator()
 {
 
 }
-Error_manager Locate_communicator::locate_request(message::Measure_request_msg request,message::Measure_response_msg& result,unsigned int timeout)
+
+
+Error_manager Locate_communicator::encapsulate_msg(Communication_message* message)
+{
+
+    //记录请求
+    if(message->get_message_type()==Communication_message::eLocate_request_msg)
+    {
+        message::Measure_request_msg request;
+        m_response_table[request.command_id()]=message::Measure_response_msg();
+    }
+    else
+    {
+        return Error_manager(LOCATER_MSG_TABLE_NOT_EXIST,NEGLIGIBLE_ERROR,"message table is not exist");
+    }
+
+    //发送请求
+    return Communication_socket_base::encapsulate_msg(message);
+}
+
+Error_manager Locate_communicator::locate_request(message::Measure_request_msg& request,message::Measure_response_msg& result)
 {
     /*
      * 检查request合法性,以及模块状态
      */
+    if(request.msg_base().sender()!=message::eMain||request.msg_base().receiver()!=message::eMeasurer)
+        return Error_manager(LOCATER_MSG_REQUEST_INVALID,MINOR_ERROR,"measure request invalid");
 
-    /*std::string response_string;
-    Error_manager code=m_nnxx_client.request(request.SerializeAsString(),response_string,timeout);
+    //设置超时,若没有设置,默认3000
+    int timeout=request.msg_base().has_timeout_ms()?request.msg_base().timeout_ms():3000;
+    //向测量节点发送测量请求,并记录请求
 
-    if(code==SUCCESS)
-    {
-        //解析返回数据
-        message::Locate_response_msg response;
+    Error_manager code;
+    Communication_message message;
 
-        if(false==response.ParseFromString(response_string))
-        {
-            //解析response数据错误,
-//            return Error_manager(LOCATE_RESPONSE_PARSE_ERROR,MAJOR_ERROR,"response string parse failed");
-        }
-        else if(response.error_manager().error_code()==SUCCESS)
+    message::Base_msg base_msg;
+    base_msg.set_msg_type(message::eLocate_request_msg);
+    base_msg.set_sender(message::eMain);
+    base_msg.set_receiver(message::eMeasurer);
+    base_msg.set_timeout_ms(timeout);
+    message.reset(base_msg,request.SerializeAsString());
+
+    code=encapsulate_msg(&message);
+    if(code!=SUCCESS)
+        return code;
+    //循环查询请求是否被处理
+    auto start_time=std::chrono::system_clock::now();
+    double time=0;
+    do{
+        //在请求表中查询结果
+        message::Measure_response_msg response=m_response_table[request.command_id()];
+        //判断是否接收到回应,若回应信息被赋值则证明有回应
+        if(response.has_msg_base() && response.has_command_id())
         {
+            message::Base_msg response_base=response.msg_base();
+            //检查类型是否匹配
+            if(response_base.msg_type() != message::eLocate_response_msg)
+            {
+                return Error_manager(LOCATER_MSG_RESPONSE_TYPE_ERROR,MAJOR_ERROR,"measure response msg type error");
+            }
+            //检查基本信息是否匹配
+            if(response_base.sender()!=message::eMeasurer ||
+                response_base.receiver()!=message::eMain||
+                response.command_id()!=request.command_id())
+            {
+                return Error_manager(LOCATER_MSG_RESPONSE_INFO_ERROR,MAJOR_ERROR,"measure response msg info error");
+            }
             result=response;
             return SUCCESS;
+
         }
-        else
-        {
-            ///将response中的错误信息,转换成错误码,返回
-            return Error_manager(Error_code(response.error_manager().error_code()),MAJOR_ERROR,response.error_manager().error_description().c_str());
-        }
-    }
-    else if(code.get_error_level()==MINOR_ERROR)
-    {
-        //处理底层处理不了的错误
-    }
-    else if(code.get_error_level()==MAJOR_ERROR)
-    {
-        //本模块功能失败的错误,向上抛出
-        return code;
-    }*/
 
-    return SUCCESS;
+        auto end_time=std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
+        time=1000.0*double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+        std::this_thread::yield();
+    }while(time<double(timeout));
+
+    return Error_manager(RESPONSE_TIMEOUT,MINOR_ERROR,"measure request timeout");
 }
 
-Error_manager Locate_communicator::create_locate_communicator(std::string str_ip,int port)
+
+Error_manager Locate_communicator::execute_msg(Communication_message* p_msg)
 {
-    /*Error_manager code=SUCCESS;
-    if(mp_locate_communicator==NULL)
-    {
-        mp_locate_communicator=new Locate_communicator();
-        char connect_str[255]={0};
-        sprintf(connect_str,"tcp://%s:%d",str_ip.c_str(),port);
-        code=mp_locate_communicator->m_nnxx_client.connect(connect_str);
-        return code;
-    } else
-    {
-        return code;
-    }*/
+    if(p_msg== nullptr)
+        return Error_manager(POINTER_IS_NULL,CRITICAL_ERROR,"measure response msg pointer is null");
 
+
+    //测量response消息
+    if(p_msg->get_message_type()==Communication_message::eLocate_response_msg)
+    {
+        message::Measure_response_msg response;
+        response.ParseFromString(p_msg->get_message_buf());
+        ///查询请求表是否存在
+        if(m_response_table.find(response.command_id())==m_response_table.end())
+        {
+            return Error_manager(LOCATER_MSG_RESPONSE_HAS_NO_REQUEST,NEGLIGIBLE_ERROR,"measure response without request");
+        }
+        ////更新表
+        m_response_table[response.command_id()]=response;
+    }
 }
 
-Locate_communicator* Locate_communicator::get_instance()
-{
-    return mp_locate_communicator;
-}

lidar_locate/Locate_communicator.h

@@ -6,20 +6,30 @@
 #define NNXX_TESTS_LOCATE_COMMUNICATOR_H
 
 #include <mutex>
+#include "communication_socket_base.h"
+#include "singleton.h"
 
-//#include "locate_message.pb.h"
-#include "../message/measure_message.pb.h"
-#include "../error_code/error_code.h"
+#include "measure_message.pb.h"
+#include "error_code.h"
 
-class Locate_communicator {
+#include "thread_safe_map.h"
+
+class Locate_communicator: public Singleton<Locate_communicator>, public Communication_socket_base
+{
+    friend Singleton<Locate_communicator>;
 public:
     virtual ~Locate_communicator();
-    Error_manager locate_request(message::Measure_request_msg request,message::Measure_response_msg& result,unsigned int timeout=3000);
-    static Error_manager create_locate_communicator(std::string str_ip,int port);
-    static Locate_communicator* get_instance();
+    Error_manager locate_request(message::Measure_request_msg& request,message::Measure_response_msg& result);
+
+
 protected:
     Locate_communicator();
-    static  Locate_communicator*  mp_locate_communicator;
+    virtual Error_manager encapsulate_msg(Communication_message* message);
+    virtual Error_manager execute_msg(Communication_message* p_msg);
+
+protected:
+
+    thread_safe_map<int,message::Measure_response_msg>         m_response_table;
 };
 
 

tool/thread_safe_map.cpp

@@ -0,0 +1,5 @@
+//
+// Created by zx on 2020/7/3.
+//
+
+#include "thread_safe_map.h"

tool/thread_safe_map.h

@@ -0,0 +1,55 @@
+//
+// Created by zx on 2020/7/3.
+//
+
+#ifndef NNXX_TESTS_THREAD_SAFE_MAP_H
+#define NNXX_TESTS_THREAD_SAFE_MAP_H
+
+
+#include <map>
+#include <mutex>
+template<typename Key, typename Val>
+class thread_safe_map
+{
+public:
+    typedef typename std::map<Key, Val>::iterator this_iterator;
+    typedef typename std::map<Key, Val>::const_iterator this_const_iterator;
+    Val& operator [](const Key& key)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_[key];
+    }
+    int erase(const Key& key )
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.erase(key);
+    }
+
+    this_iterator find( const Key& key )
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.find(key);
+    }
+    this_const_iterator find( const Key& key ) const
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.find(key);
+    }
+
+    this_iterator end()
+    {
+        return dataMap_.end();
+    }
+
+    this_const_iterator end() const
+    {
+        return dataMap_.end();
+    }
+
+private:
+    std::map<Key, Val> dataMap_;
+    std::mutex mtx_;
+};
+
+
+#endif //NNXX_TESTS_THREAD_SAFE_MAP_H