1.添加GetSpaceId()方法获取车位id，同时判断是否是车位点，包含id字段

2.等待载车板状态方法添加判断区域判断，为负就返回false
3.流程执行失败，返回错误信息
4.修改PathNode消息中id为可选。

MPC/custom_type.h

@@ -157,171 +157,9 @@ enum eCarrierIdInRegionId{
     eCarrier10,
     eCarrier11
 };
-//
-typedef std::pair<unsigned short,unsigned short > Region_Carry;
-typedef std::map<unsigned short ,Region_Carry> SpaceNo2Region_Carry;
-//class SpacesNo2Region_Carry{
-//public:
-//    SpaceNo2Region_Carry spacesNo2Region_Carry;
-//public:
-//    SpacesNo2Region_Carry(){
-//        spacesNo2Region_Carry[0] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[1] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[2] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[3] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[4] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[5] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[6] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[7] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[8] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[9] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[10] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[11] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[12] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[13] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[14] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[15] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[16] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[17] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[18] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[19] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[20] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[21] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[22] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[23] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[24] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[25] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[26] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[27] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[28] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[29] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[30] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[31] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[32] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[33] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[34] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[35] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[36] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[37] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[38] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[39] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[40] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[41] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[42] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[43] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[44] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[45] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[46] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[47] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[48] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[49] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[50] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[51] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[52] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[53] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[54] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[55] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[56] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[57] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[58] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[59] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[60] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[61] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[62] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[63] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[64] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[65] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[66] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[67] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[68] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[69] = Region_Carry(eRegion0,eCarrier0);
-//
-//        // --------------------------------------------
-//        spacesNo2Region_Carry[100] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[101] = Region_Carry(eRegion2,eCarrier1);
-//        spacesNo2Region_Carry[102] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[103] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[104] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[105] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[106] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[107] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[108] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[109] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[110] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[111] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[112] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[113] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[114] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[115] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[116] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[117] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[118] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[119] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[120] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[121] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[122] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[123] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[124] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[125] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[126] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[127] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[128] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[129] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[130] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[131] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[132] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[133] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[134] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[135] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[136] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[137] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[138] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[139] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[140] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[141] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[142] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[143] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[144] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[145] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[146] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[147] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[148] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[149] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[150] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[151] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[152] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[153] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[154] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[155] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[156] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[157] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[158] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[159] = Region_Carry(eRegion0,eCarrier0);
-//
-//        spacesNo2Region_Carry[160] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[161] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[162] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[163] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[164] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[165] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[166] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[167] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[168] = Region_Carry(eRegion0,eCarrier0);
-//        spacesNo2Region_Carry[169] = Region_Carry(eRegion0,eCarrier0);
-//
-//    }
-//};
+
+typedef std::pair<short, short > Region_Carry;
+typedef std::map<short ,Region_Carry> SpaceNo2Region_Carry;
 
 //////////////////////////////////////////////////
 

MPC/monitor/emqx/message.pb.cc

@@ -143,7 +143,9 @@ PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
 template <typename>
 PROTOBUF_CONSTEXPR PathNode::PathNode(
     ::_pbi::ConstantInitialized): _impl_{
-    /*decltype(_impl_.id_)*/ {
+    /*decltype(_impl_._has_bits_)*/{}
+  , /*decltype(_impl_._cached_size_)*/{}
+  , /*decltype(_impl_.id_)*/ {
     &::_pbi::fixed_address_empty_string, ::_pbi::ConstantInitialized {}
   }
 
@@ -156,8 +158,7 @@ PROTOBUF_CONSTEXPR PathNode::PathNode(
   , /*decltype(_impl_.w_)*/ 0
 
   , /*decltype(_impl_.theta_)*/ 0
-
-  , /*decltype(_impl_._cached_size_)*/{}} {}
+} {}
 struct PathNodeDefaultTypeInternal {
   PROTOBUF_CONSTEXPR PathNodeDefaultTypeInternal() : _instance(::_pbi::ConstantInitialized{}) {}
   ~PathNodeDefaultTypeInternal() {}
@@ -399,7 +400,7 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _impl_.x_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _impl_.y_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _impl_.theta_),
-    ~0u,  // no _has_bits_
+    PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_._has_bits_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _internal_metadata_),
     ~0u,  // no _extensions_
     ~0u,  // no _oneof_case_
@@ -413,6 +414,12 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_.w_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_.theta_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_.id_),
+    ~0u,
+    ~0u,
+    ~0u,
+    ~0u,
+    ~0u,
+    0,
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::Trajectory, _internal_metadata_),
     ~0u,  // no _extensions_
@@ -526,14 +533,14 @@ static const ::_pbi::MigrationSchema
         { 13, -1, -1, sizeof(::NavMessage::AgvStatu)},
         { 29, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
         { 53, -1, -1, sizeof(::NavMessage::Pose2d)},
-        { 64, -1, -1, sizeof(::NavMessage::PathNode)},
-        { 78, -1, -1, sizeof(::NavMessage::Trajectory)},
-        { 87, 102, -1, sizeof(::NavMessage::NewAction)},
-        { 109, -1, -1, sizeof(::NavMessage::NavCmd)},
-        { 120, -1, -1, sizeof(::NavMessage::NavResponse)},
-        { 130, -1, -1, sizeof(::NavMessage::ManualCmd)},
-        { 141, 158, -1, sizeof(::NavMessage::NavStatu)},
-        { 167, 179, -1, sizeof(::NavMessage::RobotStatu)},
+        { 64, 78, -1, sizeof(::NavMessage::PathNode)},
+        { 84, -1, -1, sizeof(::NavMessage::Trajectory)},
+        { 93, 108, -1, sizeof(::NavMessage::NewAction)},
+        { 115, -1, -1, sizeof(::NavMessage::NavCmd)},
+        { 126, -1, -1, sizeof(::NavMessage::NavResponse)},
+        { 136, -1, -1, sizeof(::NavMessage::ManualCmd)},
+        { 147, 164, -1, sizeof(::NavMessage::NavStatu)},
+        { 173, 185, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -563,40 +570,41 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
     "\001(\002\022\n\n\002L1\030\n \001(\002\022\n\n\002P1\030\013 \001(\005\022\n\n\002D1\030\014 \001(\002\022"
     "\n\n\002Y1\030\r \001(\002\022\n\n\002Y2\030\016 \001(\002\022\n\n\002CL\030\017 \001(\005\022\013\n\003e"
     "nd\030\020 \001(\005\"-\n\006Pose2d\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002"
-    "\022\r\n\005theta\030\003 \001(\002\"Q\n\010PathNode\022\t\n\001x\030\001 \001(\002\022\t"
+    "\022\r\n\005theta\030\003 \001(\002\"]\n\010PathNode\022\t\n\001x\030\001 \001(\002\022\t"
     "\n\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022\t\n\001w\030\004 \001(\002\022\r\n\005theta"
-    "\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n\nTrajectory\022!\n\005pose"
-    "s\030\001 \003(\0132\022.NavMessage.Pose2d\"\345\001\n\tNewActio"
-    "n\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode\030\002 \001(\0132\024.Nav"
-    "Message.PathNode\022&\n\010passNode\030\003 \001(\0132\024.Nav"
-    "Message.PathNode\022(\n\nstreetNode\030\004 \001(\0132\024.N"
-    "avMessage.PathNode\022\'\n\tpathNodes\030\005 \003(\0132\024."
-    "NavMessage.PathNode\022\021\n\twheelbase\030\006 \001(\002\022\023"
-    "\n\013changedMode\030\007 \001(\005\"P\n\006NavCmd\022\016\n\006action\030"
-    "\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nnewActions\030\005 \003(\0132\025"
-    ".NavMessage.NewAction\"(\n\013NavResponse\022\013\n\003"
-    "ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tManualCmd\022\013\n\003"
-    "key\030\001 \001(\t\022\026\n\016operation_type\030\002 \001(\005\022\020\n\010vel"
-    "ocity\030\003 \001(\002\"\366\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\005\022"
-    "\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030\003 \001(\005\022(\n\004"
-    "odom\030\004 \001(\0132\032.NavMessage.LidarOdomStatu\022\013"
-    "\n\003key\030\005 \001(\t\022-\n\rselected_traj\030\006 \001(\0132\026.Nav"
-    "Message.Trajectory\022,\n\014predict_traj\030\007 \001(\013"
-    "2\026.NavMessage.Trajectory\022\020\n\010in_space\030\010 \001"
-    "(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobotStatu\022\t\n\001x\030"
-    "\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022&\n\010agvSt"
-    "atu\030\004 \001(\0132\024.NavMessage.AgvStatu2\302\001\n\nNavE"
-    "xcutor\0226\n\005Start\022\022.NavMessage.NavCmd\032\027.Na"
-    "vMessage.NavResponse\"\000\0227\n\006Cancel\022\022.NavMe"
-    "ssage.NavCmd\032\027.NavMessage.NavResponse\"\000\022"
-    "C\n\017ManualOperation\022\025.NavMessage.ManualCm"
-    "d\032\027.NavMessage.NavResponse\"\000b\006proto3"
+    "\030\005 \001(\002\022\017\n\002id\030\006 \001(\tH\000\210\001\001B\005\n\003_id\"/\n\nTrajec"
+    "tory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Pose2d\""
+    "\345\001\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode"
+    "\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010passNode"
+    "\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\nstreetNo"
+    "de\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tpathNo"
+    "des\030\005 \003(\0132\024.NavMessage.PathNode\022\021\n\twheel"
+    "base\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P\n\006NavCm"
+    "d\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nnewAct"
+    "ions\030\005 \003(\0132\025.NavMessage.NewAction\"(\n\013Nav"
+    "Response\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tM"
+    "anualCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operation_type\030"
+    "\002 \001(\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavStatu\022\r\n\005"
+    "statu\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mo"
+    "de\030\003 \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessage.Lida"
+    "rOdomStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselected_traj"
+    "\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014predic"
+    "t_traj\030\007 \001(\0132\026.NavMessage.Trajectory\022\020\n\010"
+    "in_space\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobo"
+    "tStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 "
+    "\001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavMessage.AgvSt"
+    "atu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMessage"
+    ".NavCmd\032\027.NavMessage.NavResponse\"\000\0227\n\006Ca"
+    "ncel\022\022.NavMessage.NavCmd\032\027.NavMessage.Na"
+    "vResponse\"\000\022C\n\017ManualOperation\022\025.NavMess"
+    "age.ManualCmd\032\027.NavMessage.NavResponse\"\000"
+    "b\006proto3"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1596,
+    1608,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -2367,6 +2375,12 @@ void Pose2d::InternalSwap(Pose2d* other) {
 
 class PathNode::_Internal {
  public:
+  using HasBits = decltype(std::declval<PathNode>()._impl_._has_bits_);
+  static constexpr ::int32_t kHasBitsOffset =
+    8 * PROTOBUF_FIELD_OFFSET(PathNode, _impl_._has_bits_);
+  static void set_has_id(HasBits* has_bits) {
+    (*has_bits)[0] |= 1u;
+  }
 };
 
 PathNode::PathNode(::PROTOBUF_NAMESPACE_ID::Arena* arena)
@@ -2378,7 +2392,9 @@ PathNode::PathNode(const PathNode& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   PathNode* const _this = this; (void)_this;
   new (&_impl_) Impl_{
-      decltype(_impl_.id_) {}
+      decltype(_impl_._has_bits_){from._impl_._has_bits_}
+    , /*decltype(_impl_._cached_size_)*/{}
+    , decltype(_impl_.id_) {}
 
     , decltype(_impl_.x_) {}
 
@@ -2389,15 +2405,14 @@ PathNode::PathNode(const PathNode& from)
     , decltype(_impl_.w_) {}
 
     , decltype(_impl_.theta_) {}
-
-    , /*decltype(_impl_._cached_size_)*/{}};
+  };
 
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
   _impl_.id_.InitDefault();
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
         _impl_.id_.Set("", GetArenaForAllocation());
   #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
-  if (!from._internal_id().empty()) {
+  if ((from._impl_._has_bits_[0] & 0x00000001u) != 0) {
     _this->_impl_.id_.Set(from._internal_id(), _this->GetArenaForAllocation());
   }
   ::memcpy(&_impl_.x_, &from._impl_.x_,
@@ -2409,7 +2424,9 @@ PathNode::PathNode(const PathNode& from)
 inline void PathNode::SharedCtor(::_pb::Arena* arena) {
   (void)arena;
   new (&_impl_) Impl_{
-      decltype(_impl_.id_) {}
+      decltype(_impl_._has_bits_){}
+    , /*decltype(_impl_._cached_size_)*/{}
+    , decltype(_impl_.id_) {}
 
     , decltype(_impl_.x_) { 0 }
 
@@ -2421,7 +2438,6 @@ inline void PathNode::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.theta_) { 0 }
 
-    , /*decltype(_impl_._cached_size_)*/{}
   };
   _impl_.id_.InitDefault();
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
@@ -2453,15 +2469,20 @@ void PathNode::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  _impl_.id_.ClearToEmpty();
+  cached_has_bits = _impl_._has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    _impl_.id_.ClearNonDefaultToEmpty();
+  }
   ::memset(&_impl_.x_, 0, static_cast<::size_t>(
       reinterpret_cast<char*>(&_impl_.theta_) -
       reinterpret_cast<char*>(&_impl_.x_)) + sizeof(_impl_.theta_));
+  _impl_._has_bits_.Clear();
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
 const char* PathNode::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) {
 #define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  _Internal::HasBits has_bits{};
   while (!ctx->Done(&ptr)) {
     ::uint32_t tag;
     ptr = ::_pbi::ReadTag(ptr, &tag);
@@ -2511,7 +2532,7 @@ const char* PathNode::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
           goto handle_unusual;
         }
         continue;
-      // string id = 6;
+      // optional string id = 6;
       case 6:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 50)) {
           auto str = _internal_mutable_id();
@@ -2538,6 +2559,7 @@ const char* PathNode::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
     CHK_(ptr != nullptr);
   }  // while
 message_done:
+  _impl_._has_bits_.Or(has_bits);
   return ptr;
 failure:
   ptr = nullptr;
@@ -2606,8 +2628,9 @@ failure:
         5, this->_internal_theta(), target);
   }
 
-  // string id = 6;
-  if (!this->_internal_id().empty()) {
+  cached_has_bits = _impl_._has_bits_[0];
+  // optional string id = 6;
+  if (cached_has_bits & 0x00000001u) {
     const std::string& _s = this->_internal_id();
     ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::VerifyUtf8String(
         _s.data(), static_cast<int>(_s.length()), ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::SERIALIZE, "NavMessage.PathNode.id");
@@ -2630,8 +2653,9 @@ failure:
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // string id = 6;
-  if (!this->_internal_id().empty()) {
+  // optional string id = 6;
+  cached_has_bits = _impl_._has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
     total_size += 1 + ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
                                     this->_internal_id());
   }
@@ -2699,7 +2723,7 @@ void PathNode::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   ::uint32_t cached_has_bits = 0;
   (void) cached_has_bits;
 
-  if (!from._internal_id().empty()) {
+  if ((from._impl_._has_bits_[0] & 0x00000001u) != 0) {
     _this->_internal_set_id(from._internal_id());
   }
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
@@ -2756,6 +2780,7 @@ void PathNode::InternalSwap(PathNode* other) {
   auto* lhs_arena = GetArenaForAllocation();
   auto* rhs_arena = other->GetArenaForAllocation();
   _internal_metadata_.InternalSwap(&other->_internal_metadata_);
+  swap(_impl_._has_bits_[0], other->_impl_._has_bits_[0]);
   ::_pbi::ArenaStringPtr::InternalSwap(&_impl_.id_, lhs_arena,
                                        &other->_impl_.id_, rhs_arena);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<

MPC/monitor/emqx/message.pb.h

@@ -1224,7 +1224,8 @@ class PathNode final :
     kWFieldNumber = 4,
     kThetaFieldNumber = 5,
   };
-  // string id = 6;
+  // optional string id = 6;
+  bool has_id() const;
   void clear_id() ;
   const std::string& id() const;
 
@@ -1302,13 +1303,14 @@ class PathNode final :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   struct Impl_ {
+    ::PROTOBUF_NAMESPACE_ID::internal::HasBits<1> _has_bits_;
+    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
     ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr id_;
     float x_;
     float y_;
     float l_;
     float w_;
     float theta_;
-    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
   union { Impl_ _impl_; };
   friend struct ::TableStruct_message_2eproto;
@@ -3570,9 +3572,14 @@ inline void PathNode::_internal_set_theta(float value) {
   _impl_.theta_ = value;
 }
 
-// string id = 6;
+// optional string id = 6;
+inline bool PathNode::has_id() const {
+  bool value = (_impl_._has_bits_[0] & 0x00000001u) != 0;
+  return value;
+}
 inline void PathNode::clear_id() {
   _impl_.id_.ClearToEmpty();
+  _impl_._has_bits_[0] &= ~0x00000001u;
 }
 inline const std::string& PathNode::id() const {
   // @@protoc_insertion_point(field_get:NavMessage.PathNode.id)
@@ -3581,7 +3588,7 @@ inline const std::string& PathNode::id() const {
 template <typename Arg_, typename... Args_>
 inline PROTOBUF_ALWAYS_INLINE void PathNode::set_id(Arg_&& arg,
                                                      Args_... args) {
-  ;
+  _impl_._has_bits_[0] |= 0x00000001u;
   _impl_.id_.Set(static_cast<Arg_&&>(arg), args..., GetArenaForAllocation());
   // @@protoc_insertion_point(field_set:NavMessage.PathNode.id)
 }
@@ -3594,20 +3601,33 @@ inline const std::string& PathNode::_internal_id() const {
   return _impl_.id_.Get();
 }
 inline void PathNode::_internal_set_id(const std::string& value) {
-  ;
+  _impl_._has_bits_[0] |= 0x00000001u;
 
 
   _impl_.id_.Set(value, GetArenaForAllocation());
 }
 inline std::string* PathNode::_internal_mutable_id() {
-  ;
+  _impl_._has_bits_[0] |= 0x00000001u;
   return _impl_.id_.Mutable( GetArenaForAllocation());
 }
 inline std::string* PathNode::release_id() {
   // @@protoc_insertion_point(field_release:NavMessage.PathNode.id)
-  return _impl_.id_.Release();
+  if ((_impl_._has_bits_[0] & 0x00000001u) == 0) {
+    return nullptr;
+  }
+  _impl_._has_bits_[0] &= ~0x00000001u;
+  auto* released = _impl_.id_.Release();
+  #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  _impl_.id_.Set("", GetArenaForAllocation());
+  #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  return released;
 }
 inline void PathNode::set_allocated_id(std::string* value) {
+  if (value != nullptr) {
+    _impl_._has_bits_[0] |= 0x00000001u;
+  } else {
+    _impl_._has_bits_[0] &= ~0x00000001u;
+  }
   _impl_.id_.SetAllocated(value, GetArenaForAllocation());
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
         if (_impl_.id_.IsDefault()) {

MPC/navigation.cpp

@@ -658,7 +658,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
     return true;
 }
 
-bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
+bool Navigation::execute_nodes(NavMessage::NewAction action) {
     if (action.type() == 3 || action.type() == 4) //最优动作导航 or 导航中保证朝向严格一致
     {
         if (!parameter_.has_nodeangularlimit() || !parameter_.has_nodevelocitylimit() || action.pathnodes_size() == 0) {
@@ -911,10 +911,9 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 }
 
 bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
-    if (spaceNode.id().find("S") < 0)
+    int space_id = GetSpaceId(spaceNode);
+    if ( space_id < 0)
         return false;
-    std::string id = spaceNode.id().substr(1, spaceNode.id().length() - 1);
-    int space_id = stoi(id);
     if (space_id > 99 && space_id < 1000) {
         return true;
     }
@@ -922,26 +921,32 @@ bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
 }
 
 int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) {
-    if (spaceNode.id().find("S") >= 0) {
-        std::string id = spaceNode.id().substr(1, spaceNode.id().length() - 1);
+    if(spaceNode.has_id()==false){
+        return -1;
+    }
+    std::string space_flag = "S_";
+    if (spaceNode.id().find(space_flag) >= 0) {
+        std::string id = spaceNode.id().substr(2, spaceNode.id().length() - 1);
         int space_id = stoi(id);
         return space_id;
-    } else {
-        return -1;
     }
+    return -1;
 }
 
 bool Navigation::TargetIsEntrance(NavMessage::PathNode node) {
-    return node.id().find("S1101") != -1;
+    int space_id = GetSpaceId(node);
+    return space_id==1101;
 }
 
 bool Navigation::TargetIsExport(NavMessage::PathNode node) {
-    return node.id().find("S1101") != -1;
+    int space_id = GetSpaceId(node);
+    return space_id==1101;
 }
 
 
 
 bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
+    printf("execute_InOut_space\n");
     if (action.type() != 1 && action.type() != 2) {
         printf(" Inout_space failed : msg action type must ==1\n ");
         return false;
@@ -970,6 +975,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
          * 是否需要添加判断，距离较远才先运动到马路点=============================
          */
         int clampLifterAction = 0;
+        printf("IsUperSpace\n");
         if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
             clampLifterAction |= eByteLifterUp;
         } else {    //否则下降
@@ -1023,6 +1029,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             } else {
                 if(!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode())) {
                     printf(" Wait Carrier up.......\n");
+
                     if (WaitCarrierCompleted(GetSpaceId(action.spacenode()), eCarrierUp) == false) {
                         return false;
                     }
@@ -1293,10 +1300,9 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
     double l = fabs(targetNode.l());
     double w = fabs(targetNode.w());
     double theta = -targetNode.theta();
-    if (newUnfinished_cations_.front().type() == 1 && targetNode.id().find('S') != -1) {//入库只判断前后方向上是否到达
+    /*if (newUnfinished_cations_.front().type() == 1 && GetSpaceId(targetNode) > 0) {//入库只判断前后方向上是否到达
         tx = x;
-    }
-
+    }*/
     if (l < 1e-10 || w < 1e-10) {
         printf("IsArrived: Error target l or w == 0\n");
         return false;
@@ -1552,9 +1558,13 @@ bool Navigation::WaitInsideDoorCompleted(int doorID, DoorStatu status) {
 
 bool Navigation::WaitCarrierCompleted(int spaceID, CarrierStatu statu) {
     while (cancel_ == false) {
-        int region_id=-1, carrier_no=-1;
+        int region_id=0, carrier_no=0;
         SpaceNo2CarrierNo(spaceID, region_id, carrier_no);
+        if(region_id<0){
+            return true;
+        }
         if (timed_Carrier_[region_id].timeout()) {
+            printf("spaceID:%d, timed_Carrier_[%d].timeout  carrier_no id:%d\n",spaceID,region_id,carrier_no);
             return false;
         }
 
@@ -1717,7 +1727,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
 //        out[1] *= 10;
 
         //入库，行进方向上最后一米不纠偏
-        if (newUnfinished_cations_.front().type() == 1 && node.id().find('S') != -1) {//入库
+        if (newUnfinished_cations_.front().type() == 1 && GetSpaceId(node) > 0) {//入库
             if (fabs(target_in_agv.x()) < 0.5) {
                 printf("target_in_agv: %f", target_in_agv.x());
                 for (double &i: down_w) {
@@ -1742,11 +1752,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         //printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
         //添加车位号,距目标点距离信息
         double distance = Pose2d::distance(target_in_agv, Pose2d(0, 0, 0));
-        int space_id = 0;
-        if (node.id().find('S') != -1) {
-            std::string id = node.id().substr(1, node.id().length() - 1);
-            space_id = stoi(id);
-        }
+        int space_id = GetSpaceId(node);
         double Y1 = 0.0, Y2 = 0.0;
         if (move_mode_ == eDouble) {
             double dy = -timeYawDiff1_.Get();
@@ -1820,11 +1826,11 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
         return false;
     }
     Pose2d current = timedPose_.Get();
-    if (targetNode.id().find('S') != -1) {
-        //车位点强制可到达
-        return true;
-    }
-
+//    if (GetSpaceId(targetNode) > 0) {
+//        printf("车位点强制可到达\n");
+//        //车位点强制可到达
+//        return true;
+//    }
     double tx = targetNode.x();
     double ty = targetNode.y();
     double l = fabs(targetNode.l());
@@ -1911,38 +1917,39 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     running_ = true;
     actionType_ = eReady;
     printf("Navigation beg...\n");
-
+    bool ret=false;
     while (newUnfinished_cations_.empty() == false && cancel_ == false) {
         std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
         NavMessage::NewAction act = newUnfinished_cations_.front();
         NavMessage::NewAction last_act = newUnfinished_cations_.back();
         if (act.type() == 1) {  //入库
+            printf("入库\n");
             space_id_ = act.spacenode().id();
             obs_w_ = 1.1;
             obs_h_ = 1.87;
-            if (!execute_InOut_space(act)) {
+            ret=execute_InOut_space(act);
+            if (!ret) {
                 printf(" In space failed\n");
                 isInSpace_ = false;
                 break;
             }
         } else if (act.type() == 2) {  //出库
+            printf("出库\n");
             obs_w_ = 1.25;
             obs_h_ = 1.87;
-            if (!execute_InOut_space(act)) {
+            ret=execute_InOut_space(act);
+            if (!ret) {
                 printf(" out space failed\n");
                 break;
             } else {
                 isInSpace_ = false;
             }
         } else if (act.type() == 3 || act.type() == 4) { //马路导航
+            printf("马路导航\n");
             obs_w_ = 1.1;
             obs_h_ = 1.87;
-            int space_id = -1;
-            if (last_act.type() == 1) {
-                std::string id = last_act.spacenode().id().substr(1, last_act.spacenode().id().length() - 1);
-                space_id = stoi(id);
-            }
-            if (!execute_nodes(act, space_id)) {
+            ret=execute_nodes(act);
+            if (!ret) {
                 printf(" street nav failed\n");
                 break;
             }
@@ -1950,7 +1957,8 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             printf(" 汽车模型导航....\n");
 
         } else if (act.type() == 6) {//夹持
-            if (this->clamp_close() == false) {
+            ret=clamp_close();
+            if (ret== false) {
                 printf("夹持failed ...\n");
                 break;
             }
@@ -1961,13 +1969,16 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             }
         } else if (act.type() == 8) { //切换模式
             SwitchMode(act.changedmode(), act.wheelbase());
+            ret=true;
         } else if (act.type() == 9) { //提升机构提升
-            if (this->lifter_rise() == false) {
+            ret=lifter_rise();
+            if (ret == false) {
                 printf("提升failed ...\n");
                 break;
             }
         } else if (act.type() == 10) { //提升机构下降
-            if (this->lifter_down() == false) {
+            ret=lifter_down();
+            if (ret == false) {
                 printf("下降failed ...\n");
                 break;
             }
@@ -1979,7 +1990,14 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     }
     actionType_ = eReady;
     Stop();
-    if (cancel_ == true) {
+    if(ret==false){
+        response.set_ret(-4);
+        response.set_info("navigation ret false");
+        printf(" navigation ret false\n");
+        while (newUnfinished_cations_.empty() == false)
+            newUnfinished_cations_.pop();
+    }
+    else if (cancel_ == true) {
         response.set_ret(-3);
         response.set_info("navigation canceled");
         printf(" navigation canceled\n");
@@ -2001,9 +2019,10 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     running_ = false;
 }
 
-void Navigation::SwitchMode(int mode, float wheelBase) {
+bool Navigation::SwitchMode(int mode, float wheelBase) {
     wheelBase_ = wheelBase;
     printf("Child AGV can not Switch Mode\n");
+    return true;
 }
 
 float Navigation::compute_cuv(const Pose2d &target) {
@@ -2223,6 +2242,8 @@ int Navigation::GetPortIDBySpace(NavMessage::PathNode node) {
     else if (port_id == 1102){
         return 2;
     }
+
+    return -1;
 }
 
 

MPC/navigation.h

@@ -114,7 +114,7 @@ public:
 
     bool SlowlyStop();
 
-    virtual void SwitchMode(int mode, float wheelBase);
+    virtual bool SwitchMode(int mode, float wheelBase);
 
     bool PoseTimeout();
     bool WaitClampLifterStatu(int clampLifterAction);
@@ -178,7 +178,7 @@ protected:
      * 按照路径点导航
      */
 
-    bool execute_nodes(NavMessage::NewAction action,int next_actionTypeID=-1);
+    bool execute_nodes(NavMessage::NewAction action);
 
     /*
      * 进出库
@@ -210,7 +210,7 @@ protected:
 
     virtual bool singleClamp_fullyOpen_until_inspaceBegin();
 
-    static bool IsUperSpace(NavMessage::PathNode spaceNode);
+    bool IsUperSpace(NavMessage::PathNode spaceNode);
 
     virtual int GetSpaceId(NavMessage::PathNode spaceNode);
 

MPC/navigation_main.h

@@ -26,15 +26,15 @@ public:
 
     virtual void HandleAgvStatu(const MqttMsg &msg);
 
-    virtual void SwitchMode(int mode, float wheelBase) {
+    virtual bool SwitchMode(int mode, float wheelBase) {
         printf("change mode to:%d\n", mode);
         wheelBase_ = wheelBase;
         if (move_mode_ == mode)
-            return;
+            return true;
         if (mode == eDouble) {
             if (timedBrotherPose_.timeout() == true || timedPose_.timeout() == true) {
                 std::cout << "Brother/self pose is timeout can not set MainAGV pose" << std::endl;
-                return;
+                return false;
             }
             //Pose2d transform(-wheelBase_/2.0,0,0);
             //Navigation::ResetPose(pose * transform);
@@ -44,7 +44,7 @@ public:
 
             if (diff.x() > 3.4 || diff.x() < 2.3 || diff.y() > 0.2 || diff.theta() > 5 * M_PI / 180.0) {
                 std::cout << " distance with two agv is too far diff: " << diff << std::endl;
-                return;
+                return false;
             }
 
             Pose2d self_pose = timedPose_.Get();
@@ -64,6 +64,7 @@ public:
         }
 
         move_mode_ = mode;
+        return true;
     };
 
 protected:

config/SpaceNO2CarrarierNO.txt

@@ -1,3 +1,4 @@
+65 -1 2
 101 2 1
 103 2 2
 105 2 3
@@ -16,6 +17,8 @@
 115 7 2
 117 7 3
 119 7 4
+
+
 121 7 5
 123 7 6
 

message.proto

@@ -54,7 +54,7 @@ message PathNode //导航路径点及精度
     float l = 3;    //目标点旋转矩形方程，代表精度范围
     float w = 4;
     float theta = 5;
-    string id = 6;
+    optional string id = 6;
 }
 
 message Trajectory