1.自己区分前后车（未测试）

MPC/loaded_mpc.cpp

@@ -131,9 +131,9 @@ public:
                     CppAD::AD<double> rb = (ox - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
                                            (oy - vars[ny + t]) * CppAD::cos(vars[nth + t]);
                     if(!directY_)
-                        fg[1 + nobs + N * i + t] = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / (obs_h*0.6), 8);
+                        fg[1 + nobs + N * i + t] = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / (obs_h*0.76), 8);
                     else
-                        fg[1 + nobs + N * i + t] = CppAD::pow(ra / (obs_w*0.6), 8) + CppAD::pow(rb / obs_h, 8);
+                        fg[1 + nobs + N * i + t] = CppAD::pow(ra / (obs_w*0.76), 8) + CppAD::pow(rb / obs_h, 8);
                 }
             }
         }

MPC/navigation.cpp

@@ -257,6 +257,20 @@ 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);
+        }
+    }else{
+        if(parameter_.main_agv()){
+            isFront_.reset(false,1.0);
+        }else{
+            isFront_.reset(true,1.0);
+        }
+    }
+
 }
 
 void Navigation::Cancel(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
@@ -322,7 +336,8 @@ Navigation::Navigation() {
     move_mode_ = eSingle;
     monitor_ = nullptr;
     terminator_ = nullptr;
-
+    obs_h_=4.0;
+    obs_w_=2.0;
     timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5);
 }
 
@@ -694,22 +709,24 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
     double acc_angular = 20 * M_PI / 180.0;
     double dt = 0.1;
     Pose2d rotated = 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();
+    if(rotated.theta()>0.)
+        isFront=!isFront;
+
     //前车先到,后车进入2点,保持与前车一致的朝向
-    if (brother.in_space() && brother.space_id() == space.id()) {
-        printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__);
-//        rotated.mutable_theta() = brother.odom().theta();
-        Pose2d vec(x - rotated.x(), y - rotated.y(), 0);
-        rotated.mutable_theta() = Pose2d::vector2yaw(vec.x(), vec.y());
+    if (isFront==false ) {
+        printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
         if (move_mode_ == eSingle) {
             x -= wheelbase * cos(rotated.theta());
             y -= wheelbase * sin(rotated.theta());
             printf("确定车位点：eSingle\n");
         }
     } else { //当后车先到,倒车入库
-        printf("RotateBeforeEnterSpace | 后车先到, __LINE__ = %d\n", __LINE__);
+        printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__);
         rotated.mutable_theta() = space.theta();
         rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
@@ -795,6 +812,9 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         action.mutable_streetnode()->set_l(0.05);
         action.mutable_streetnode()->set_w(0.05);
         action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
+        /*
+         * 是否需要添加判断，距离较远才先运动到马路点==============================？？？？？？？？？
+         */
         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;
@@ -829,59 +849,18 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             //单车进入口搬车，夹持杆打开到全开位
 
         }
-//
-//        //移动到预入库点位，较高精度
-//        NavMessage::PathNode pre_target;
-//        bool jump_preenter = false;
-//        if (move_mode_ == eSingle){
-//            double x = action.spacenode().x();
-//            double y = action.spacenode().y();
-//            double theta = action.spacenode().theta();
-//
-//            double back_distance = 3.3 + 0.2 + 1;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
-//            Pose2d current = timedPose_.Get();
-//            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
-//            if (diff < back_distance) jump_preenter = true;
-//            x -= back_distance* cos(theta);
-//            y -= back_distance* sin(theta);
-//
-//            pre_target.set_x(x);
-//            pre_target.set_y(y);
-//            pre_target.set_theta(theta);
-//            pre_target.set_l(0.10);
-//            pre_target.set_w(0.05);
-//            pre_target.set_id("R_0");
-//        }
-//        else if (move_mode_ == eDouble){
-//            double x = action.spacenode().x();
-//            double y = action.spacenode().y();
-//            double theta = action.spacenode().theta();
-//
-//            double back_distance = 3.3 + 0.2 + 1 + action.wheelbase()/2;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
-//            Pose2d current = timedPose_.Get();
-//            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
-//            if (diff < back_distance) jump_preenter = true;
-//            x -= back_distance* cos(theta);
-//            y -= back_distance* sin(theta);
-//
-//            pre_target.set_x(x);
-//            pre_target.set_y(y);
-//            pre_target.set_theta(theta);
-//            pre_target.set_l(0.05);
-//            pre_target.set_w(0.03);
-//            pre_target.set_id("R_0");
-//        }
-//        else{ return printf("PreEnter space failed. move_mode_ error!. line: %d\n", __LINE__);}
-//        if (!jump_preenter) {
-//            printf("PreEnter space beg...\npre_target: x:%f, y:%f, theta:%f\n",pre_target.x(),pre_target.y(),pre_target.theta());
-//            if (MoveToTarget(pre_target, limit_v, limit_w, true, true) == false) {
-//                printf(" PreEnter pre_space:%s failed. type:%d. line:%d\n", pre_target.id().data(), action.type(),
-//                       __LINE__);
-//                return false;
-//            }
-//        }
-
         //入库
+        if(isFront_.Get()==false && move_mode_==eSingle){
+            while(timedBrotherNavStatu_.Get().in_space()==false){
+                if(cancel_){
+                    return false;
+                }
+                printf(" 后车 等待 前车先入库。。。\n");
+                usleep(1000*500);
+                continue;
+            }
+        }
+        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());
@@ -921,6 +900,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
                       << std::endl;
             return false;
         }
+        isInSpace_=false;
 
     }
     return true;
@@ -1020,8 +1000,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 =1.5;// 0.95;//0.85;
-    double obs_h =3.0;// 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]
@@ -1029,8 +1009,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
         if (statu[2] > M_PI)
             statu[2] -= 2 * M_PI;
         mpc_parameter = parameter_.y_mpc_parameter();
-        obs_w = 3.0;// 0.95;//0.85;
-        obs_h = 1.5;//1.55;//1.45;
+        obs_w = obs_h_;// 0.95;//0.85;
+        obs_h = obs_w_;//1.55;//1.45;
     }
 
     Pose2d brother = timedBrotherPose_.Get();
@@ -1553,13 +1533,16 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
         NavMessage::NewAction act = newUnfinished_cations_.front();
         if (act.type() == 1) {  //入库
             space_id_ = act.spacenode().id();
-            isInSpace_ = true;
+            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;
             if (!execute_InOut_space(act)) {
                 printf(" out space failed\n");
                 break;
@@ -1567,6 +1550,8 @@ 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)){
                 printf(" street nav failed\n");
                 break;

MPC/navigation.h

@@ -234,6 +234,10 @@ protected:
 
     NavParameter::Navigation_parameter parameter_;
     RWheelPosition RWheel_position_;
+
+    double obs_w_;
+    double obs_h_;
+    TimedLockData<bool> isFront_;
 };
 
 double limit_gause(double x, double min, double max);