1.添加指令流程类型（存取车）

2.上位机NavCmd添加流程信息，同时优化存车流程离开入口，掉头动作触发条件

MPC/custom_type.h

@@ -127,6 +127,16 @@ enum eClamLiftActionType{
 };
 //////////////////////////////////////////////////
 
+//////////////////////////////////////////////////
+/// 指令流程类型
+enum eSequenceType{
+    eSequenceDefault = 0,
+    eSequencePark = 1,
+    eSequencePick = 2
+};
+//////////////////////////////////////////////////
+
+
 //////////////////////////////////////////////////
 /// plc区域+编号载车板 对应 车位表
 enum eRegionId{

MPC/navigation.cpp

@@ -789,6 +789,7 @@ Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
 }
 
 Navigation::MpcResult Navigation::RotateAfterOutExport() {
+    printf("RotateAfterOutExport...\n");
     if (move_mode_ != eDouble)
         return eMpcSuccess;
     Pose2d init = timedPose_.Get();
@@ -1213,7 +1214,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         isInSpace_ = false;
 
         // 存车流程离开入口，掉头
-        if (GetRemainActionsCount() > 1) {
+        if (GetSequenceType() == eSequencePark) {
             if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
                 Pose2d current_pose = timedPose_.Get();
                 double current_theta = current_pose.theta();
@@ -2472,4 +2473,13 @@ NavMessage::NewAction Navigation::GetLastAction() {
     return newUnfinished_cations_.back();
 }
 
+int Navigation::GetSequenceType() {
+    if (global_navCmd_.has_sequence()){
+        return eSequenceDefault;
+    }
+    else{
+        return global_navCmd_.sequence();
+    }
+}
+
 

MPC/navigation.h

@@ -126,6 +126,9 @@ public:
 
     int GetPortIDBySpace(NavMessage::PathNode node);
 
+    //获取当前流程是出/入库
+    int GetSequenceType();
+
     // 获取当前未完成动作数量
     virtual int GetRemainActionsCount();
     virtual NavMessage::NewAction GetCurrentAction();

MPC/navigation_main.cpp

@@ -27,7 +27,7 @@ void NavigationMain::ResetPose(const Pose2d &pose) {
         Pose2d brother = timedBrotherPose_.Get();
         Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));
 
-        if (diff.x() > 3.6 || diff.x() < 2.2 || diff.y() > 0.5 || diff.theta() > 5 * M_PI / 180.0) {
+        if (diff.x() > 3.6 || diff.x() < 2.2 || diff.y() > 0.5 || diff.theta() > 10 * M_PI / 180.0) {
             std::cout << " distance with two agv is too far diff: " << diff << std::endl;
             return;
         }

message.proto

@@ -81,6 +81,7 @@ message NavCmd
     int32 action = 1;  //  0 开始任务，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,
     string key = 2;
     repeated NewAction newActions = 5;
+    optional int32 sequence = 6; // 0：无， 1：存车， 2：取车
 }
 
 message NavResponse {