1.优化mpc计算unknown时，下发反馈的实时速度

MPC/custom_type.h

@@ -63,7 +63,8 @@ private:
 enum MpcError {
     success = 0,
     no_solution = 1,
-    failed
+    failed = 2,
+    unknown
 };
 //////////////////////////////////////////////////
 

MPC/loaded_mpc.cpp

@@ -370,6 +370,8 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
                wmg, angular, max_velocity_);
         if (solution.status == CppAD::ipopt::solve_result<Dvector>::local_infeasibility)
             return no_solution;
+        if (solution.status == CppAD::ipopt::solve_result<Dvector>::unknown)
+            return unknown;
         return failed;
     }
 

MPC/navigation.cpp

@@ -592,8 +592,6 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         return true;
     }
     bool already_in_mpc = false;
-    int retry_count = 0;
-    int retry_count_limit = 3;
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         if (PoseTimeout()) {
@@ -638,10 +636,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
             printf(" MPC to target:%f %f l:%f,w:%f theta:%f  impossible ,retry ...\n",
                    node.x(), node.y(), node.l(), node.w(), node.theta());
         } else {
-            retry_count++;
-            printf("MpcToTarget failed,ret:%d\n", ret);
-            if (retry_count > retry_count_limit)
-                return false;
+
         }
     }
     if (cancel_) {
@@ -1234,7 +1229,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
     return ret == success || ret == no_solution;
 }
 
-bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY) {
+bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, int mpc_result, bool directY) {
     if (PoseTimeout() == true) {
         printf(" MPC once Error:Pose is timeout \n");
         return false;
@@ -1297,6 +1292,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     //       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);
+    mpc_result = ret;
 
     //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
     if (ret == no_solution) {
@@ -1309,6 +1305,18 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
         out.push_back(0);
         out.push_back(0);
     }
+
+    if (ret == unknown) {
+        printf(" mpc solve unknown set v/w = current\n");
+        double current_v = timedV_.Get(), current_w = timedA_.Get();
+        out.clear();
+        out.push_back(current_v);
+        out.push_back(current_w);
+        out.push_back(current_v);
+        out.push_back(current_w);
+        out.push_back(current_v);
+        out.push_back(current_w);
+    }
     //
 //    diff_yaw.push_back(0);
 //    diff_yaw.push_back(0);
@@ -1733,6 +1741,9 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     ofs.open(log_file_dir + log_file_name, std::ios::app);  //打开的地址和方式
     auto beg_time = std::chrono::steady_clock::now();
 
+    int retry_count = 0;
+    int retry_count_limit = 3;
+
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(20));
         if (pause_ == true) {
@@ -1800,13 +1811,27 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         std::vector<double> out;
 //        std::vector<double> diff_yaw; //两车分别与整车的相对角度
         bool ret;
+        int mpc_result = 0;
         TimerRecord::Execute([&, this]() {
-            ret = mpc_once(traj, limit_v, out, directY);
+            ret = mpc_once(traj, limit_v, out, mpc_result, directY);
         }, "mpc_once");
+
         if (ret == false) {
-            SlowlyStop();
-            ofs.close();//关闭文件
-            return eMpcFailed;
+            if (mpc_result == unknown){
+                retry_count++;
+                if (retry_count > retry_count_limit) {
+                    printf("MpcToTarget failed,ret:%d\n", ret);
+                    SlowlyStop();
+                    ofs.close();//关闭文件
+                    return eMpcFailed;
+                }
+                printf("MpcToTarget failed,mpc_result:%d, retry...\n", mpc_result);
+            } else {
+                printf("MpcToTarget failed,ret:%d\n", ret);
+                SlowlyStop();
+                ofs.close();//关闭文件
+                return eMpcFailed;
+            }
         }
         const int down_count = 3;//下发前多少步
         double down_v[down_count] = {out[0], out[2], out[4]};//下发线速度

MPC/navigation.h

@@ -218,7 +218,7 @@ protected:
 
     virtual int GetSpaceId(NavMessage::PathNode spaceNode);
 
-    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);
+    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, int mpc_result, bool directY = false);
 
     /* 原地旋转mpc
      *