增加调度模拟, 停取车测试

CMakeLists.txt

@@ -23,6 +23,7 @@ include_directories(
         ./system
         ./lidar_locate
         ./parkspace
+        ./dispatch
 )
 link_directories("/usr/local/lib")
 
@@ -43,6 +44,7 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool/TaskQueue TASK_QUEUE_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication COMMUNICATION_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/system SYSTEM_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/parkspace PARKSPACE_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/dispatch DISPATCH_SRC )
 
 add_executable(process
         main.cpp
@@ -50,17 +52,12 @@ add_executable(process
         ${locate_src}
         ${robot_src}
         ${message_src}
-
-#        ${LASER_SRC}
-#        ${PLC_SRC}
-#        ${TERMINOR_SRC}
-#        ${LOCATE_SRC}
-#        ${TASK_MANAGER_SRC}
         ${TOOL_SRC}
         ${TASK_QUEUE_SRC}
         ${COMMUNICATION_SRC}
         ${SYSTEM_SRC}
         ${PARKSPACE_SRC}
+        ${DISPATCH_SRC}
         )
 
 target_link_libraries(process

communication/communication_message.h

@@ -29,9 +29,9 @@ public:
 		eLocate_request_msg=0x12,               //定位请求消息
 		eLocate_response_msg=0x13,              //定位反馈消息
 
-		eHarware_statu_msg=0x21,                //调度模块硬件状态消息
-		eExecute_request_msg=0x22,              //请求调度消息
-		eExecute_response_msg=0x23,             //调度结果反馈消息
+        eDispatch_status_msg=0x21,                //调度模块硬件状态消息
+        eDispatch_request_msg=0x22,              //请求调度消息
+        eDispatch_response_msg=0x23,             //调度结果反馈消息
 
         eParkspace_allocation_status_msg = 0x31,   //车位分配模块状态消息，包括车位信息
         eParkspace_allocation_request_msg = 0x32,  //请求分配车位消息

dispatch/dispatch_communicator.cpp

@@ -0,0 +1,213 @@
+//
+// Created by zx on 2020/7/21.
+//
+
+#include "dispatch_communicator.h"
+
+
+Dispatch_communicator::~Dispatch_communicator(){}
+
+Error_manager Dispatch_communicator::dispatch_request(message::Dispatch_request_msg& request,
+        message::Dispatch_response_msg& result)
+{
+
+    /*
+      * 检查request合法性,以及模块状态
+      */
+
+    if(request.base_info().sender()!=message::eMain||request.base_info().receiver()!=message::eDispatch)
+        return Error_manager(ERROR,MINOR_ERROR,"dispatch request invalid");
+
+    if(m_response_table.find(request)==true)
+        return Error_manager(ERROR,MAJOR_ERROR," dispatch request repeated");
+    //设置超时,若没有设置,默认300000  五分钟
+    int timeout=request.base_info().has_timeout_ms()?request.base_info().timeout_ms():300000;
+    //向测量节点发送测量请求,并记录请求
+
+    Error_manager code;
+    Communication_message message;
+
+    message::Base_info base_msg;
+    base_msg.set_msg_type(message::eDispatch_request_msg);
+    base_msg.set_sender(message::eMain);
+    base_msg.set_receiver(message::eDispatch);
+    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::Dispatch_response_msg response;
+        ///查询是否存在,并且删除该记录,
+        if(m_response_table.find(request,response))
+        {
+            //判断是否接收到回应,若回应信息被赋值则证明有回应
+            if (response.has_base_info() && response.has_command_id())
+            {
+                message::Base_info response_base = response.base_info();
+                //检查类型是否匹配
+                if (response_base.msg_type() != message::eDispatch_response_msg) {
+                    return Error_manager(ERROR, CRITICAL_ERROR,
+                                         "dispatch response basemsg type error");
+                }
+                //检查基本信息是否匹配
+                if (response_base.sender() != message::eDispatch ||
+                    response_base.receiver() != message::eMain ||
+                    response.command_id() != request.command_id()) {
+                    return Error_manager(ERROR, MAJOR_ERROR,
+                                         "dispatch response basemsg info error");
+                }
+                result = response;
+                m_response_table.erase(request);
+
+                return SUCCESS;
+
+            }
+        }
+        else
+        {
+            //未查询到记录,任务已经被提前取消,记录被删除
+            return Error_manager(FAILED,MINOR_ERROR,"dispatch request canceled");
+        }
+
+        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();
+        usleep(1000);
+    }while(time<double(timeout));
+    m_response_table.erase(request);
+    return Error_manager(RESPONSE_TIMEOUT,MINOR_ERROR,"dispatch request timeout");
+}
+/*
+ * 提前取消请求
+ */
+Error_manager Dispatch_communicator::cancel_request(message::Dispatch_request_msg& request)
+{
+    message::Dispatch_response_msg t_response;
+    if(m_response_table.find(request,t_response))
+    {
+        m_response_table.erase(request);
+    }
+    return SUCCESS;
+}
+Error_manager Dispatch_communicator::check_statu()
+{
+    return SUCCESS;
+}
+
+
+Dispatch_communicator::Dispatch_communicator()
+{
+}
+Error_manager Dispatch_communicator::encapsulate_msg(Communication_message* message)
+{
+    Error_manager code;
+    //记录请求
+    switch (message->get_message_type())
+    {
+        case Communication_message::eDispatch_request_msg:
+        {
+            message::Dispatch_request_msg request;
+            if(false==request.ParseFromString(message->get_message_buf()))
+            {
+                code=Error_manager(ERROR,CRITICAL_ERROR,"request message parse failed");
+            }
+            m_response_table[request]=message::Dispatch_response_msg();
+            //发送请求
+            code= Communication_socket_base::encapsulate_msg(message);
+            if(code!=SUCCESS)
+            {
+                m_response_table.erase(request);
+            }
+            break;
+        }
+        default:
+            code= Error_manager(FAILED,CRITICAL_ERROR," measure发送任务类型不存在");
+            break;
+    }
+    return code;
+}
+Error_manager Dispatch_communicator::execute_msg(Communication_message* p_msg)
+{
+    if(p_msg== nullptr)
+        return Error_manager(POINTER_IS_NULL,CRITICAL_ERROR,"dispatch response msg pointer is null");
+
+
+    //测量response消息
+    switch (p_msg->get_message_type())
+    {
+        ///测量结果反馈消息
+        case Communication_message::eDispatch_response_msg:
+        {
+            message::Dispatch_response_msg response;
+            response.ParseFromString(p_msg->get_message_buf());
+            message::Dispatch_request_msg request=create_request_by_response(response);
+            ///查询请求表是否存在,并且更新
+            if(m_response_table.find_update(request,response)==false)
+            {
+                return Error_manager(ERROR,NEGLIGIBLE_ERROR,"dispatch response without request");
+            }
+
+            break;
+        }
+            ///测量系统状态
+        case Communication_message::eDispatch_status_msg:
+        {
+            //m_measure_statu_msg=*p_msg;
+            break;
+        }
+    }
+    return SUCCESS;
+}
+/*
+ * 检测消息是否可被处理
+ */
+Error_manager Dispatch_communicator::check_msg(Communication_message* p_msg)
+{
+    //通过 p_msg->get_message_type() 和 p_msg->get_receiver() 判断这条消息是不是给我的.
+    //子类重载时, 增加自己模块的判断逻辑, 以后再写.
+    if ( (p_msg->get_message_type() == Communication_message::Message_type::eDispatch_response_msg
+          ||p_msg->get_message_type() == Communication_message::Message_type::eDispatch_status_msg)
+         && p_msg->get_receiver() == Communication_message::Communicator::eMain )
+    {
+        return Error_code::SUCCESS;
+    }
+    else
+    {
+        //认为接受人
+        return Error_code::INVALID_MESSAGE;
+    }
+}
+/*
+ * 心跳发送函数,重载
+ */
+Error_manager Dispatch_communicator::encapsulate_send_data()
+{
+    return SUCCESS;
+}
+//检查消息是否可以被解析, 需要重载
+Error_manager Dispatch_communicator::check_executer(Communication_message* p_msg)
+{
+    return SUCCESS;
+}
+
+/*
+ * 根据接收到的response,生成对应的 request
+ */
+message::Dispatch_request_msg Dispatch_communicator::create_request_by_response(message::Dispatch_response_msg& response)
+{
+    message::Dispatch_request_msg request;
+    message::Base_info baseInfo;
+    baseInfo.set_msg_type(message::eDispatch_request_msg);
+    baseInfo.set_sender(response.base_info().receiver());
+    baseInfo.set_receiver(response.base_info().sender());
+    request.mutable_base_info()->CopyFrom(baseInfo);
+    request.set_command_id(response.command_id());
+    return request;
+}

dispatch/dispatch_communicator.h

@@ -0,0 +1,57 @@
+//
+// Created by zx on 2020/7/21.
+//
+
+#ifndef NNXX_TESTS_DISPATCH_COMMUNICATOR_H
+#define NNXX_TESTS_DISPATCH_COMMUNICATOR_H
+
+#include "communication_socket_base.h"
+#include "singleton.h"
+
+#include "dispatch_message.pb.h"
+#include "error_code.h"
+
+#include "thread_safe_map.h"
+#include "message_compare.h"
+
+RegistoryCompare(message,message::Dispatch_request_msg)
+
+class Dispatch_communicator :public Singleton<Dispatch_communicator>, public Communication_socket_base
+{
+    friend Singleton<Dispatch_communicator>;
+public:
+    virtual ~Dispatch_communicator();
+    Error_manager dispatch_request(message::Dispatch_request_msg& request,message::Dispatch_response_msg& result);
+    /*
+     * 提前取消请求
+     */
+    Error_manager cancel_request(message::Dispatch_request_msg& request);
+    Error_manager check_statu();
+
+
+protected:
+    Dispatch_communicator();
+    virtual Error_manager encapsulate_msg(Communication_message* message);
+    virtual Error_manager execute_msg(Communication_message* p_msg);
+    /*
+     * 检测消息是否可被处理
+     */
+    virtual Error_manager check_msg(Communication_message* p_msg);
+    /*
+     * 心跳发送函数,重载
+     */
+    virtual Error_manager encapsulate_send_data();
+    //检查消息是否可以被解析, 需要重载
+    virtual Error_manager check_executer(Communication_message* p_msg);
+
+    /*
+     * 根据接收到的response,生成对应的 request
+     */
+    message::Dispatch_request_msg create_request_by_response(message::Dispatch_response_msg& msg);
+
+private:
+    thread_safe_map<message::Dispatch_request_msg,message::Dispatch_response_msg>          m_response_table;
+};
+
+
+#endif //NNXX_TESTS_DISPATCH_COMMUNICATOR_H

lidar_locate/Locate_communicator.h

@@ -14,7 +14,6 @@
 #include "error_code.h"
 
 #include "thread_safe_map.h"
-
 #include "message_compare.h"
 
 /*

main.cpp

@@ -16,6 +16,7 @@
 
 #include <glog/logging.h>
 #include <Parkspace_communicator.h>
+#include <dispatch_communicator.h>
 //using google::protobuf::io::FileInputStream;
 //using google::protobuf::io::FileOutputStream;
 using google::protobuf::io::ZeroCopyInputStream;
@@ -71,11 +72,28 @@ Error_manager init_communicators()
     }
     Parkspace_communicator::get_instance_pointer()->communication_run();
 
+    /*
+     * 初始化调度模块
+     */
+    Dispatch_communicator::get_instance_pointer()->communication_connect("tcp://192.168.2.192:5555");
+    Dispatch_communicator::get_instance_pointer()->communication_run();
+
+
+
+    /*
+     *
+     * 此处添加等待各个通讯模块正常代码
+     *
+     */
+
+
     ///最后初始化与终端通讯的对象
     if(System_communicator::get_instance_pointer()== nullptr)
         return FAILED;
     System_communicator::get_instance_pointer()->communication_bind("tcp://127.0.0.1:9001");
     System_communicator::get_instance_pointer()->communication_run();
+
+
     /*
      * 初始化指令执行模块
      */

message/dispatch_message.proto

@@ -0,0 +1,73 @@
+syntax = "proto2";
+package message;
+import "message_base.proto";
+
+//调度管理 的状态
+enum Dispatch_manager_status
+{
+    E_DISPATCH_MANAGER_UNKNOW               = 0;    //未知
+    E_DISPATCH_MANAGER_READY                = 1;    //准备，待机
+    E_DISPATCH_MANAGER_STORE                = 2;    //正在存车
+    E_DISPATCH_MANAGER_PICKUP               = 3;    //正在取车
+
+    E_DISPATCH_MANAGER_FAULT               = 10;    //故障
+}
+
+//搬运器状态, 楚天项目就是AGV系统
+enum Carrier_status
+{
+    E_CARRIER_UNKNOW               = 0;     //未知
+    E_CARRIER_READY                = 1;     //准备，待机
+    E_CARRIER_STORE                = 2;	    //正在存车
+    E_CARRIER_PICKUP               = 3;	    //正在取车
+
+    E_CARRIER_FAULT                = 10;     //故障
+}
+
+
+//搬运器位置. AGV或者抓取机器人的坐标
+message Carrier_position
+{
+    required float x=1;             //X轴坐标
+    required float y=2;             //Y轴坐标
+    required float z=3;             //Z轴坐标, 楚天AGV和电梯一一对应,所以Z轴就用电梯高度表示
+}
+
+//搬运机构各个零部件状态
+message Dispatch_status_msg
+{
+    required Base_info                  base_info=1;                    //消息类型
+
+    required Dispatch_manager_status    dispatch_manager_status = 2;    //调度管理 的状态
+ //   repeated Carrier_status             carrier_status = 3;             //搬运器状态, 楚天有3套AGV系统
+
+}
+
+//调度方向, 停车取车
+enum Dispatch_motion_direction
+{
+    E_STORE_CAR             =0;         //停车, 出入口 -> 停车位
+    E_PICKUP_CAR            =1;         //取车, 停车位 -> 出入口
+}
+
+//执行搬运请求
+message Dispatch_request_msg
+{
+    required Base_info                  base_info=1;                            //消息类型
+    required int32                      command_id=2;                           //指令唯一标识符id
+
+    required Dispatch_motion_direction  dispatch_motion_direction=3;            //调度方向, 停车取车
+    required int32                      terminal_id=4;                          //终端id, 出入口
+    required int32                      parkspace_id=5;                         //车位编号, 停车位
+    optional Locate_information         locate_information=6;                   //汽车测量信息, 只有停车时有数据, 取车时没有数据.
+}
+
+//搬运动作执行完成后反馈结果
+message Dispatch_response_msg
+{
+    required Base_info                  base_info=1;                    //消息类型
+    required int32                      command_id=2;                   //指令唯一标识符id
+    required Error_manager              error_manager = 3;
+}
+
+

message/message_base.pb.cc

@@ -342,35 +342,35 @@ void AddDescriptorsImpl() {
       "\016\n\006height\030\007 \001(\002\0223\n\020parkspace_status\030\010 \001("
       "\0162\031.message.Parkspace_status\022#\n\010car_info"
       "\030\t \001(\0132\021.message.Car_info\022\022\n\nentry_time\030"
-      "\n \001(\t\022\022\n\nleave_time\030\013 \001(\t*\351\004\n\014Message_ty"
+      "\n \001(\t\022\022\n\nleave_time\030\013 \001(\t*\355\004\n\014Message_ty"
       "pe\022\r\n\teBase_msg\020\000\022\020\n\014eCommand_msg\020\001\022\026\n\022e"
       "Locate_status_msg\020\021\022\027\n\023eLocate_request_m"
-      "sg\020\022\022\030\n\024eLocate_response_msg\020\023\022\026\n\022eHarwa"
-      "re_statu_msg\020!\022\030\n\024eExecute_request_msg\020\""
-      "\022\031\n\025eExecute_response_msg\020#\022$\n eParkspac"
-      "e_allocation_status_msg\0201\022%\n!eParkspace_"
-      "allocation_request_msg\0202\022&\n\"eParkspace_a"
-      "llocation_response_msg\0203\022!\n\035eParkspace_s"
-      "earch_request_msg\0204\022\"\n\036eParkspace_search"
-      "_response_msg\0205\022\"\n\036eParkspace_release_re"
-      "quest_msg\0206\022#\n\037eParkspace_release_respon"
-      "se_msg\0207\022\036\n\032eStore_command_request_msg\020A"
-      "\022\037\n\033eStore_command_response_msg\020B\022\037\n\033ePi"
-      "ckup_command_request_msg\020C\022 \n\034ePickup_co"
-      "mmand_response_msg\020D\022\027\n\023eEntrance_statu_"
-      "msg\020Q*e\n\014Communicator\022\n\n\006eEmpty\020\000\022\t\n\005eMa"
-      "in\020\001\022\016\n\teTerminor\020\200\002\022\017\n\neParkspace\020\200\004\022\016\n"
-      "\teMeasurer\020\200\006\022\r\n\010eProcess\020\200\010*e\n\013Error_le"
-      "vel\022\n\n\006NORMAL\020\000\022\024\n\020NEGLIGIBLE_ERROR\020\001\022\017\n"
-      "\013MINOR_ERROR\020\002\022\017\n\013MAJOR_ERROR\020\003\022\022\n\016CRITI"
-      "CAL_ERROR\020\004*q\n\020Parkspace_status\022\024\n\020ePark"
-      "space_empty\020\000\022\027\n\023eParkspace_occupied\020\001\022\030"
-      "\n\024eParkspace_reserverd\020\002\022\024\n\020eParkspace_e"
-      "rror\020\003*(\n\tDirection\022\014\n\010eForward\020\001\022\r\n\teBa"
-      "ckward\020\002"
+      "sg\020\022\022\030\n\024eLocate_response_msg\020\023\022\030\n\024eDispa"
+      "tch_status_msg\020!\022\031\n\025eDispatch_request_ms"
+      "g\020\"\022\032\n\026eDispatch_response_msg\020#\022$\n ePark"
+      "space_allocation_status_msg\0201\022%\n!eParksp"
+      "ace_allocation_request_msg\0202\022&\n\"eParkspa"
+      "ce_allocation_response_msg\0203\022!\n\035eParkspa"
+      "ce_search_request_msg\0204\022\"\n\036eParkspace_se"
+      "arch_response_msg\0205\022\"\n\036eParkspace_releas"
+      "e_request_msg\0206\022#\n\037eParkspace_release_re"
+      "sponse_msg\0207\022\036\n\032eStore_command_request_m"
+      "sg\020A\022\037\n\033eStore_command_response_msg\020B\022\037\n"
+      "\033ePickup_command_request_msg\020C\022 \n\034ePicku"
+      "p_command_response_msg\020D\022\027\n\023eEntrance_st"
+      "atu_msg\020Q*f\n\014Communicator\022\n\n\006eEmpty\020\000\022\t\n"
+      "\005eMain\020\001\022\016\n\teTerminor\020\200\002\022\017\n\neParkspace\020\200"
+      "\004\022\016\n\teMeasurer\020\200\006\022\016\n\teDispatch\020\200\010*e\n\013Err"
+      "or_level\022\n\n\006NORMAL\020\000\022\024\n\020NEGLIGIBLE_ERROR"
+      "\020\001\022\017\n\013MINOR_ERROR\020\002\022\017\n\013MAJOR_ERROR\020\003\022\022\n\016"
+      "CRITICAL_ERROR\020\004*q\n\020Parkspace_status\022\024\n\020"
+      "eParkspace_empty\020\000\022\027\n\023eParkspace_occupie"
+      "d\020\001\022\030\n\024eParkspace_reserverd\020\002\022\024\n\020eParksp"
+      "ace_error\020\003*(\n\tDirection\022\014\n\010eForward\020\001\022\r"
+      "\n\teBackward\020\002"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 1928);
+      descriptor, 1933);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "message_base.proto", &protobuf_RegisterTypes);
 }

message/message_base.pb.h

@@ -92,9 +92,9 @@ enum Message_type {
   eLocate_status_msg = 17,
   eLocate_request_msg = 18,
   eLocate_response_msg = 19,
-  eHarware_statu_msg = 33,
-  eExecute_request_msg = 34,
-  eExecute_response_msg = 35,
+  eDispatch_status_msg = 33,
+  eDispatch_request_msg = 34,
+  eDispatch_response_msg = 35,
   eParkspace_allocation_status_msg = 49,
   eParkspace_allocation_request_msg = 50,
   eParkspace_allocation_response_msg = 51,
@@ -129,11 +129,11 @@ enum Communicator {
   eTerminor = 256,
   eParkspace = 512,
   eMeasurer = 768,
-  eProcess = 1024
+  eDispatch = 1024
 };
 bool Communicator_IsValid(int value);
 const Communicator Communicator_MIN = eEmpty;
-const Communicator Communicator_MAX = eProcess;
+const Communicator Communicator_MAX = eDispatch;
 const int Communicator_ARRAYSIZE = Communicator_MAX + 1;
 
 const ::google::protobuf::EnumDescriptor* Communicator_descriptor();

message/message_base.proto

@@ -13,9 +13,9 @@ enum Message_type
     eLocate_response_msg=0x13;              //定位反馈消息
 
 
-    eHarware_statu_msg=0x21;                //调度模块硬件状态消息
-    eExecute_request_msg=0x22;              //请求调度消息
-    eExecute_response_msg=0x23;             //调度结果反馈消息
+    eDispatch_status_msg=0x21;                //调度模块硬件状态消息
+    eDispatch_request_msg=0x22;              //请求调度消息
+    eDispatch_response_msg=0x23;             //调度结果反馈消息
 
     eParkspace_allocation_status_msg = 0x31;   //车位分配模块状态消息，包括车位信息
     eParkspace_allocation_request_msg = 0x32;  //请求分配车位消息
@@ -48,7 +48,7 @@ enum Communicator
     //测量单元
     eMeasurer=0x0300;
     //调度机构
-    eProcess=0x0400;
+    eDispatch=0x0400;
     //...
 
 

message/robot_message.proto

@@ -1,68 +0,0 @@
-syntax = "proto2";
-package message;
-import "message_base.proto";
-
-enum Robot_statu        //硬件状态
-{
-    eNormal=0;             //正常且空闲
-    eBusy=1;               //工作中
-    eMiss=2;                //连接丢失
-    eError=3;              //故障报错
-
-}
-
-//硬件位置.
-message Position
-{
-    required float x=1;
-    required float y=2;
-    required float z=3;
-
-}
-
-//搬运器状态
-message Carrier_status
-{
-    required Robot_statu             statu=1;                   //状态
-    optional int32                      command_id=2;                   //指令唯一标识符id
-    optional string                     error_description=3;       //搬运器错误信息(可无)
-    required Position                   position=4;                //搬运器位置(z表示电梯位置)
-    required bool                       empty=5;                   //是否空载
-}
-
-//搬运机构各个零部件状态
-message Robot_statu_msg
-{
-    required Message_type               msg_type=1;                 //消息类型
-    required Carrier_status             carrier1_statu=2;           //搬运器1状态
-    required Carrier_status             carrier2_statu=3;           //搬运器2状态
-    required Carrier_status             carrier3_statu=4;           //搬运器3状态
-}
-
-enum Action_type
-{
-    ePark=0;
-    ePick=1;
-}
-
-//执行搬运请求
-message Robot_carry_request_msg
-{
-    required Base_info                   msg_info=1;         //消息类型                         //消息类型
-    required int32                      command_id=2;                   //指令唯一标识符id
-    required Action_type                action_type=3;
-    required int32                      from_id=4;
-    required int32                      destination=5;
-    required Locate_information         locate_information=6;
-}
-
-//搬运动作执行完成后反馈结果
-message Robot_carry_response_msg
-{
-    required Base_info                   msg_info=1;                    //消息类型
-    required int32                      command_id=2;                   //指令唯一标识符id
-    required int32                      error_code=3;
-    optional string                     error_description=4;
-}
-
-

proto.sh

@@ -1,6 +1,6 @@
 protoc -I=./message message_base.proto --cpp_out=./message
 protoc -I=./message measure_message.proto --cpp_out=./message
-protoc -I=./message robot_message.proto --cpp_out=./message
 protoc -I=./message parkspace_allocation_message.proto --cpp_out=./message
 protoc -I=./message terminal_message.proto --cpp_out=./message
-protoc -I=./message process_message.proto --cpp_out=./message
+protoc -I=./message process_message.proto --cpp_out=./message
+protoc -I=./message dispatch_message.proto --cpp_out=./message

system/PickupProcessTask.cpp

@@ -3,6 +3,7 @@
 //
 
 #include <Parkspace_communicator.h>
+#include <dispatch_message.pb.h>
 #include "PickupProcessTask.h"
 #include "process_message.pb.h"
 #include "command_manager.h"
@@ -56,8 +57,20 @@ void PickupProcessTask::Main()
             code=pickup_step();
             if(code!=SUCCESS)
             {
+                LOG(ERROR)<<"取车调度失败,取车终端:"<<m_terminor_id<<"指令id:"<<m_command_id
+                            <<", 车位id:"<<m_parcspace_search_response_msg.car_position().parkspace_id()
+                            <<", 车位楼层:"<<m_parcspace_search_response_msg.car_position().floor()
+                            <<", 车位序号:"<<m_parcspace_search_response_msg.car_position().index()
+                            <<", 车牌号:"<<m_car_info.license()
+                            <<", 库内车牌号:"<<m_parcspace_search_response_msg.car_position().car_info().license();
                 break;
             }
+            LOG(WARNING)<<"取车调度成功,取车终端:"<<m_terminor_id<<"指令id:"<<m_command_id
+                        <<", 车位id:"<<m_parcspace_search_response_msg.car_position().parkspace_id()
+                        <<", 车位楼层:"<<m_parcspace_search_response_msg.car_position().floor()
+                        <<", 车位序号:"<<m_parcspace_search_response_msg.car_position().index()
+                        <<", 车牌号:"<<m_car_info.license()
+                        <<", 库内车牌号:"<<m_parcspace_search_response_msg.car_position().car_info().license();
         }
         //第二步,清除车位
         case 1:
@@ -79,15 +92,6 @@ void PickupProcessTask::Main()
             }
         }
     }
-    //暂停 5-15s 代替
-    int delay=rand()%7000+4000;
-    usleep(1000*delay);
-    LOG(WARNING)<<"取车成功,取车终端:"<<m_terminor_id<<"指令id:"<<m_command_id
-                <<", 车位id:"<<m_parcspace_search_response_msg.car_position().parkspace_id()
-                <<", 车位楼层:"<<m_parcspace_search_response_msg.car_position().floor()
-                <<", 车位序号:"<<m_parcspace_search_response_msg.car_position().index()
-                <<", 车牌号:"<<m_car_info.license()
-                <<", 库内车牌号:"<<m_parcspace_search_response_msg.car_position().car_info().license();
     Command_manager::get_instance_pointer()->updata_pickup_entrance_statu(m_terminor_id,message::eReady);
     ////资源回收
 
@@ -98,7 +102,40 @@ void PickupProcessTask::Main()
  */
 Error_manager PickupProcessTask::pickup_step()
 {
-    return SUCCESS;
+    /*
+  * 检查是否曾经分配过车位
+  */
+    if(m_parcspace_search_response_msg.has_car_position()==false)
+    {
+        return Error_manager(FAILED,MINOR_ERROR," 取车流程释放车位请求缺少车位信息");
+    }
+
+    message::Dispatch_request_msg request;
+    message::Base_info base_info;
+    base_info.set_msg_type(message::eDispatch_request_msg);
+    base_info.set_sender(message::eMain);
+    base_info.set_receiver(message::eDispatch);
+    base_info.set_timeout_ms(1000*300); //测量超时300s
+    request.mutable_base_info()->CopyFrom(base_info);
+
+    message::Parkspace_info space_info=m_parcspace_search_response_msg.car_position();
+    request.set_dispatch_motion_direction(message::E_PICKUP_CAR);
+    request.set_parkspace_id(space_info.parkspace_id());
+    request.set_terminal_id(m_terminor_id);
+    request.set_command_id(m_command_id);
+
+
+    message::Dispatch_response_msg response;
+    Error_manager code=Dispatch_communicator::get_instance_pointer()->dispatch_request(request,response);
+    if(code!=SUCCESS)
+        return code;
+
+    if(response.error_manager().error_code()==0) {
+        return SUCCESS;
+    }
+    else
+        return Error_manager(FAILED,MINOR_ERROR,"取车流程调度反馈错误码");
+
 }
 
 /*

system/PickupProcessTask.h

@@ -7,6 +7,7 @@
 #include <parkspace_allocation_message.pb.h>
 #include "error_code.h"
 #include "TaskQueue/BaseTask.h"
+#include "dispatch_communicator.h"
 
 class PickupProcessTask :public tq::BaseTask{
 public:
@@ -44,6 +45,8 @@ protected:
 private:
     //查询到的车位信息
     message::Parkspace_search_response_msg                  m_parcspace_search_response_msg;
+    //取车调度反馈信息
+    message::Dispatch_response_msg                          m_dispatch_response_msg;
 
 };
 

system/StoreProcessTask.cpp

@@ -3,6 +3,8 @@
 //
 
 #include <Parkspace_communicator.h>
+#include <dispatch_message.pb.h>
+#include "dispatch_communicator.h"
 #include "StoreProcessTask.h"
 #include "process_message.pb.h"
 #include "command_manager.h"
@@ -46,12 +48,6 @@ Error_manager StoreProcessTask::locate_step() {
         return code;
 
     if(m_measure_response_msg.error_manager().error_code()==0) {
-        /*LOG(INFO)<<"测量成功,停车终端:"<<m_terminor_id
-                 <<", 指令id:"<<m_command_id
-                 <<", 车辆宽:"<<m_measure_response_msg.locate_information().locate_width()
-                 <<", 车辆高:"<<m_measure_response_msg.locate_information().locate_height()
-                 <<", 车辆角度:"<<m_measure_response_msg.locate_information().locate_angle()
-                 <<", 车牌号:"<<m_parcspace_alloc_response_msg.allocated_space_info().car_info().license();*/
         return SUCCESS;
     }
     else
@@ -59,6 +55,47 @@ Error_manager StoreProcessTask::locate_step() {
 
 }
 
+/*
+* 调度
+*/
+Error_manager StoreProcessTask::dispatch_step()
+{
+    /*
+     * 判断调度所需的数据是否都正常
+     */
+    //1,测量信息是否存在
+    if(m_measure_response_msg.has_base_info()== false
+        ||m_measure_response_msg.has_locate_information()==false
+        ||m_parcspace_alloc_response_msg.has_base_info()== false
+        ||m_parcspace_alloc_response_msg.has_allocated_space_info()==false)
+    {
+        return Error_manager(ERROR,MAJOR_ERROR,"调度所需的前置数据(测量,车位)不存在");
+    }
+
+    message::Dispatch_request_msg request;
+    message::Base_info base_info;
+    base_info.set_msg_type(message::eDispatch_request_msg);
+    base_info.set_sender(message::eMain);
+    base_info.set_receiver(message::eDispatch);
+    base_info.set_timeout_ms(1000*15); //测量超时15s
+    request.mutable_base_info()->CopyFrom(base_info);
+
+    request.set_command_id(m_command_id);
+    request.set_terminal_id(m_terminor_id);
+    request.set_dispatch_motion_direction(message::E_STORE_CAR);
+    request.set_parkspace_id(m_parcspace_alloc_response_msg.allocated_space_info().parkspace_id());
+
+    Error_manager code=Dispatch_communicator::get_instance_pointer()->dispatch_request(request,m_dispatch_response_msg);
+    if(code!=SUCCESS)
+        return code;
+
+    if(m_dispatch_response_msg.error_manager().error_code()==0) {
+        return SUCCESS;
+    }
+    else
+        return Error_manager(FAILED,MINOR_ERROR,"dispatch response error_code error");
+}
+
 /*
      * 分配车位
      */
@@ -170,10 +207,12 @@ void StoreProcessTask::Main()
         //第二步,调度
         case 1:
         {
-            //暂停 5-15s 代替
-            int delay=rand()%5000+5000;
-            usleep(1000*delay);
-            //调度失败,break,完成return
+            code=dispatch_step();
+            if(code!=SUCCESS)
+            {
+                LOG(ERROR)<<"调度失败:"<<code.to_string();
+                break;
+            }
         }
         //第三步,占据车位
         case 2:
@@ -204,5 +243,8 @@ void StoreProcessTask::Main()
          */
     }
 
+    //更新状态
+    Command_manager::get_instance_pointer()->updata_store_entrance_statu(m_terminor_id,message::eReady);
+
 
 }

system/StoreProcessTask.h

@@ -6,6 +6,7 @@
 #define NNXX_TESTS_STOREPROCESS_H
 
 #include <parkspace_allocation_message.pb.h>
+#include "dispatch_communicator.h"
 #include "TaskQueue/BaseTask.h"
 #include "Locate_communicator.h"
 
@@ -20,6 +21,8 @@ public:
      * 分配车位
      */
     Error_manager alloc_space();
+
+
 protected:
     virtual void Main();
 
@@ -28,6 +31,11 @@ protected:
      */
     Error_manager locate_step();
 
+    /*
+     * 调度
+     */
+    Error_manager dispatch_step();
+
    /*
     * 车位解锁,当停车失败时需要车位解锁
     */
@@ -45,6 +53,8 @@ protected:
     message::Measure_response_msg                   m_measure_response_msg;         //测量模块的测量数据
     message::Parkspace_allocation_response_msg      m_parcspace_alloc_response_msg; //分配的车位数据
 
+    message::Dispatch_response_msg                  m_dispatch_response_msg;        //调度模块的反馈数据
+
 public:
     bool                        mb_completed;
 

test/Terminal_communication.cpp

@@ -133,17 +133,6 @@ Error_manager Terminal_communication::set_terminal_id(int id)
 }
 
 
-Error_manager Terminal_communication::send_pickup_task(message::Parkspace_search_request_msg& msg)
-{
-    if(msg.has_base_info()==false)
-        return FAILED;
-    if(msg.base_info().sender()!=message::eTerminor ||msg.base_info().msg_type()!=message::eParkspace_search_request_msg)
-        return FAILED;
-    Communication_message message;
-    message.reset(msg.base_info(),msg.SerializeAsString());
-    return Communication_socket_base::encapsulate_msg(&message);
-}
-
 /*
     * 发送停车指令请求
     */
@@ -185,8 +174,8 @@ Error_manager Terminal_communication::store_request(message::Store_command_reque
                 return Error_manager(PARKSPACE_RELEASE_RESPONSE_INFO_ERROR, MAJOR_ERROR,
                                      "parkspace store response msg info error");
             }
-            response=m_store_response_msg;
-            m_store_response_msg=message::Store_command_response_msg();
+            response.CopyFrom(m_store_response_msg);
+            m_store_response_msg.clear_base_info();
             return SUCCESS;
 
         }
@@ -271,8 +260,11 @@ Error_manager Terminal_communication::pickup_request(message::Pickup_command_req
 Error_manager Terminal_communication::check_store_entrance_statu(int terminal_id,message::Entrance_statu& statu)
 {
     std::lock_guard<std::mutex> lk(m_statu_lock);
-    auto it=m_entrance_status.mutable_store_msg_map()->find(terminal_id);
-    if(it==m_entrance_status.mutable_store_msg_map()->end())
+    if(m_entrance_status.has_base_info()==false)
+        return ERROR;
+    auto map=m_entrance_status.store_msg_map();
+    auto it=map.find(terminal_id);
+    if(it==map.end())
     {
         return ERROR;
     }
@@ -289,8 +281,11 @@ Error_manager Terminal_communication::check_store_entrance_statu(int terminal_id
 Error_manager Terminal_communication::check_pickup_entrance_statu(int terminal_id,message::Entrance_statu& statu)
 {
     std::lock_guard<std::mutex> lk(m_statu_lock);
-    auto it=m_entrance_status.mutable_pickup_msg_map()->find(terminal_id);
-    if(it==m_entrance_status.mutable_pickup_msg_map()->end())
+    if(m_entrance_status.has_base_info()==false)
+        return ERROR;
+    auto map=m_entrance_status.pickup_msg_map();
+    auto it=map.find(terminal_id);
+    if(it==map.end())
     {
         return ERROR;
     }

test/Terminal_communication.h

@@ -38,11 +38,6 @@ public:
      */
     Error_manager pickup_request(message::Pickup_command_request_msg& request,message::Pickup_command_response_msg& response);
 
-    /*
-     * 发送查询车辆信息消息,用于取车终端接收,id表示哪个终端执行取车任务
-     */
-    Error_manager send_pickup_task(message::Parkspace_search_request_msg& msg);
-
     /*
      * 检查入口完成状态
      */

test/pickup_terminal.cpp

@@ -54,9 +54,9 @@ Error_manager pickup_terminal::pickup(message::Car_info& car_info)
         return ERROR;
     }
     //等待停车完成
+    message::Entrance_statu statu;
     while(m_exit_cond.wait_for_millisecond(200)==false)
     {
-        message::Entrance_statu statu;
         code = Terminal_communication::get_instance_pointer()->check_pickup_entrance_statu(m_terminal_id, statu);
         if (code != SUCCESS) {
             continue;

test/store_terminal.cpp

@@ -64,13 +64,13 @@ Error_manager store_terminal::storing(message::Car_info& car_info)
     }
     usleep(1000*100);
     //等待停车完成
+    message::Entrance_statu statu;
     while(m_exit_cond.wait_for_millisecond(200)==false) {
-        message::Entrance_statu statu;
+
         code = Terminal_communication::get_instance_pointer()->check_store_entrance_statu(m_terminal_id, statu);
         if (code != SUCCESS) {
             continue;
         }
-
         if (statu == message::eReady) {
             if (m_thread_safe_output_queue)
                 m_thread_safe_output_queue->push(car_info);

test/terminal_client.cpp

@@ -85,8 +85,8 @@ int main() {
     threadsafe_queue<message::Car_info> input_queue;
     threadsafe_queue<message::Car_info> output_queue;
 
-    const int n_input=6;
-    const int n_output=6;
+    const int n_input=2;
+    const int n_output=2;
     std::vector<store_terminal*> input_terminals;
     std::vector<pickup_terminal*> output_terminals;
     for(int i=0;i<n_input;++i)
@@ -117,6 +117,7 @@ int main() {
         car_info.set_car_height(1.5);
         car_info.set_car_width(1.7);
         input_queue.push(car_info);
+       // std::cout<<" input : "<<++n<<std::endl;
 
         int in_count=input_queue.size();
         int out_count=output_queue.size();