1.回退下发主副车角度改动

2.修复整车模式下发轴距为0Bug
3.防撞开启

MPC/loaded_mpc.cpp

@@ -25,10 +25,12 @@ public:
     Eigen::VectorXd m_coeffs;                 //曲线方程
     Eigen::VectorXd m_statu;                  //当前状态
     Eigen::VectorXd m_condition;              //搜索条件参数
-    FG_eval_half_agv(Eigen::VectorXd coeffs, Eigen::VectorXd statu, Eigen::VectorXd condition) {
+    bool directY_;
+    FG_eval_half_agv(Eigen::VectorXd coeffs, Eigen::VectorXd statu, Eigen::VectorXd condition,bool directY) {
         m_coeffs = coeffs;
         m_statu = statu;
         m_condition = condition;
+        directY_=directY;
     }
 
     typedef CPPAD_TESTVECTOR(CppAD::AD<double>) ADvector;
@@ -113,45 +115,29 @@ public:
             fg[1 + ndlt + t] = (vars[ndlt + t] - vars[ndlt + t - 1]) / dt;
         }
 
-        if (m_statu.size() == 2 + 3) {
-            float obs_x = m_statu[2];
-            float obs_y = m_statu[3];
-            float obs_theta = m_statu[4];
-            Pose2d obs_relative_pose(obs_x, obs_y, obs_theta);
-            std::vector<Pose2d> obs_vertexes = Pose2d::generate_rectangle_vertexs(obs_relative_pose, obs_w * 2,
-                                                                                  obs_h * 2);
-            for (auto t = 0; t < N/2; ++t) {
-                CppAD::AD<double> min_distance = 1e19-1;
-                for (auto i = 0; i < 8; ++i) {
-                    CppAD::AD<double> ra = (obs_vertexes[i].x() - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
-                                           (obs_vertexes[i].y() - vars[ny + t]) * CppAD::sin(vars[nth + t]);
-                    CppAD::AD<double> rb = (obs_vertexes[i].x() - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
-                                           (obs_vertexes[i].y() - vars[ny + t]) * CppAD::cos(vars[nth + t]);
-                    CppAD::AD<double> distance = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
-                    if (distance < min_distance) min_distance = distance;
+
+        if (m_statu.size() == 2 + 16) {
+            //与障碍物的距离
+            for (int i = 0; i < 8; ++i) {
+                double ox = m_statu[2 + i * 2];
+                double oy = m_statu[2 + i * 2 + 1];
+                for (int t = 0; t < N; ++t) {
+                    /*第i点的矩形方程为:   {[(x-xi)cos(Θi)-(y-yi)sin(Θi)]/1.35}**8  +
+                     *                  {[(x-xi)sin(Θi)+(y-yi)cos(Θi)]/0.75}**8  = 1.0
+                     *
+                     * */
+                    CppAD::AD<double> ra = (ox - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
+                                           (oy - vars[ny + t]) * CppAD::sin(vars[nth + t]);
+                    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);
+                    else
+                        fg[1 + nobs + N * i + t] = CppAD::pow(ra / (obs_w*0.6), 8) + CppAD::pow(rb / obs_h, 8);
                 }
-                fg[1 + nobs + t] = min_distance;
             }
-//            std::cout << " fg[n_obs]: " << fg[n_obs] << " | __LINE__:" << __LINE__ << std::endl;
         }
-//        if (m_statu.size() == 2 + 3) {
-//            //与障碍物的距离
-//            for (int i = 0; i < 8; ++i) {
-//                double ox = m_statu[2 + i * 2];
-//                double oy = m_statu[2 + i * 2 + 1];
-//                for (int t = 0; t < N; ++t) {
-//                    /*第i点的矩形方程为:   {[(x-xi)cos(Θi)-(y-yi)sin(Θi)]/1.35}**8  +
-//                     *                  {[(x-xi)sin(Θi)+(y-yi)cos(Θi)]/0.75}**8  = 1.0
-//                     *
-//                     * */
-//                    CppAD::AD<double> ra = (ox - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
-//                                           (oy - vars[ny + t]) * CppAD::sin(vars[nth + t]);
-//                    CppAD::AD<double> rb = (ox - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
-//                                           (oy - vars[ny + t]) * CppAD::cos(vars[nth + t]);
-//                    fg[1 + nobs + N * i + t] = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
-//                }
-//            }
-//        }
+
     }
 };
 
@@ -166,7 +152,8 @@ LoadedMPC::LoadedMPC(const Pose2d &obs, double obs_w, double obs_h, double min_v
 LoadedMPC::~LoadedMPC() {}
 
 MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd statu, MPC_parameter mpc_param,
-                          std::vector<double> &out, Trajectory &select_traj, Trajectory &optimize_trajectory) {
+                          std::vector<double> &out, Trajectory &select_traj,
+                          Trajectory &optimize_trajectory,bool directY) {
     auto start = std::chrono::steady_clock::now();
     // State vector holds all current values neede for vars below
     Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
@@ -247,14 +234,17 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     }
 
     // Lower and upper limits for the constraints
+    size_t n_constraints = N * 5;
     /*
      * 障碍物是否进入 碰撞检测范围内
      */
     float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
-    distance = 1000;
-    bool find_obs = distance < 10;
-    if (find_obs) printf(" find obs ..............................\n");
-    size_t n_constraints = 5 * N + find_obs * N/2;
+    bool find_obs = false;
+    if (distance < 20) {
+        printf(" mpc find obs ,w=%f,h=%f\n",obs_w_,obs_h_);
+        find_obs = true;
+    }
+    if (find_obs) n_constraints = N * (5 + 8);
 
     Dvector constraints_lowerbound(n_constraints);
     Dvector constraints_upperbound(n_constraints);
@@ -277,7 +267,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
         constraints_lowerbound[i] = -max_acc_dlt;
         constraints_upperbound[i] = max_acc_dlt;
         //延迟处理,前delay个周期内认定为匀速运动(加速度为0)
-        if (i < nv+delay){
+        if (i < ndlt+delay){
             constraints_lowerbound[i] = 0;
             constraints_upperbound[i] = 0;
         }
@@ -285,7 +275,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     // 与障碍物保持距离的约束
     if (find_obs) {
-        for (int i = nobs; i < nobs + N/2; ++i) {
+        for (int i = nobs; i < nobs + 8 * N; ++i) {
             constraints_lowerbound[i] = 1.0;
             constraints_upperbound[i] = 1e19;
         }
@@ -311,18 +301,23 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
         angular = -max_dlt;
     }
 
-    Eigen::VectorXd status(2 + 3 * find_obs);
-    status[0] = line_velocity;
-    status[1] = angular;
+    Eigen::VectorXd statu_velocity(2);
     if (find_obs) {
-        status[2]= obs_relative_pose_.x();
-        status[3] = obs_relative_pose_.y();
-        status[4] = obs_relative_pose_.theta();
+        statu_velocity = Eigen::VectorXd(2 + 16);
+
+        std::vector<Pose2d> vertexs = Pose2d::generate_rectangle_vertexs(obs_relative_pose_, obs_w_ * 2, obs_h_ * 2);
+        for (int i = 0; i < vertexs.size(); ++i) {
+            //std::cout<<"vetex:"<<vertexs[i]<<std::endl;
+            statu_velocity[2 + i * 2] = vertexs[i].x();
+            statu_velocity[2 + i * 2 + 1] = vertexs[i].y();
+        }
     }
+    statu_velocity[0] = line_velocity;
+    statu_velocity[1] = angular;
 
     Eigen::VectorXd condition(6);
     condition << dt, ref_velocity, obs_w_, obs_h_, targetPoseInAGV.x(), targetPoseInAGV.y();
-    FG_eval_half_agv fg_eval(coef, status, condition);
+    FG_eval_half_agv fg_eval(coef, statu_velocity, condition,directY);
 
     // options for IPOPT solver
     std::string options;

MPC/loaded_mpc.h

@@ -23,7 +23,8 @@ public:
     virtual ~LoadedMPC();
 
     virtual MpcError solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd statu, MPC_parameter mpc_param,
-                           std::vector<double> &out, Trajectory &select_traj, Trajectory &optimize_trajectory);
+                           std::vector<double> &out, Trajectory &select_traj,
+                           Trajectory &optimize_trajectory,bool directY);
 
 
 protected:

MPC/navigation.cpp

@@ -1020,8 +1020,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 = 0.95;//0.85;
-    double obs_h = 1.55;//1.45;
+    double obs_w =1.5;// 0.95;//0.85;
+    double obs_h =3.0;// 1.55;//1.45;
 
     if (directY == true) {
         //将车身朝向旋转90°，车身朝向在(-pi,pi]
@@ -1029,8 +1029,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 = 0.95;//0.85;
-        obs_h = 1.55;//1.45;
+        obs_w = 3.0;// 0.95;//0.85;
+        obs_h = 1.5;//1.55;//1.45;
     }
 
     Pose2d brother = timedBrotherPose_.Get();
@@ -1038,7 +1038,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);
 
@@ -1049,7 +1049,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);
+                    out, selected_trajectory, optimize_trajectory,directY);
 
     //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
     if (ret == no_solution) {
@@ -1556,6 +1556,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             isInSpace_ = true;
             if (!execute_InOut_space(act)) {
                 printf(" In space failed\n");
+                isInSpace_ = false;
                 break;
             }
         } else if (act.type() == 2) {  //出库