1.mqtt publish 加锁

2.马路点导航取消提升机构动作
3.马路点导航精度方向调整。最后一点精度不旋转
4.马路点导航，单车夹持杆并行打开到半开位
5.从入口出库旋转180度
6.修正入库精度方向（+90度）
7.修正入库旋转方法返回值判断错误
8.双单车入库，先进车越过后进车目标点后，后进车再开始动作
9.添加判断载车板车位函数
10.添加入库时全开位夹持与行走并行函数
11.修改整车模式下，整车角度计算方法。改为两车角度取平均

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

@@ -235,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;
@@ -258,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);
         }
     }
 
@@ -338,8 +337,8 @@ Navigation::Navigation() {
     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);
 }
 
@@ -503,9 +502,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;
@@ -521,22 +520,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;
 }
@@ -568,9 +555,9 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         }
         //巡线无法到达目标点
         Pose2d target(node.x(), node.y(), 0);
-        Pose2d targetInCurrent=Pose2d::relativePose(target,current);
+        Pose2d targetInCurrent = Pose2d::relativePose(target, current);
 
-        if (enableTotarget == false && Pose2d::distance(Pose2d(0,0,0),targetInCurrent)>0.3) {
+        if (enableTotarget == false && Pose2d::distance(Pose2d(0, 0, 0), targetInCurrent) > 0.3) {
             if (already_in_mpc && enable_rotate == false) {
                 printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
                 return false;
@@ -581,11 +568,11 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
                 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;
-                }
+            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;
         }
         already_in_mpc = true;
@@ -607,7 +594,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, int space_id) {
     if (action.type() == 3 || action.type() == 4) //最优动作导航 or 导航中保证朝向严格一致
     {
         if (!parameter_.has_nodeangularlimit() || !parameter_.has_nodevelocitylimit() || action.pathnodes_size() == 0) {
@@ -622,41 +609,33 @@ bool Navigation::execute_nodes(NavMessage::NewAction action,int space_id) {
 
         /////////////////////////////////////////
 
-        LifterStatus lifter_actType;
-        std::thread* thread_lifter= nullptr;
-        if (space_id>99 && space_id< 1000){
-            //进载车板库前提升机构上升
-            lifter_actType=eRose;
-            thread_lifter=new std::thread(&Navigation::lifter_rise,this);
-        }
-        else{
-            //否则下降
-            lifter_actType=eDowned;
-            thread_lifter=new std::thread(&Navigation::lifter_down,this);
-        }
 
+        std::thread *thread_clampOpenHalf = nullptr;
+        if(move_mode_==eSingle) {
+            thread_clampOpenHalf = new std::thread(&Navigation::clamp_half_open, this);
+        }
         ////////////////////////////////////////////////////////////
-
-
         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_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
                 node.set_l(0.03);
                 node.set_w(0.20);
             } else {
-                node.set_theta(0);
+                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.20);
+                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;
                 }
@@ -666,16 +645,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action,int space_id) {
             else
                 isInSpace_ = false;
         }
-
-        while(timed_lifter_.Get()!=lifter_actType && cancel_==false){
-            usleep(1000*100);
-            continue;
+        if(move_mode_==eSingle) {
+            printf(" wait clamp open half...\n");
+            thread_clampOpenHalf->join();
+            printf("clamp open half completed!!!\n");
+            delete thread_clampOpenHalf;
         }
-        printf(" wait lifter action:%d\n",lifter_actType);
-        thread_lifter->join();
-        printf("lifter action:%d  completed!!!\n",lifter_actType);
-        delete thread_lifter;
-        if(cancel_==true){
+        if (cancel_ == true) {
             return false;
         }
 
@@ -686,21 +662,19 @@ bool Navigation::execute_nodes(NavMessage::NewAction action,int space_id) {
     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;
@@ -717,17 +691,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;
 }
@@ -750,13 +723,13 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
     double y = space.y();
     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;
+    bool isFront = isFront_.Get();
+    printf(" front__:%d  theta:%f\n", isFront, vecTheta);
+    if (vecTheta < 0.)
+        isFront = !isFront;
 
     //如果是后车，计算目标点（车位点-轴距）
-    if (isFront==false ) {
+    if (isFront == false) {
         printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
         if (move_mode_ == eSingle) {
             x -= wheelbase * cos(vecTheta);
@@ -769,7 +742,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
         //rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
 
-    Pose2d spaceInCurrent=Pose2d::relativePose(Pose2d(space.x(),space.y(),0),current);
+    Pose2d spaceInCurrent = Pose2d::relativePose(Pose2d(space.x(), space.y(), 0), current);
 
 
     target.set_x(x);
@@ -778,7 +751,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
     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) {
@@ -792,7 +765,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(30));
         Pose2d current_now = timedPose_.Get();
-        if(spaceInCurrent.x()<0.)
+        if (spaceInCurrent.x() < 0.)
             current_now = current_now.rotate(current_now.x(), current_now.y(), M_PI);
         double yawDiff = vecTheta - current_now.theta();
 
@@ -812,14 +785,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;
 
@@ -851,11 +824,12 @@ 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.2);
-        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
+        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y())+M_PI/2.0);
         /*
          * 是否需要添加判断，距离较远才先运动到马路点=============================
          */
@@ -868,15 +842,28 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         printf("inout ----------------------------------------\n");
         //计算车位朝向
         action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
-        //计算当前车是进入车位1点还是2点
+        //计算当前车是进入车位1点还是2点,    提升下降动作与旋转调整并行
+        LifterStatus lifter_actType;
+        std::thread *thread_lifter = nullptr;
+        if (IsUperSpace(action.spacenode())) {
+            //进载车板库前提升机构上升
+            lifter_actType = eRose;
+            thread_lifter = new std::thread(&Navigation::lifter_rise, this);
+        } else {
+            //否则下降
+            lifter_actType = eDowned;
+            thread_lifter = new std::thread(&Navigation::lifter_down, this);
+        }
         NavMessage::PathNode new_target;
-        if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcFailed) {
+        bool retRotate=RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcSuccess;
+        printf(" wait lifter action:%d\n", lifter_actType);
+        thread_lifter->join();
+        delete thread_lifter;
+        printf("lifter action:%d  completed!!!\n", lifter_actType);
+
+        if(retRotate==false){
             return false;
         }
-        if(move_mode_==eSingle){
-            if(!clamp_fully_open())
-                return false;
-        }
 
         //入库
         Pose2d rotated = timedPose_.Get();
@@ -884,23 +871,42 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         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());
+
+        std::thread *thread_clamp = nullptr;
+        thread_clamp = new std::thread(&Navigation::singleClamp_fullyOpen_until_inspaceBegin, this);
+
+        bool retMove=MoveToTarget(new_target, limit_v, limit_w, true, false);
+        thread_clamp->join();
+        delete thread_clamp;
+
+        if(retMove==false){
+            printf(" Enter space:%s failed. type:%d\n", action.spacenode().id().data(), action.type());
             return false;
         }
     } else if (action.type() == 2) {
@@ -911,7 +917,7 @@ 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);
@@ -922,14 +928,22 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
                 printf(" out space failed type:%d: MoveTo StreetNode failed\n", action.type());
                 return false;
             }
-
         } else {
             std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff
                       << std::endl;
             return false;
         }
-        isInSpace_=false;
+        isInSpace_ = false;
 
+        //从入口出库旋转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;
 }
@@ -1028,8 +1042,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]
@@ -1046,7 +1060,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);
 
@@ -1057,7 +1071,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) {
@@ -1092,7 +1106,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;
     }
 
@@ -1206,9 +1220,36 @@ bool Navigation::clamp_half_open() {
     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");
@@ -1228,7 +1269,6 @@ bool Navigation::clamp_fully_open(){
         return false;
     }
     return false;
-
 }
 
 
@@ -1239,10 +1279,10 @@ bool Navigation::lifter_rise() {
         if (timed_lifter_.Get() == eRose)
             return true;
 
-        if (timed_lifter_.Get() != eRose){
-            if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
+        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()){
+                if (!clamp_half_open()) {
                     printf(" clamp half open failed\n");
                     return false;
                 }
@@ -1254,7 +1294,7 @@ bool Navigation::lifter_rise() {
                 printf("timed lifter is timeout\n");
                 return false;
             }
-            if (timed_lifter_.timeout()){
+            if (timed_lifter_.timeout()) {
                 return false;
             }
             if (timed_lifter_.Get() == eRose)
@@ -1274,10 +1314,10 @@ bool Navigation::lifter_down() {
         if (timed_lifter_.Get() == eDowned)
             return true;
         actionType_ = eLifterDown;
-        if (timed_lifter_.Get() != eDowned){
-            if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
+        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()){
+                if (!clamp_half_open()) {
                     printf(" clamp half open failed\n");
                     return false;
                 }
@@ -1288,7 +1328,7 @@ bool Navigation::lifter_down() {
                 printf("timed lifter is timeout\n");
                 return false;
             }
-            if (timed_lifter_.timeout()){
+            if (timed_lifter_.timeout()) {
                 return false;
             }
             if (timed_lifter_.Get() == eDowned)
@@ -1340,10 +1380,10 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     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) {
@@ -1384,8 +1424,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
          */
@@ -1406,21 +1446,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;
                 }
             }
@@ -1429,10 +1470,10 @@ 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]};
@@ -1446,15 +1487,18 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         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]);//下发速度
@@ -1467,7 +1511,8 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         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;  //一行日志的内容
@@ -1494,7 +1539,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;
     }
@@ -1555,7 +1600,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;
 }
@@ -1589,19 +1634,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();
+        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.25;
+            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;
@@ -1609,14 +1654,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;
-            int space_id=-1;
-            if(last_act.type()==1) {
+            obs_w_ = 1.25;
+            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)){
+            if (!execute_nodes(act, space_id)) {
                 printf(" street nav failed\n");
                 break;
             }
@@ -1645,8 +1690,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();
     }
@@ -1675,7 +1721,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
 }
 
 void Navigation::SwitchMode(int mode, float wheelBase) {
-    wheelBase_=wheelBase;
+    wheelBase_ = wheelBase;
     printf("Child AGV can not Switch Mode\n");
 }
 

MPC/navigation.h

@@ -177,6 +177,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

MPC/navigation_main.cpp

@@ -32,7 +32,7 @@ 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()+brother.theta())/2.0;//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);
@@ -311,7 +311,7 @@ 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;

MPC/navigation_main.h

@@ -6,7 +6,6 @@
 #define NAVIGATION_NAVIGATION_MAIN_H
 
 #include "navigation.h"
-
 class NavigationMain : public Navigation {
 
 public: