1.MPC旋转添加延时预判

2.修复双车提升机构下降时，到位判断条件不对问题
3.RotateReferToTarget()下发指令逻辑优化
4.马路点，预入库点，精度调整为0.1*0.1米
5.能否出库判断改为行驶与非行驶方向上任一方向距车位点小于5cm即可
6.添加出库后提升机构下降功能
7.入库最后1米，出库最开始1米，不下发角速度

MPC/loaded_mpc.cpp

@@ -8,7 +8,7 @@
 #include <cppad/ipopt/solve.hpp>
 
 size_t N = 15;                  //优化考虑后面多少步
-size_t delay = 3;  // 发送到执行的延迟，即几个周期的时间
+size_t delay = 2;  // 发送到执行的延迟，即几个周期的时间
 
 size_t nx = 0;
 size_t ny = nx + N;
@@ -363,7 +363,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
         if (solve_angular < 0) correct_angular = -correct_angular;
     }
     ///-----
-    printf("input:%.4f %.5f(%.5f) output:%.4f %.5f(%.5f) ref:%.3f max_v:%f time:%.3f\n",
+    printf(" input:%.4f %.5f(%.5f) output:%.4f %.5f(%.5f) ref:%.3f max_v:%f time:%.3f\n",
            line_velocity, wmg, angular, solve_velocity, solve_angular, correct_angular,
            ref_velocity, max_velocity_, time);
 

MPC/navigation.cpp

@@ -482,16 +482,29 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
         }
 
         //下发速度
-        if (fabs(target_yaw_diff) > 2 * M_PI / 180.0) {
+        if (fabs(target_yaw_diff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+            printf(" MPC_rotate | refer target completed\\n,cuv:%f\n", target_yaw_diff);
+            Stop();
+            return eMpcSuccess;
+        } else{
             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;
+            printf(" MPC_rotate | input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n",
+                   timedA_.Get(), out[0], out[0]/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;
 }
@@ -577,13 +590,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
                 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.3);
-                node.set_w(0.1);
+                node.set_l(0.10);
+                node.set_w(0.10);
 
             } else {
                 node.set_theta(0);
-                node.set_w(0.2);
-                node.set_l(0.2);
+                node.set_w(0.10);
+                node.set_l(0.10);
             }
             if (MoveToTarget(node, mpc_velocity, adjust_angular, anyDirect, true) == false)
                 return false;
@@ -598,8 +611,8 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
 
 Navigation::MpcResult
 Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
-    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
-//    if (timedPose_.timeout()) {
+//    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
+    if (timedPose_.timeout()) {
         printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
         return eMpcFailed;
     }
@@ -614,6 +627,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
     double y = space.y();
     //前车先到,后车进入2点,保持与前车一致的朝向
     if (brother.in_space() && brother.space_id() == space.id()) {
+        printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__);
         rotated.mutable_theta() = brother.odom().theta();
         if (move_mode_ == eSingle) {
             x -= wheelbase * cos(rotated.theta());
@@ -621,16 +635,17 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
             printf("确定车位点：eSingle\n");
         }
     } else { //当后车先到,倒车入库
+        printf("RotateBeforeEnterSpace | 后车先到, __LINE__ = %d\n", __LINE__);
         rotated.mutable_theta() = space.theta();
         rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
-
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
     target.set_l(0.05);
     target.set_w(0.05);
     target.set_id(space.id());
+    printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
 
 //    //整车协调的时候，保持前后与车位一致即可
 //    if (move_mode_ == eDouble) {
@@ -642,7 +657,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 //    }
 
     while (cancel_ == false) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        std::this_thread::sleep_for(std::chrono::milliseconds(30));
         Pose2d current = timedPose_.Get();
         double yawDiff = (rotated - current).theta();
 
@@ -696,8 +711,8 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     if (action.type() == 1) {
         Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
                    0);
-        action.mutable_streetnode()->set_l(0.05);
-        action.mutable_streetnode()->set_w(0.05);
+        action.mutable_streetnode()->set_l(0.06);
+        action.mutable_streetnode()->set_w(0.06);
         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());
@@ -736,8 +751,8 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             pre_target.set_x(x);
             pre_target.set_y(y);
             pre_target.set_theta(theta);
-            pre_target.set_l(0.03);
-            pre_target.set_w(0.03);
+            pre_target.set_l(0.05);
+            pre_target.set_w(0.05);
             pre_target.set_id("R_0");
         }
         else if (move_mode_ == eDouble){
@@ -755,20 +770,22 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             pre_target.set_x(x);
             pre_target.set_y(y);
             pre_target.set_theta(theta);
-            pre_target.set_l(0.03);
-            pre_target.set_w(0.03);
+            pre_target.set_l(0.05);
+            pre_target.set_w(0.05);
             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, false) == false) {
-                printf(" PreEnter space:%s failed. type:%d. line:%d\n", action.spacenode().id().data(), action.type(),
+                printf(" PreEnter pre_space:%s failed. type:%d. line:%d\n", pre_target.id().data(), action.type(),
                        __LINE__);
                 return false;
             }
         }
 
         //入库
+        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());
             return false;
@@ -776,17 +793,24 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     } else if (action.type() == 2) {
         Pose2d space(action.spacenode().x(), action.spacenode().y(), 0);
         Pose2d diff = Pose2d::abs(Pose2d::relativePose(space, timedPose_.Get()));
-        if (diff.y() < 0.05) {
+        if (diff.x() < 0.05 || diff.y() < 0.05) {
             //出库，移动到马路点，允许自由方向，不允许中途旋转
             Pose2d vec(action.streetnode().x() - action.spacenode().x(),
                        action.streetnode().y() - action.spacenode().y(), 0);
             action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
             action.mutable_streetnode()->set_l(0.2);
             action.mutable_streetnode()->set_w(0.1);
+            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__);
+            }
+
         } else {
             std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff
                       << std::endl;
@@ -939,6 +963,9 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
     double l = fabs(targetNode.l());
     double w = fabs(targetNode.w());
     double theta = -targetNode.theta();
+    if (newUnfinished_cations_.front().type() == 1 && targetNode.id().find('S')!=-1){//入库只判断前后方向上是否到达
+        tx = x;
+    }
 
     if (l < 1e-10 || w < 1e-10) {
         printf("IsArrived: Error target l or w == 0\n");
@@ -1099,7 +1126,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     }
     printf(" exec along autoDirect:%d\n", autoDirect);
     while (cancel_ == false) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+        std::this_thread::sleep_for(std::chrono::milliseconds(30));
         if (pause_ == true) {
             //发送暂停消息
             continue;
@@ -1143,16 +1170,33 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             if (out[0] < 0 && out[0] > -limit_v.min)
                 out[0] = -limit_v.min;
         }
-        //行进方向上最后一米不纠偏
-        if(fabs(target_in_agv.x()) < 1.4){
-            out[1] = 0.0;
+
+        //放大角速度
+        out[1] *= 10;
+
+        //入库，行进方向上最后一米不纠偏
+        if (newUnfinished_cations_.front().type() == 1 && node.id().find('S')!=-1){//入库
+            if(fabs(target_in_agv.x()) < 1.4){
+                printf("target_in_agv: %f", target_in_agv.x());
+                out[1] = 0.0;
+            }
+        }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),
+                            timedPose_.Get());
+            if(fabs(agv_in_space.x()) < 1.3){
+                printf("agv_in_space: %f", agv_in_space.x());
+                out[1] = 0.0;
+            }
         }
+
         //下发速度
         //printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
         //添加车位号,距目标点距离信息
         double distance = Pose2d::distance(target_in_agv, Pose2d(0,0,0));
         int space_id = 0;
-        if(node.id().find('S')){
+        if(node.id().find('S') != -1){
             std::string id = node.id().substr(1,node.id().length()-1);
             space_id = stoi(id);
         }
@@ -1182,7 +1226,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
         return false;
     }
     Pose2d current = timedPose_.Get();
-    if(targetNode.id().find('S')){
+    if(targetNode.id().find('S') != -1){
         //车位点强制可到达
         return true;
     }
@@ -1202,7 +1246,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
 //    }
     double W = 2.5;
     double L = 1.565;
-    double minYaw = 3 * M_PI / 180.0;
+    double minYaw = 5 * M_PI / 180.0;
     if (move_mode_ == eDouble) {
         L = 1.565 + 2.78;
     }
@@ -1269,7 +1313,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     cancel_ = false;
     running_ = true;
     actionType_ = eReady;
-    printf(" navigation beg...\n");
+    printf("Navigation beg...\n");
 
     while (newUnfinished_cations_.empty() == false && cancel_ == false) {
         std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
@@ -1289,7 +1333,6 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 isInSpace_ = false;
             }
         } else if (act.type() == 3 || act.type() == 4) { //马路导航
-
             if (!execute_nodes(act))
                 break;
         } else if (act.type() == 5) {

MPC/navigation_main.cpp

@@ -177,7 +177,7 @@ bool NavigationMain::lifter_down() {
                 printf("timed lifter is timeout\n");
                 return false;
             }
-            if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get() == eRose) {
+            if (timed_lifter_.Get() == eDowned && timed_other_lifter_.Get() == eDowned) {
                 printf("双车提升机构下降completed!!!\n");
                 return true;
             }
@@ -213,28 +213,25 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         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();
-        if (move_mode_ == eSingle) {
-            printf("确定车位点：eSingle\n");
-        }
     }
 
     if (move_mode_ == eDouble){
         x -= wheelbase/2 * cos(rotated.theta());
         y -= wheelbase/2 * sin(rotated.theta());
-        printf("确定车位点：eDouble\n");
+        printf("RotateBeforeEnterSpace | 整车模式, __LINE__ = %d\n", __LINE__);
     }
-    std::cout << "===============================> RotateBeforeEnterSpace ,target:" << rotated << std::endl;
+
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
     target.set_l(0.05);
     target.set_w(0.05);
     target.set_id(space.id());
+    printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
 
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));

MPC/rotate_mpc.cpp

@@ -8,8 +8,9 @@
 #include <cppad/ipopt/solve.hpp>
 
 size_t mpc_rotate_steps = 20;                  //优化考虑后面多少步
-size_t delay = 3;  // 发送到执行的延迟，即几个周期的时间
+size_t mpc_rotate_delay = 2;  // 发送到执行的延迟，即几个周期的时间
 #define N mpc_rotate_steps
+#define delay mpc_rotate_delay
 
 class FG_eval_rotate {
 public:
@@ -169,8 +170,14 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
     }
     /// limit of angular acceleration
     for (auto i = N; i < 2 * N; ++i) {
-        constraints_lowerbound[i] = -max_angular_acceleration_velocity;
-        constraints_upperbound[i] = max_angular_acceleration_velocity;
+        //延迟处理,前delay个周期内认定为匀速运动(加速度为0)
+        if (i < N + delay) {
+            constraints_lowerbound[i] = 0;
+            constraints_upperbound[i] = 0;
+        } else{
+            constraints_lowerbound[i] = -max_angular_acceleration_velocity;
+            constraints_upperbound[i] = max_angular_acceleration_velocity;
+        }
     }
     /// limit of obs distance
     if (find_obs)
@@ -192,26 +199,6 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
 
     Pose2d targetAngularInAGV = current_to_target;
-    /// 调整到适合的目标角度
-//    if (targetAngularInAGV.theta() > 0) {
-////        double acc_time = (max_angular_velocity - angular_velocity) / max_angular_acceleration_velocity;
-////        double max_angular_once = max_angular_velocity * N * dt - acc_time * (max_angular_velocity - angular_velocity);
-//        double max_angular_once = 9.0 / 180 * M_PI;
-//        if (targetAngularInAGV.theta() > max_angular_once ) {
-//            targetAngularInAGV.set_theta(max_angular_once);
-//        }
-//    }
-//    else if (targetAngularInAGV.theta() < 0) {
-////        double acc_time = fabs((-max_angular_velocity - angular_velocity) / -max_angular_acceleration_velocity);
-////        double max_angular_once = -max_angular_velocity * N * dt + acc_time * (-max_angular_velocity - angular_velocity);
-//        double max_angular_once = -9.0 / 180 * M_PI;
-//        if (targetAngularInAGV.theta() < max_angular_once ) {
-//            targetAngularInAGV.set_theta(max_angular_once);
-//        }
-//    } else{
-//
-//    }
-//    printf("target: %f\n",targetAngularInAGV.theta()/M_PI*180);
 
     Eigen::VectorXd condition(4);
     condition << dt, targetAngularInAGV.theta(), obs_w_, obs_h_;
@@ -254,7 +241,7 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     // 输出第一步的角速度
     out.clear();
-    double solve_angular_velocity = solution.x[N];
+    double solve_angular_velocity = solution.x[N+delay];
 
     out.push_back(solve_angular_velocity);