MPC/custom_type.h

@@ -98,7 +98,7 @@ enum ActionType {
     eHorizontal = 2,
     eVertical = 3,
     eClampClose = 5,
-    eClampOpen = 6,
+    eClampHalfOpen = 6,
     eLifterRise = 7,    //提升机构上升
     eLifterDown =8,
     eClampFullyOpen = 9

MPC/monitor/monitor_emqx.cpp

@@ -24,6 +24,11 @@ void Monitor_emqx::set_speedcmd_topic(std::string speedcmd)
   speedcmd_topic_=speedcmd;
 }
 
+void Monitor_emqx::set_clampLifterCmd_topic(std::string topic)
+{
+    clampLifterCmd_topic_=topic;
+}
+
 void Monitor_emqx::clamp_close(int mode)
 {
   MqttMsg msg;
@@ -35,21 +40,21 @@ void Monitor_emqx::clamp_close(int mode)
   speed.set_m1(mode);
   speed.set_end(1);
   msg.fromProtoMessage(speed);
-  Publish(speedcmd_topic_,msg);
+  Publish(clampLifterCmd_topic_,msg);
 }
 
-void Monitor_emqx::clamp_open(int mode)
+void Monitor_emqx::clamp_half_open(int mode)
 {
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
   speed.set_h1(heat_);
-  speed.set_t1(eClampOpen);
+  speed.set_t1(eClampHalfOpen);
 
   speed.set_m1(mode);
   speed.set_end(1);
   msg.fromProtoMessage(speed);
-  Publish(speedcmd_topic_,msg);
+  Publish(clampLifterCmd_topic_,msg);
 }
 
 void Monitor_emqx::clamp_fully_open(int mode)
@@ -63,7 +68,7 @@ void Monitor_emqx::clamp_fully_open(int mode)
     speed.set_m1(mode);
     speed.set_end(1);
     msg.fromProtoMessage(speed);
-    Publish(speedcmd_topic_,msg);
+    Publish(clampLifterCmd_topic_,msg);
 }
 
 void Monitor_emqx::lifter_rise(int mode)
@@ -77,7 +82,7 @@ void Monitor_emqx::lifter_rise(int mode)
     speed.set_m1(mode);
     speed.set_end(1);
     msg.fromProtoMessage(speed);
-    Publish(speedcmd_topic_,msg);
+    Publish(clampLifterCmd_topic_,msg);
 }
 
 void Monitor_emqx::lifter_down(int mode)
@@ -91,7 +96,7 @@ void Monitor_emqx::lifter_down(int mode)
     speed.set_m1(mode);
     speed.set_end(1);
     msg.fromProtoMessage(speed);
-    Publish(speedcmd_topic_,msg);
+    Publish(clampLifterCmd_topic_,msg);
 }
 
 void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a,double L)
@@ -159,7 +164,8 @@ void Monitor_emqx::stop()
   Publish(speedcmd_topic_,msg);
 }
 
-void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[], double L, int P, double D) {
+void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[],
+                                double L, int P, double D,double Y1,double Y2) {
 //    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
 //    {
 //        printf(" Main agv mpc must set wheelBase\n ");
@@ -172,6 +178,10 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
         w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
         velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
     }
+
+    double yaw1 =fabs(Y1)>0.0001?Y1:0.0;
+    double yaw2 =fabs(Y2)>0.0001?Y2:0.0;
+
     MqttMsg msg;
     NavMessage::ToAgvCmd agvCmd;
     heat_=(heat_+1)%255;
@@ -184,6 +194,8 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
     agvCmd.set_w2(w_correct[1]);
     agvCmd.set_v3(velocity[2]);
     agvCmd.set_w3(w_correct[2]);
+    agvCmd.set_y1(yaw1);
+    agvCmd.set_y2(yaw2);
     agvCmd.set_l1(L);
     agvCmd.set_p1(P);
     agvCmd.set_d1(D);

MPC/monitor/monitor_emqx.h

@@ -15,12 +15,14 @@ class Monitor_emqx : public Terminator_emqx
     Monitor_emqx(std::string nodeId);
     ~Monitor_emqx();
     void set_speedcmd_topic(std::string speedcmd);
+    void set_clampLifterCmd_topic(std::string topic);
     void set_speed(int mode,ActionType type,double v,double a);
     void set_speed(int mode,ActionType type,double v,double a,double L);
-    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double L=0,int P=0,double D=0);
+    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],
+                      double L=0,int P=0,double D=0,double Y1=0.,double Y2=0.);
 //    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double yaw_diff[], double L=0,int P=0,double D=0);
     void clamp_close(int mode);
-    void clamp_open(int mode);
+    void clamp_half_open(int mode);
     void clamp_fully_open(int mode);
     void lifter_rise(int mode);
     void lifter_down(int mode);
@@ -29,6 +31,7 @@ class Monitor_emqx : public Terminator_emqx
  protected:
 
     std::string speedcmd_topic_;
+    std::string clampLifterCmd_topic_;
 
  public:
     int heat_=0;

MPC/monitor/terminator_emqx.cpp

@@ -29,10 +29,14 @@ bool Terminator_emqx::Connect(std::string ip,int port)
 
 void Terminator_emqx::Publish(std::string topic,const MqttMsg& msg)
 {
-  if(client_)
-    client_->publish(topic,1,msg);
-  else
-    printf("publish  failed : emqx client maybe disconnected...\n");
+  if(client_) {
+      mtx_.lock();
+      client_->publish(topic, 1, msg);
+      mtx_.unlock();
+  }
+  else {
+      printf("publish  failed : emqx client maybe disconnected...\n");
+  }
 }
 
 bool Terminator_emqx::AddCallback(std::string topic,Callback callback,void* context)

MPC/navigation.cpp

@@ -80,6 +80,7 @@ bool Navigation::Init(const NavParameter::Navigation_parameter &parameter) {
             return false;
         }
         monitor_->set_speedcmd_topic(agv_p.pubspeedtopic());
+        monitor_->set_clampLifterCmd_topic(agv_p.pubclampliftertopic());
         monitor_->AddCallback(agv_p.subposetopic(), Navigation::RobotPoseCallback, this);
         monitor_->AddCallback(agv_p.subspeedtopic(), Navigation::RobotSpeedCallback, this);
     }
@@ -234,7 +235,6 @@ bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
             plc.set_w(timedA_.Get());
             plc.set_clamp(timed_clamp_.Get());
             plc.set_lifter(timed_lifter_.Get());
-            //printf(" timed_clamp_:%d\n ",timed_clamp_.Get());
             robotStatu.mutable_agvstatu()->CopyFrom(plc);
         }
         return true;
@@ -257,17 +257,17 @@ void Navigation::ResetLifter(LifterStatus status) {
 
 void Navigation::ResetPose(const Pose2d &pose) {
     timedPose_.reset(pose, 1.0);
-    if(pose.theta()>0){
-        if(parameter_.main_agv()){
-            isFront_.reset(true,1.0);
-        }else{
-            isFront_.reset(false,1.0);
+    if (pose.theta() > 0) {
+        if (parameter_.main_agv()) {
+            isFront_.reset(true, 1.0);
+        } else {
+            isFront_.reset(false, 1.0);
         }
-    }else{
-        if(parameter_.main_agv()){
-            isFront_.reset(false,1.0);
-        }else{
-            isFront_.reset(true,1.0);
+    } else {
+        if (parameter_.main_agv()) {
+            isFront_.reset(false, 1.0);
+        } else {
+            isFront_.reset(true, 1.0);
         }
     }
 
@@ -331,13 +331,14 @@ bool Navigation::SlowlyStop() {
 
 
 Navigation::Navigation() {
+    wheelBase_ = 0;
     isInSpace_ = false; //是否在车位或者正在进入车位
     RWheel_position_ = eUnknow;
     move_mode_ = eSingle;
     monitor_ = nullptr;
     terminator_ = nullptr;
-    obs_h_=4.0;
-    obs_w_=2.0;
+    obs_h_ = 4.0;
+    obs_w_ = 2.0;
     timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5);
 }
 
@@ -425,6 +426,49 @@ bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate,
     return false;
 }
 
+
+bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate) {
+    std::cout<<"Adjust to : "<<target<<std::endl;
+    if (inited_ == false) {
+        printf(" MPC_rotate has not inited\n");
+        return false;
+    }
+    if (PoseTimeout()) {
+        printf(" MPC_rotate Error:Pose is timeout \n");
+        return false;
+    }
+
+    Pose2d current = timedPose_.Get();
+    NavMessage::PathNode yawTarget;
+    yawTarget.set_x(current.x() + 100.);
+    yawTarget.set_y(current.y());
+    int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+    std::cout<<"Adjust RotateRefer : "<<Pose2d(yawTarget.x(),yawTarget.y(),0)<<std::endl;
+    RotateReferToTarget(yawTarget, limit_rotate, directions);
+
+    current = timedPose_.Get();
+    Pose2d diff=target-current;
+    Pose2d newTarget;
+    NavMessage::PathNode node;
+    if(fabs(diff.x())>fabs(diff.y())){
+        newTarget=Pose2d(current.x(),target.y(),current.theta());
+        node.set_theta(M_PI/2.0);
+    }else{
+        newTarget=Pose2d(target.x(),current.y(),current.theta());
+        node.set_theta(0);
+    }
+    node.set_x(newTarget.x());
+    node.set_y(newTarget.y());
+    node.set_l(0.03);
+    node.set_w(0.2);
+
+    stLimit limitV={0.03,0.5};
+    stLimit limitW={0.03,0.1};
+    std::cout<<" Adjust MoveTarget:"<<Pose2d(node.x(),node.y(),node.theta())<<std::endl;
+    return MoveToTarget(node,limitV,limitW,true,true);
+
+}
+
 Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node, stLimit limit_rotate, int directions) {
     if (inited_ == false) {
         printf(" MPC_rotate has not inited\n");
@@ -501,9 +545,9 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
             printf(" MPC_rotate | refer target completed\\n,cuv:%f\n", target_yaw_diff);
             Stop();
             return eMpcSuccess;
-        } else{
+        } else {
             const int down_count = 3;
-            double v[down_count] = {0,0,0};
+            double v[down_count] = {0, 0, 0};
             double w[down_count] = {out[0], out[1], out[2]};
 //            if(out[0]>=0){
 //                w[0]=1.*M_PI/180.0;
@@ -519,22 +563,10 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
             double input_w = timedA_.Get();
             for (int i = 0; i < down_count; ++i) {
                 printf("  MPC_rotate |P[%d], input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n",
-                       i, input_w, w[i], w[i]/M_PI*180, target_yaw_diff, directions);
+                       i, input_w, w[i], w[i] / M_PI * 180, target_yaw_diff, directions);
             }
         }
         continue;
-
-//        //下发速度
-//        if (fabs(target_yaw_diff) > 2 * M_PI / 180.0) {
-//            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
-//            actionType_ = eRotation;
-//            printf(" MPC_rotate | input angular_v:%f, down:%f, diff:%f autoDirect:%d\n",
-//                   timedA_.Get(), out[0], target_yaw_diff, directions);
-//        } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
-//            printf(" MPC_rotate refer target completed,cuv:%f\n", target_yaw_diff);
-//            return eMpcSuccess;
-//        }
-//        continue;
     }
     return eMpcFailed;
 }
@@ -565,24 +597,31 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
             enableTotarget = x_enable;
         }
         //巡线无法到达目标点
-        if (enableTotarget == false) {
-            if (already_in_mpc && enable_rotate == false) {
+        Pose2d target(node.x(), node.y(), 0);
+        Pose2d targetInCurrent = Pose2d::relativePose(target, current);
+
+        if (enableTotarget == false ) {
+            bool adjustRet=AdjustReferToTarget(target,limit_w);
+            if(adjustRet==false)
+                return false;
+            /*if (already_in_mpc && enable_rotate == false) {
                 printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
                 return false;
             }
-            Pose2d target(node.x(), node.y(), 0);
-//        if (RotateReferToTarget(target, limit_w, anyDirect) == false) {
+
             int directions = Direction::eForward;
             if (anyDirect)
                 directions = Direction::eForward | Direction::eBackward |
                              Direction::eLeft | Direction::eRight;
+
             if (RotateReferToTarget(node, limit_w, directions) != eMpcSuccess) {
                 std::cout << " Rotate refer to target failed,target: " << target <<
                           " directions: " << std::hex << directions << " line_: " << __LINE__ << std::endl;
                 return false;
             }
-            continue;
+            continue;*/
         }
+
         already_in_mpc = true;
         MpcResult ret = MpcToTarget(node, limit_v, anyDirect);
         if (ret == eMpcSuccess) {
@@ -602,7 +641,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
     return true;
 }
 
-bool Navigation::execute_nodes(NavMessage::NewAction action) {
+bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
     if (action.type() == 3 || action.type() == 4) //最优动作导航 or 导航中保证朝向严格一致
     {
         if (!parameter_.has_nodeangularlimit() || !parameter_.has_nodevelocitylimit() || action.pathnodes_size() == 0) {
@@ -615,55 +654,66 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
         stLimit wmg_limit = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
         stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
 
+        /////////////////////////////////////////
+
+        if(move_mode_==eSingle) {
+            clamp_half_open();
+        }
+        ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
             NavMessage::PathNode node = action.pathnodes(i);
             if (i + 1 < action.pathnodes_size()) {
                 NavMessage::PathNode next = action.pathnodes(i + 1);
                 Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
-                node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
-                node.set_l(0.05);
-                node.set_w(0.10);
-            } else {
-                node.set_theta(0);
+                node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
                 node.set_l(0.03);
-                node.set_w(0.10);
+                node.set_w(0.20);
+            } else {
+                NavMessage::PathNode befor = action.pathnodes(i - 1);
+                Pose2d vec(node.x() - befor.x(), node.y() - befor.y(), 0);
+                node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
+                node.set_l(0.02);
+                node.set_w(0.2);
             }
             int directions = Direction::eForward;
             if (anyDirect)
                 directions = Direction::eForward | Direction::eBackward |
                              Direction::eLeft | Direction::eRight;
-            if (i > 0){
-                if (RotateReferToTarget(node, wmg_limit, directions) != eMpcSuccess){
+            /*if (i > 0) {
+                if (RotateReferToTarget(node, wmg_limit, directions) != eMpcSuccess) {
                     printf(" before move node  RotateReferToNone Failed!!\n");
                     return false;
                 }
-            }
+            }*/
             if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true) == false)
                 return false;
             else
                 isInSpace_ = false;
         }
+        if (cancel_ == true) {
+            return false;
+        }
+
         return true;
     }
+
     printf("execute PathNode failed ,action type is not 3/4  :%d\n", action.type());
     return false;
 }
 
-Navigation::MpcResult Navigation::RotateAfterOutSpace(){
-    if(move_mode_!=eDouble)
+Navigation::MpcResult Navigation::RotateAfterOutSpace() {
+    if (move_mode_ != eDouble)
         return eMpcSuccess;
     Pose2d init = timedPose_.Get();
-    double diff1=fabs(M_PI/2.0-init.theta());
-    double diff2=fabs(-M_PI/2.0-init.theta());
-    double target= diff1<diff2?M_PI/2.0:-M_PI/2.0;
+    double diff1 = fabs(M_PI / 2.0 - init.theta());
+    double diff2 = fabs(-M_PI / 2.0 - init.theta());
+    double target = diff1 > diff2 ? M_PI / 2.0 : -M_PI / 2.0;
     stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         Pose2d current = timedPose_.Get();
         double yawDiff = (target - current.theta());
 
-
-
         //一次变速
         std::vector<double> out;
         bool ret;
@@ -680,17 +730,16 @@ Navigation::MpcResult Navigation::RotateAfterOutSpace(){
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             return eMpcSuccess;
-        } else{
+        } else {
             const int down_count = 3;
-            double v[down_count] = {0,0,0};
+            double v[down_count] = {0, 0, 0};
             double w[down_count] = {out[0], out[1], out[2]};
             SendMoveCmd(move_mode_, eRotation, v, w);
             actionType_ = eRotation;
             printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
-                   timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
+                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
         }
         continue;
-
     }
     return eMpcFailed;
 }
@@ -708,22 +757,22 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
     stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
     double acc_angular = 20 * M_PI / 180.0;
     double dt = 0.1;
-    Pose2d rotated = timedPose_.Get();
+    Pose2d current = timedPose_.Get();
     double x = space.x();
     double y = space.y();
-    Pose2d vec(x - rotated.x(), y - rotated.y(), 0);
-    rotated.mutable_theta() = Pose2d::vector2yaw(vec.x(), vec.y());
-    bool isFront=isFront_.Get();
-    printf(" front__:%d  theta:%f\n",isFront,rotated.theta());
-    if(rotated.theta()<0.)
-        isFront=!isFront;
-
-    //前车先到,后车进入2点,保持与前车一致的朝向
-    if (isFront==false ) {
+    Pose2d vec(x - current.x(), y - current.y(), 0);
+    double vecTheta = Pose2d::vector2yaw(vec.x(), vec.y());
+    bool isFront = isFront_.Get();
+    printf(" front__:%d  theta:%f\n", isFront, vecTheta);
+    if (vecTheta < 0.)
+        isFront = !isFront;
+
+    //如果是后车，计算目标点（车位点-轴距）
+    if (isFront == false) {
         printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
         if (move_mode_ == eSingle) {
-            x -= wheelbase * cos(rotated.theta());
-            y -= wheelbase * sin(rotated.theta());
+            x -= wheelbase * cos(vecTheta);
+            y -= wheelbase * sin(vecTheta);
             printf("确定车位点：eSingle\n");
         }
     } else { //当后车先到,倒车入库
@@ -731,16 +780,17 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
         //rotated.mutable_theta() = space.theta();
         //rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
-    if(rotated.theta()<0.)
-        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
+
+    Pose2d spaceInCurrent = Pose2d::relativePose(Pose2d(space.x(), space.y(), 0), current);
+
 
     target.set_x(x);
     target.set_y(y);
-    target.set_theta(rotated.theta());
+    target.set_theta(current.theta());
     target.set_l(0.02);
     target.set_w(0.05);
     target.set_id(space.id());
-    printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
+    printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n", x, y, target.theta());
 
 //    //整车协调的时候，保持前后与车位一致即可
 //    if (move_mode_ == eDouble) {
@@ -753,8 +803,10 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
 
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(30));
-        Pose2d current = timedPose_.Get();
-        double yawDiff = (rotated - current).theta();
+        Pose2d current_now = timedPose_.Get();
+        if (spaceInCurrent.x() < 0.)
+            current_now = current_now.rotate(current_now.x(), current_now.y(), M_PI);
+        double yawDiff = vecTheta - current_now.theta();
 
         //一次变速
         std::vector<double> out;
@@ -772,14 +824,14 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             return eMpcSuccess;
-        } else{
+        } else {
             const int down_count = 3;
-            double v[down_count] = {0,0,0};
+            double v[down_count] = {0, 0, 0};
             double w[down_count] = {out[0], out[1], out[2]};
             SendMoveCmd(move_mode_, eRotation, v, w);
             actionType_ = eRotation;
             printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
-                   timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
+                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
         }
         continue;
 
@@ -811,14 +863,16 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     if (action.type() == 1) {
 //        LogWriter("In space beg-");
 
-        Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
+        Pose2d vec(action.spacenode().x() - action.streetnode().x(),
+                   action.spacenode().y() - action.streetnode().y(),
                    0);
         action.mutable_streetnode()->set_l(0.02);
-        action.mutable_streetnode()->set_w(0.05);
-        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
+        action.mutable_streetnode()->set_w(0.2);
+        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y())+M_PI/2.0);
         /*
-         * 是否需要添加判断，距离较远才先运动到马路点==============================？？？？？？？？？
+         * 是否需要添加判断，距离较远才先运动到马路点=============================
          */
+
         if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true) == false) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
@@ -827,55 +881,57 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         printf("inout ----------------------------------------\n");
         //计算车位朝向
         action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
-        //计算当前车是进入车位1点还是2点
-        NavMessage::PathNode new_target;
-        if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcFailed) {
-            return false;
-        }
-
-        std::string id = action.spacenode().id().substr(1,action.spacenode().id().length()-1);
-        int space_id = stoi(id);
-        if (space_id>99 && space_id< 1000){
-            //进载车板库前提升机构上升
-            if (lifter_rise() == false){
-                printf(" Enter space | lifter rise failed. line:%d\n",__LINE__);
-            }
+        //计算当前车是进入车位1点还是2点,    提升下降动作与旋转调整并行
+        if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
+            lifter_rise();
+        } else {    //否则下降
+            lifter_down();
         }
-        else{
-            //否则下降
-            if (lifter_down() == false){
-                printf(" Enter space | lifter down failed. line:%d\n",__LINE__);
-            }
+        NavMessage::PathNode new_target;
+        bool retRotate=RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcSuccess;
 
+        if(retRotate==false){
+            return false;
         }
 
-        if (space_id == 1101){
-            //单车进入口搬车，夹持杆打开到全开位
-
-        }
         //入库
         Pose2d rotated = timedPose_.Get();
         double x = new_target.x();
         double y = new_target.y();
         Pose2d vecSpace(x - rotated.x(), y - rotated.y(), 0);
         rotated.mutable_theta() = Pose2d::vector2yaw(vecSpace.x(), vecSpace.y());
-        bool isFront=isFront_.Get();
-        if(rotated.theta()<0.)
-            isFront=!isFront;
-        if(isFront==false && move_mode_==eSingle){
-            while(timedBrotherNavStatu_.Get().in_space()==false){
-                if(cancel_){
+        bool isFront = isFront_.Get();
+        if (rotated.theta() < 0.)
+            isFront = !isFront;
+        // 后进车，且单车，且先进车已过后进车的目标点
+        Pose2d target(new_target.x(),new_target.y(),0);
+        if (isFront == false && move_mode_ == eSingle) {
+            while(true) {
+                if (cancel_) {
                     return false;
                 }
-                printf(" 后车 等待 前车先入库。。。\n");
-                usleep(1000*500);
+                Pose2d current=timedPose_.Get();
+                Pose2d brother_pose = timedBrotherPose_.Get();
+                Pose2d vecCurent2T=current-target;
+                Pose2d vecBrother2T=brother_pose-target;
+                double dot=vecCurent2T.x()*vecBrother2T.x()+vecCurent2T.y()*vecBrother2T.y();
+                if (timedBrotherNavStatu_.Get().in_space() == true && dot<0.) {
+                        printf("先进车已经过后进车目标点，后进车停止等待 dot=%f\n",dot);
+                        break;
+                }
+                printf(" 后车 等待 先车先入库。。。\n");
+                usleep(1000 * 500);
                 continue;
             }
         }
-        isInSpace_=true;
+        isInSpace_ = true;
         printf("Enter space beg...\n");
-        if (MoveToTarget(new_target, limit_v, limit_w, true, false) == false) {
-            printf(" Enter space:%s failed. type:%d\n",action.spacenode().id().data(), action.type());
+        if(move_mode_==eSingle)
+            clamp_half_open();  //////// 后续再改为全打开位
+        bool retMove=MoveToTarget(new_target, limit_v, limit_w, true, false);
+
+        if(retMove==false){
+            printf(" Enter space:%s failed. type:%d\n", action.spacenode().id().data(), action.type());
             return false;
         }
     } else if (action.type() == 2) {
@@ -886,34 +942,33 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         float theta = Pose2d::vector2yaw(vec.x(), vec.y());
         Pose2d space(action.spacenode().x(), action.spacenode().y(), theta);
         Pose2d current = timedPose_.Get();
-        Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(),current.y(),0),space));
+        Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(), current.y(), 0), space));
         if (diff.x() < 0.05 || diff.y() < 0.10) {
             //出库，移动到马路点，允许自由方向，不允许中途旋转
             action.mutable_streetnode()->set_theta(theta);
-            action.mutable_streetnode()->set_l(0.04);
-            action.mutable_streetnode()->set_w(0.06);
+            action.mutable_streetnode()->set_l(0.03);
+            action.mutable_streetnode()->set_w(0.2);
             std::cout << " Out space target street node:" << space << std::endl;
             if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, false) == false) {
                 printf(" out space failed type:%d: MoveTo StreetNode failed\n", action.type());
                 return false;
             }
-
-            //出库后提升机构下降
-            if (lifter_down() == false){
-                printf(" Out space | lifter down failed. line:%d\n",__LINE__);
-            }
-//            if(RotateAfterOutSpace()!=eMpcSuccess) {
-//                printf(" after out space rotate Failed!!!\n");
-//                return false;
-//            }
-//            LogWriter("Out space end-");
         } else {
             std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff
                       << std::endl;
             return false;
         }
-        isInSpace_=false;
-
+        isInSpace_ = false;
+        lifter_down();
+        //从入口出库旋转180
+        if (move_mode_ == eDouble && action.spacenode().id().find("S1101") != -1) {
+            Pose2d current_pose = timedPose_.Get();
+            double current_theta = current_pose.theta();
+            if (RotateAfterOutSpace() != eMpcSuccess) {
+                printf("Rotate after out entrance failed\n");
+                return false;
+            }
+        }
     }
     return true;
 }
@@ -1012,8 +1067,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     Trajectory optimize_trajectory;
     Trajectory selected_trajectory;
     NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
-    double obs_w =obs_w_;// 0.95;//0.85;
-    double obs_h =obs_h_;// 1.55;//1.45;
+    double obs_w = obs_w_;// 0.95;//0.85;
+    double obs_h = obs_h_;// 1.55;//1.45;
 
     if (directY == true) {
         //将车身朝向旋转90°，车身朝向在(-pi,pi]
@@ -1030,7 +1085,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     if (directY)
         relative = Pose2d::relativePose(brother.rotate(brother.x(), brother.y(), M_PI / 2),
                                         pose.rotate(pose.x(), pose.y(), M_PI / 2));//计算另一节在当前小车坐标系下的坐标
-    std::cout<<"pose:"<<pose<<",  brother:"<<brother<<", relative:"<<relative<<std::endl;
+    std::cout << "pose:" << pose << ",  brother:" << brother << ", relative:" << relative << std::endl;
     if (move_mode_ == eDouble)
         relative = Pose2d(-1e8, -1e8, 0);
 
@@ -1041,7 +1096,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     //printf(" r:%f  dt:%f acc:%f  acc_a:%f\n",mpc_parameter.shortest_radius(),
     //       mpc_parameter.dt(),mpc_parameter.acc_velocity(),mpc_parameter.acc_angular());
     ret = MPC.solve(traj, traj[traj.size() - 1], statu, parameter,
-                    out, selected_trajectory, optimize_trajectory,directY);
+                    out, selected_trajectory, optimize_trajectory, directY);
 
     //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
     if (ret == no_solution) {
@@ -1076,7 +1131,7 @@ 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 && targetNode.id().find('S') != -1) {//入库只判断前后方向上是否到达
         tx = x;
     }
 
@@ -1089,7 +1144,7 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
     double b = (x - tx) * sin(theta) + (y - ty) * cos(theta);
 
     if (pow(a / l, 8) + pow(b / w, 8) <= 1) {
-        if (fabs(timedV_.Get()) < 0.06 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(timedV_.Get()) < 0.05 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" Arrived target: %f  %f l:%f w:%f theta:%f\n", tx, ty, l, w, theta);
             return true;
         }
@@ -1124,9 +1179,10 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
     }
 }
 
-void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance) {
+void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1,double Y2) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance);
+        monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance,Y1,Y2);
         if (type == eRotation)
             RWheel_position_ = eR;
         if (type == eVertical)
@@ -1169,30 +1225,60 @@ bool Navigation::clamp_close() {
     return false;
 }
 
-bool Navigation::clamp_open() {
+bool Navigation::clamp_half_open() {
     if (monitor_) {
         printf("Clamp openning...\n");
-        monitor_->clamp_open(move_mode_);
-        actionType_ = eClampOpen;
+        monitor_->clamp_half_open(move_mode_);
+        actionType_ = eClampHalfOpen;
         while (cancel_ == false) {
             if (timed_clamp_.timeout()) {
                 printf("timed clamp is timeout\n");
                 return false;
             }
-            if (timed_clamp_.Get() == eOpened)
+            if (timed_clamp_.Get() == eHalfOpened)
                 return true;
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            monitor_->clamp_open(move_mode_);
-            actionType_ = eClampOpen;
+            monitor_->clamp_half_open(move_mode_);
+            actionType_ = eClampHalfOpen;
         }
         return false;
     }
     return false;
 }
 
+bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
+    if(spaceNode.id().find("S")<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;
+    }
+    return false;
+}
+
+bool Navigation::singleClamp_fullyOpen_until_inspaceBegin(){
+    if(move_mode_==eDouble){
+        return true;
+    }
+    while(cancel_==false){
+        if(timedV_.timeout()){
+            return false;
+        }
+        if(fabs(timedV_.Get())<0.02){
+
+            usleep(1000*300);
+            continue;
+        }
+        break;
+    }
+    return clamp_fully_open();
+};
 
-bool Navigation::clamp_fully_open(){
-    if (monitor_){
+bool Navigation::clamp_fully_open() {
+    if (monitor_) {
+        if (timed_clamp_.Get() == eFullyOpened)
+            return true;
         printf("Clamp fully openning...\n");
         monitor_->clamp_fully_open(move_mode_);
         actionType_ = eClampFullyOpen;
@@ -1210,20 +1296,34 @@ bool Navigation::clamp_fully_open(){
         return false;
     }
     return false;
-
 }
 
 
 bool Navigation::lifter_rise() {
     if (monitor_) {
         printf("Lifter upping...\n");
-        monitor_->lifter_rise(move_mode_);
         actionType_ = eLifterRise;
+        if (timed_lifter_.Get() == eRose)
+            return true;
+
+        if (timed_lifter_.Get() != eRose) {
+            if (timed_clamp_.Get() != eHalfOpened && timed_clamp_.Get() != eClosed) {
+                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
+                if (!clamp_half_open()) {
+                    printf(" clamp half open failed\n");
+                    return false;
+                }
+            }
+        }
+
         while (cancel_ == false) {
             if (timed_lifter_.timeout()) {
                 printf("timed lifter is timeout\n");
                 return false;
             }
+            if (timed_lifter_.timeout()) {
+                return false;
+            }
             if (timed_lifter_.Get() == eRose)
                 return true;
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -1238,13 +1338,26 @@ bool Navigation::lifter_rise() {
 bool Navigation::lifter_down() {
     if (monitor_) {
         printf("Lifter downing...\n");
-        monitor_->lifter_down(move_mode_);
+        if (timed_lifter_.Get() == eDowned)
+            return true;
         actionType_ = eLifterDown;
+        if (timed_lifter_.Get() != eDowned) {
+            if (timed_clamp_.Get() != eHalfOpened && timed_clamp_.Get() != eClosed) {
+                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
+                if (!clamp_half_open()) {
+                    printf(" clamp half open failed\n");
+                    return false;
+                }
+            }
+        }
         while (cancel_ == false) {
             if (timed_lifter_.timeout()) {
                 printf("timed lifter is timeout\n");
                 return false;
             }
+            if (timed_lifter_.timeout()) {
+                return false;
+            }
             if (timed_lifter_.Get() == eDowned)
                 return true;
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -1287,17 +1400,21 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         } else {
             std::cout << " MPC X axis  target_relative:" << target_relative << std::endl;
         }
+        if(!PossibleToTarget(node, directY)){
+            directY=!directY;
+        }
     }
+
     printf(" exec along autoDirect:%d\n", autoDirect);
 
     //文件log
     std::ofstream ofs;  //创建流对象
     std::string log_file_dir = "../data/";
     time_t t = time(0);
-    char tmp[32]={NULL};
-    strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S.txt",localtime(&t));
+    char tmp[32] = {NULL};
+    strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S.txt", localtime(&t));
     std::string log_file_name = tmp;
-    ofs.open(log_file_dir+log_file_name,std::ios::app);  //打开的地址和方式
+    ofs.open(log_file_dir + log_file_name, std::ios::app);  //打开的地址和方式
     auto beg_time = std::chrono::steady_clock::now();
 
     while (cancel_ == false) {
@@ -1338,8 +1455,8 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             return eMpcFailed;
         }
         const int down_count = 3;//下发前多少步
-        double down_v[down_count] ={out[0], out[2], out[4]};//下发线速度
-        double down_w[down_count] ={out[1], out[3], out[5]};//下发角速度
+        double down_v[down_count] = {out[0], out[2], out[4]};//下发线速度
+        double down_w[down_count] = {out[1], out[3], out[5]};//下发角速度
         /*
          * AGV 在终点附近低速巡航时，设置最小速度0.05
          */
@@ -1360,21 +1477,22 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
 //        out[1] *= 10;
 
         //入库，行进方向上最后一米不纠偏
-        if (newUnfinished_cations_.front().type() == 1 && node.id().find('S')!=-1){//入库
-            if(fabs(target_in_agv.x()) < 0.5){
+        if (newUnfinished_cations_.front().type() == 1 && node.id().find('S') != -1) {//入库
+            if (fabs(target_in_agv.x()) < 0.5) {
                 printf("target_in_agv: %f", target_in_agv.x());
-                for (double & i : down_w) {
+                for (double &i: down_w) {
                     i = 0.0;
                 }
             }
-        }else if (newUnfinished_cations_.front().type() == 2){//出库,行进方向上开始一米不纠偏
+        } else if (newUnfinished_cations_.front().type() == 2) {//出库,行进方向上开始一米不纠偏
             Pose2d agv_in_space =
                     Pose2d::relativePose(
-                            Pose2d(newUnfinished_cations_.front().spacenode().x(), newUnfinished_cations_.front().spacenode().y(), 0),
+                            Pose2d(newUnfinished_cations_.front().spacenode().x(),
+                                   newUnfinished_cations_.front().spacenode().y(), 0),
                             timedPose_.Get());
-            if(fabs(agv_in_space.x()) < 0.3){
+            if (fabs(agv_in_space.x()) < 0.3) {
                 printf("agv_in_space: %f", agv_in_space.x());
-                for (double & i : down_w) {
+                for (double &i: down_w) {
                     i = 0.0;
                 }
             }
@@ -1383,32 +1501,40 @@ 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));
+        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);
+        if (node.id().find('S') != -1) {
+            std::string id = node.id().substr(1, node.id().length() - 1);
             space_id = stoi(id);
         }
-//        double diff_yaw_[2] = {diff_yaw[0], diff_yaw[1]};
+        double Y1=0.0,Y2=0.0;
+        if(move_mode_==eDouble){
+            double dy=-timeYawDiff1_.Get();
+            Y1=0.2*(1.0/(1+exp(-16.*dy))-0.5)*1.0;
+            Y2=timeYawDiff2_.Get();
+        }
         if (directY == false)
-            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance);
+            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance,Y1,Y2);
         else
-            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance);
+            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance,Y1,Y2);
         actionType_ = directY ? eHorizontal : eVertical;
 
         //日志打印
         std::string log_inf_line = std::to_string(move_mode_);
         log_inf_line += " ";
 
-        log_inf_line += std::to_string(parameter_.x_mpc_parameter().shortest_radius())+" "+std::to_string(parameter_.x_mpc_parameter().acc_angular());
+        log_inf_line += std::to_string(parameter_.x_mpc_parameter().shortest_radius()) + " " +
+                        std::to_string(parameter_.x_mpc_parameter().acc_angular());
         log_inf_line += " ";
 
         Pose2d cur_pose = timedPose_.Get();//自身x,y,theta
-        log_inf_line += std::to_string(cur_pose.x()) + " " + std::to_string(cur_pose.y()) + " " + std::to_string(cur_pose.theta());
+        log_inf_line += std::to_string(cur_pose.x()) + " " + std::to_string(cur_pose.y()) + " " +
+                        std::to_string(cur_pose.theta());
         log_inf_line += " ";
 
         Pose2d cur_Bro_pose = timedBrotherPose_.Get();//另一节x,y,theta
-        log_inf_line += std::to_string(cur_Bro_pose.x()) + " " + std::to_string(cur_Bro_pose.y()) + " " + std::to_string(cur_Bro_pose.theta());
+        log_inf_line += std::to_string(cur_Bro_pose.x()) + " " + std::to_string(cur_Bro_pose.y()) + " " +
+                        std::to_string(cur_Bro_pose.theta());
         log_inf_line += " ";
 
         log_inf_line += std::to_string(out[0]) + " " + std::to_string(out[1]);//下发速度
@@ -1417,11 +1543,14 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         log_inf_line += " ";
         log_inf_line += std::to_string(out[4]) + " " + std::to_string(out[5]);//下发速度
         log_inf_line += " ";
+        log_inf_line += std::to_string(Y1) + " " + std::to_string(Y2);//下发速度
+        log_inf_line += " ";
 
         auto cur_time = std::chrono::steady_clock::now();
         auto duration = std::chrono::duration_cast<std::chrono::microseconds>(cur_time - beg_time);
         double time =
-                double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+                double(duration.count()) * std::chrono::microseconds::period::num /
+                std::chrono::microseconds::period::den;
         log_inf_line += std::to_string(time);
 
         ofs << log_inf_line << std::endl;  //一行日志的内容
@@ -1448,7 +1577,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
         return false;
     }
     Pose2d current = timedPose_.Get();
-    if(targetNode.id().find('S') != -1){
+    if (targetNode.id().find('S') != -1) {
         //车位点强制可到达
         return true;
     }
@@ -1509,7 +1638,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
     printf(" impossible to target:%f %f l:%f,w:%f theta:%f, ",
            targetNode.x(), targetNode.y(), targetNode.l(), targetNode.w(), targetNode.theta());
     printf(" current:%f, %f, %f\n",
-           current.x(), current.y(),current.theta());
+           current.x(), current.y(), current.theta());
 
     return false;
 }
@@ -1543,18 +1672,19 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     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) {  //入库
             space_id_ = act.spacenode().id();
-            obs_w_=1.25;
-            obs_h_=1.87;
+            obs_w_ = 1.1;
+            obs_h_ = 1.87;
             if (!execute_InOut_space(act)) {
                 printf(" In space failed\n");
                 isInSpace_ = false;
                 break;
             }
         } else if (act.type() == 2) {  //出库
-            obs_w_=1.25;
-            obs_h_=1.87;
+            obs_w_ = 1.25;
+            obs_h_ = 1.87;
             if (!execute_InOut_space(act)) {
                 printf(" out space failed\n");
                 break;
@@ -1562,9 +1692,14 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 isInSpace_ = false;
             }
         } else if (act.type() == 3 || act.type() == 4) { //马路导航
-            obs_w_=1.25;
-            obs_h_=1.87;
-            if (!execute_nodes(act)){
+            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)) {
                 printf(" street nav failed\n");
                 break;
             }
@@ -1577,7 +1712,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 break;
             }
         } else if (act.type() == 7) {
-            if (this->clamp_open() == false) {
+            if (this->clamp_half_open() == false) {
                 printf("打开夹持 failed...\n");
                 break;
             }
@@ -1593,8 +1728,9 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 printf("下降failed ...\n");
                 break;
             }
-        }else {
-            printf(" action type invalid not handled !!\n");break;
+        } else {
+            printf(" action type invalid not handled !!\n");
+            break;
         }
         newUnfinished_cations_.pop();
     }
@@ -1623,6 +1759,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
 }
 
 void Navigation::SwitchMode(int mode, float wheelBase) {
+    wheelBase_ = wheelBase;
     printf("Child AGV can not Switch Mode\n");
 }
 

MPC/navigation.h

@@ -33,7 +33,7 @@ public:
     enum ClampStatu {
         eInvalid = 0,
         eClosed = 1,
-        eOpened = 2,    //半开位（默认）
+        eHalfOpened = 2,    //半开位（默认）
         eFullyOpened = 3 //全开位
     };
 
@@ -103,6 +103,7 @@ public:
             ofs << log_inf_line << std::endl;  //一行日志的内容
             ofs.close();
         }
+        return true;
     }
 
 protected:
@@ -112,7 +113,8 @@ protected:
 
     virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
 
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance);
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1=0.0,double Y2=0.0);
 //    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], double diff_yaw[], int space_id, double distance);
 
     static void RobotPoseCallback(const MqttMsg &msg, void *context);
@@ -144,10 +146,13 @@ protected:
 
     virtual MpcResult RotateReferToTarget(NavMessage::PathNode target, stLimit limit_rotate, int directions);
 
+    virtual bool AdjustReferToTarget(const Pose2d& target, stLimit limit_v);
+
     /*
      * 按照路径点导航
      */
-    bool execute_nodes(NavMessage::NewAction action);
+
+    bool execute_nodes(NavMessage::NewAction action,int next_actionTypeID=-1);
 
     /*
      * 进出库
@@ -167,7 +172,7 @@ protected:
 
     virtual bool clamp_close();
 
-    virtual bool clamp_open();
+    virtual bool clamp_half_open();
 
     virtual bool clamp_fully_open();
 
@@ -175,6 +180,10 @@ protected:
 
     virtual bool lifter_down();
 
+    virtual bool singleClamp_fullyOpen_until_inspaceBegin();
+
+    static bool IsUperSpace(NavMessage::PathNode spaceNode);
+
     bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);
 
     /* 原地旋转mpc
@@ -207,6 +216,8 @@ protected:
     Terminator_emqx *terminator_ = nullptr;  //本机emqx连接
     Terminator_emqx *brotherEmqx_ = nullptr;  //本机emqx连接
 
+    TimedLockData<double> timeYawDiff1_;
+    TimedLockData<double> timeYawDiff2_;
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;
@@ -226,6 +237,7 @@ protected:
     bool cancel_ = false;
 
     int move_mode_ = eSingle;
+    float wheelBase_;   //两节agv的位姿与单节的位姿变换
 
     bool isInSpace_; //是否在车位或者正在进入车位
     std::string space_id_;
@@ -238,6 +250,7 @@ protected:
     double obs_w_;
     double obs_h_;
     TimedLockData<bool> isFront_;
+
 };
 
 double limit_gause(double x, double min, double max);

MPC/navigation_main.cpp

@@ -32,10 +32,17 @@ void NavigationMain::ResetPose(const Pose2d &pose) {
         }
         Pose2d abs_diff = pose - brother;
         //计算两车朝向的方向
-        float theta = Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
+        float theta = pose.theta();//Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
         Pose2d agv = Pose2d((pose.x() + brother.x()) / 2.0, (pose.y() + brother.y()) / 2.0, theta);
-
-        Navigation::ResetPose(agv);
+        Pose2d abs_diff1 = pose - agv;
+        Pose2d abs_diff2 = brother - agv;
+        timeYawDiff1_.reset(pose.theta());
+
+        timeYawDiff2_.reset((brother-pose).theta());
+        Pose2d brotherInPose=Pose2d::relativePose(brother,pose);
+        timeYawDiff1_.reset(brotherInPose.y());
+        //timeYawDiff2_.reset(abs_diff2.theta());
+        Navigation::ResetPose(pose);
 //        timedPose_().m_diffYaw1=pose.theta()-theta;
 //        timedPose_().m_diffYaw2=brother.theta()-theta;
 //        printf("m_diffYaw1:%f, m_diffYaw2:%f\n",timedPose_().m_diffYaw1, timedPose_().m_diffYaw2);
@@ -68,9 +75,11 @@ void NavigationMain::SendMoveCmd(int mode, ActionType type,
     }
 }
 
-void NavigationMain::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance) {
+void NavigationMain::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                                 int space_id, double distance,double Y1,double Y2) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_, space_id, distance);
+        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_,
+                               space_id, distance,Y1,Y2);
         if (type == eRotation)
             RWheel_position_ = eR;
         if (type == eVertical)
@@ -133,24 +142,24 @@ bool NavigationMain::clamp_close() {
     return false;
 }
 
-bool NavigationMain::clamp_open() {
+bool NavigationMain::clamp_half_open() {
     if (move_mode_ == eSingle)
-        return Navigation::clamp_open();
+        return Navigation::clamp_half_open();
 
     if (monitor_) {
         printf("双车松夹持\n");
-        monitor_->clamp_open(move_mode_);
+        monitor_->clamp_half_open(move_mode_);
         while (cancel_ == false) {
             if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
                 printf("timed clamp is timeout\n");
                 return false;
             }
-            if (timed_clamp_.Get() == eOpened && timed_other_clamp_.Get() == eOpened) {
+            if (timed_clamp_.Get() == eHalfOpened && timed_other_clamp_.Get() == eHalfOpened) {
                 printf("双车松夹持completed!!!\n");
                 return true;
             }
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            monitor_->clamp_open(move_mode_);
+            monitor_->clamp_half_open(move_mode_);
         }
         return false;
     }
@@ -163,7 +172,19 @@ bool NavigationMain::lifter_rise() {
     }
     if (monitor_) {
         printf("双车提升机构提升\n");
-        monitor_->lifter_rise(move_mode_);
+        actionType_ = eLifterRise;
+        if (timed_lifter_.Get() == eRose)
+            return true;
+        else {
+            if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
+                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
+                if(!clamp_half_open()){
+                    printf(" clamp half open failed\n");
+                    return false;
+                }
+            }
+        }
+
         while (cancel_ == false) {
             if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                 printf("timed lifter is timeout\n");
@@ -187,7 +208,19 @@ bool NavigationMain::lifter_down() {
     }
     if (monitor_) {
         printf("双车提升机构下降\n");
-        monitor_->lifter_down(move_mode_);
+        actionType_ = eLifterDown;
+        if (timed_lifter_.Get() == eDowned)
+            return true;
+        else {
+            if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
+                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
+                if(!clamp_half_open()){
+                    printf(" clamp half open failed\n");
+                    return false;
+                }
+            }
+        }
+
         while (cancel_ == false) {
             if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                 printf("timed lifter is timeout\n");
@@ -219,21 +252,21 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     double acc_angular = 25 * M_PI / 180.0;
     double dt = 0.1;
 
-    Pose2d rotated = timedPose_.Get();
+    Pose2d current = timedPose_.Get();
     double x = space.x();
     double y = space.y();
-    Pose2d vec(x - rotated.x(), y - rotated.y(), 0);
-    rotated.mutable_theta() = Pose2d::vector2yaw(vec.x(), vec.y());
+    Pose2d vec(x - current.x(), y - current.y(), 0);
+    double vecTheta = Pose2d::vector2yaw(vec.x(), vec.y());
     bool isFront=isFront_.Get();
-    if(rotated.theta()<0.)
+    if(vecTheta<0.)
         isFront=!isFront;
 
     //前车先到,后车进入2点,保持与前车一致的朝向
     if (isFront==false ) {
         printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
         if (move_mode_ == eSingle) {
-            x -= wheelbase * cos(rotated.theta());
-            y -= wheelbase * sin(rotated.theta());
+            x -= wheelbase * cos(vecTheta);
+            y -= wheelbase * sin(vecTheta);
             printf("确定车位点：eSingle\n");
         }
     } else { //当后车先到,倒车入库
@@ -244,33 +277,36 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
 
 
     if (move_mode_ == eDouble){
-        x -= wheelbase/2 * cos(rotated.theta());
-        y -= wheelbase/2 * sin(rotated.theta());
+        x -= wheelbase/2 * cos(vecTheta);
+        y -= wheelbase/2 * sin(vecTheta);
         printf("RotateBeforeEnterSpace | 整车模式, __LINE__ = %d\n", __LINE__);
     }
-    if(rotated.theta()<0.)
-        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
+
+    Pose2d spaceInCurrent=Pose2d::relativePose(Pose2d(space.x(),space.y(),0),current);
+
 
     target.set_x(x);
     target.set_y(y);
-    target.set_theta(rotated.theta());
+    target.set_theta(current.theta());
     target.set_l(0.02);
-    target.set_w(0.05);
+    target.set_w(0.1);
     target.set_id(space.id());
     printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
 
-    if (move_mode_ == eDouble) {
-        double yawDiff1 = (rotated - timedPose_.Get()).theta();
-        double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta();
-        if (fabs(yawDiff1) > fabs(yawDiff2)) {
-            rotated = rotated + Pose2d(0, 0, M_PI);
-        }
-    }
+
 
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        Pose2d current = timedPose_.Get();
-        double yawDiff = (rotated - current).theta();
+        Pose2d current_now = timedPose_.Get();
+        if(spaceInCurrent.x()<0.)
+            current_now = current_now.rotate(current_now.x(), current_now.y(), M_PI);
+        double yawDiff = vecTheta - current_now.theta();
+        if (move_mode_ == eDouble) {
+            double yawDiff2 = vecTheta - (Pose2d(0, 0, M_PI) + current_now).theta();
+            if (fabs(yawDiff) > fabs(yawDiff2)) {
+                yawDiff=yawDiff2;
+            }
+        }
 
         //一次变速
         std::vector<double> out;
@@ -284,10 +320,26 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 3 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
-            return eMpcSuccess;
+
+            usleep(1000*1000);
+            Pose2d current_2 = timedPose_.Get();
+            if(spaceInCurrent.x()<0.)
+                current_2 = current_2.rotate(current_2.x(), current_2.y(), M_PI);
+            yawDiff = vecTheta - current_2.theta();
+            if (move_mode_ == eDouble) {
+                double yawDiff2 = vecTheta - (Pose2d(0, 0, M_PI) + current_now).theta();
+                if (fabs(yawDiff) > fabs(yawDiff2)) {
+                    yawDiff=yawDiff2;
+                }
+            }
+            if(fabs(yawDiff) < 0.3){
+                return eMpcSuccess;
+            }else{
+                continue;
+            }
         } else{
             const int down_count = 3;
             double v[down_count] = {0,0,0};

MPC/navigation_main.h

@@ -6,7 +6,6 @@
 #define NAVIGATION_NAVIGATION_MAIN_H
 
 #include "navigation.h"
-
 class NavigationMain : public Navigation {
 
 public:
@@ -29,6 +28,7 @@ public:
 
     virtual void SwitchMode(int mode, float wheelBase) {
         printf("change mode to:%d\n", mode);
+        wheelBase_ = wheelBase;
         if (move_mode_ == mode)
             return;
         if (mode == eDouble) {
@@ -62,7 +62,7 @@ public:
 
             printf(" Switch MoveMode --> main(%d), wheelBase: %f\n", mode, wheelBase);
         }
-        wheelBase_ = wheelBase;
+
         move_mode_ = mode;
     };
 
@@ -71,11 +71,12 @@ protected:
     RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
 
     virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance);
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1=0.,double Y2=0.);
 
     virtual bool clamp_close();
 
-    virtual bool clamp_open();
+    virtual bool clamp_half_open();
 
     virtual bool lifter_rise();
 
@@ -84,7 +85,7 @@ protected:
     virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu);
 
 protected:
-    float wheelBase_;   //两节agv的位姿与单节的位姿变换
+
     TimedLockData<int> timed_other_clamp_;
     TimedLockData<int> timed_other_lifter_;
 };

config/navigation.prototxt

@@ -7,6 +7,7 @@ Agv_emqx
 	ip:"192.168.0.71"
 	port:1883
 	pubSpeedTopic:"monitor_child/speedcmd"
+	pubClampLifterTopic:"monitor_child/clampLifter"
 	subPoseTopic:"monitor_child/statu"
 	subSpeedTopic:"monitor_child/speed"
 }

config/navigation_main.prototxt

@@ -7,6 +7,7 @@ Agv_emqx
 	ip:"192.168.0.70"
 	port:1883
 	pubSpeedTopic:"monitor_child/speedcmd"
+	pubClampLifterTopic:"monitor_child/clampLifter"
 	subPoseTopic:"monitor_child/statu"
 	subSpeedTopic:"monitor_child/speed"
 }

message.proto

@@ -13,7 +13,7 @@ message LidarOdomStatu {
 message AgvStatu {
     float v = 1;
     float w = 2;
-    int32 clamp = 3;    //夹持状态  0，中间状态，1夹紧到位，2，打开到位，
+    int32 clamp = 3;    //夹持状态  0，中间状态，1夹紧到位，2，半打开到位，3. 全打开到位
     int32 clamp_other = 4; //另外一节
     int32 lifter = 5;   //提升机构 0中间状态，1上升到位， 2下降到位
     int32 lifter_other = 6;     //另外一节

parameter.proto

@@ -8,6 +8,7 @@ message AgvEmqx_parameter
   string pubSpeedTopic=4;
   string subPoseTopic=5;
   string subSpeedTopic=6;
+  string pubClampLifterTopic=7;
 }
 message Emqx_parameter
 {