MPC/loaded_mpc.cpp

@@ -221,12 +221,12 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     //printf("min_v:%f   max_v:%f\n",min_velocity_,max_velocity_);
     double max_dlt = max_wmg;//5*M_PI/180.0;
-    double acc_sigmoid= (max_velocity_+1.3)/(1+exp(-4.*(fabs(line_velocity)-0.7)))+0.3;
+    double acc_sigmoid= (max_velocity_+1.15)/(1+exp(-4.*(fabs(line_velocity)-0.7)))+0.45;
     double max_acc_line_velocity = mpc_param.acc_velocity*acc_sigmoid;
     double max_acc_dlt = mpc_param.acc_angular;
 
     double ref1=sqrt(0.5*dis/mpc_param.acc_velocity)+min_velocity_;
-    double ref2=2.5/(1.+exp(-2.5*(dis-1.1)))-0.15+min_velocity_;
+    double ref2=2.5/(1.+exp(-2.5*(dis-1.2)))-0.12+min_velocity_;
     double ref_velocity = std::min(ref1,ref2);//max_velocity_/(1+exp(-4.*(dis-0.8)))+min_velocity_;
 
     Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);

MPC/navigation.cpp

@@ -666,6 +666,21 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
         ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
             NavMessage::PathNode node = action.pathnodes(i);
+
+            if (i == 0) {
+                bool x_enable = PossibleToTarget(node, false, 1);
+                bool y_enable = PossibleToTarget(node, true, 1);
+                if (x_enable == false && y_enable == false) {
+                    stLimit l_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
+                    int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+                    bool adjustRet = AdjustReferToTarget(Pose2d(node.x(), node.y(), 0),
+                                                         l_w, directions);
+                    if (adjustRet == false) {
+                        return false;
+                    }
+                }
+            }
+
             if (i < action.pathnodes_size() - 1) {
                 NavMessage::PathNode next = action.pathnodes(i + 1);
                 Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
@@ -677,7 +692,7 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
                 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.25);
+                node.set_w(0.2);
             }
             int directions = Direction::eForward;
             if (anyDirect)
@@ -711,13 +726,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
             }*/
 
             //提升机构提前动作
-            if (GetRemainActionsCount() > 1){
+            if (GetRemainActionsCount() > 1) {
                 NavMessage::NewAction last_action = GetLastAction();
-                if (IsUperSpace(last_action.spacenode())){
-                    if ((i+1) == action.pathnodes_size()){
+                if (IsUperSpace(last_action.spacenode())) {
+                    if ((i + 1) == action.pathnodes_size()) {
                         clampLifterAction |= eByteLifterUp;
                     }
-                } else{
+                } else {
                     clampLifterAction |= eByteLifterDown;
                 }
             }
@@ -767,12 +782,12 @@ Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.7 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" DoubleOutSpaceCorrectTheta refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
-            if (WaitClampLifterStatu(clampLifterAction) == false) {
-                return eMpcFailed;
-            }
+//            if (WaitClampLifterStatu(clampLifterAction) == false) {
+//                return eMpcFailed;
+//            }
             return eMpcSuccess;
         } else {
             const int down_count = 3;
@@ -796,7 +811,18 @@ Navigation::MpcResult Navigation::RotateAfterOutExport() {
     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};
+    stLimit limit_rotate = {parameter_.r_mpc_parameter().min(), parameter_.r_mpc_parameter().max()};
+
+    //文件log
+    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_Rotate.txt", localtime(&t));
+    std::string log_file_name = tmp;
+    ofs.open(log_file_dir + log_file_name, std::ios::app);  //打开的地址和方式
+    auto beg_time = std::chrono::steady_clock::now();
+
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(80));
         Pose2d current = timedPose_.Get();
@@ -810,13 +836,31 @@ Navigation::MpcResult Navigation::RotateAfterOutExport() {
         }, "Rotation_mpc_once");
         if (ret == false) {
             Stop();
+            ofs.close();
             return eMpcFailed;
         }
 
+        std::string log_inf_line = "";
+        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;
+        log_inf_line += std::to_string(time);
+
+        log_inf_line += " ";
+        log_inf_line += std::to_string(timedA_.Get());
+        log_inf_line += " ";
+        log_inf_line += std::to_string(out[0]);
+
+        ofs << log_inf_line << std::endl;  //一行日志的内容
+
+
         //下发速度
         if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
+            ofs.close();
             return eMpcSuccess;
         } else {
             const int down_count = 3;
@@ -829,6 +873,7 @@ Navigation::MpcResult Navigation::RotateAfterOutExport() {
         }
         continue;
     }
+    ofs.close();
     return eMpcFailed;
 }
 
@@ -933,6 +978,11 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 //        }
 //    }
 
+
+    double angular_pericision = 0.5;
+    if (GetSequenceType() == eSequencePick && TargetIsExport(space)) {
+        angular_pericision = 5;
+    }
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(30));
         Pose2d current_now = timedPose_.Get();
@@ -952,7 +1002,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < angular_pericision * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             return eMpcSuccess;
@@ -1026,12 +1076,34 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     stLimit limit_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
     //入库,起点为streetNode
     if (action.type() == 1) {
+        if (TargetIsExport(action.spacenode()) && GetSequenceType() == eSequencePick) {
+
+        } else {
+            Pose2d targetInCurrent = Pose2d::relativePose(Pose2d(action.streetnode().x(),
+                                                                 action.streetnode().y(), 0), timedPose_.Get());
+            if (fabs(targetInCurrent.x()) < 0.5 && fabs(targetInCurrent.y()) < 0.5) {
+                int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+                bool adjustRet = AdjustReferToTarget(Pose2d(action.streetnode().x(), action.streetnode().y(), 0),
+                                                     limit_w, directions);
+                if (adjustRet == false) {
+                    return false;
+                }
+            }
+        }
+
+
         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()) + M_PI / 2.0);
+
+        if (GetSequenceType() == eSequencePick && TargetIsExport(action.spacenode())) {
+            action.mutable_streetnode()->set_l(0.5);
+            action.mutable_streetnode()->set_w(0.5);
+        }
+
         /*
          * 是否需要添加判断，距离较远才先运动到马路点=============================
          */
@@ -1045,6 +1117,19 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         if (move_mode_ == eSingle)
             clampLifterAction |= eByteClampHalfOpen;
 
+
+        bool x_enable = PossibleToTarget(action.streetnode(), false, 1);
+        bool y_enable = PossibleToTarget(action.streetnode(), true, 1);
+        if (x_enable == false && y_enable == false) {
+            stLimit l_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
+            int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+            bool adjustRet = AdjustReferToTarget(Pose2d(action.streetnode().x(), action.streetnode().y(), 0),
+                                                 l_w, directions);
+            if (adjustRet == false) {
+                return false;
+            }
+        }
+
         if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true, clampLifterAction) == false) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
@@ -1162,8 +1247,23 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         printf(" clampLifter_statu1 :%d\n", clampLifterAction);
         if (move_mode_ == eSingle)
             clampLifterAction |= eByteClampFullyOpen;  //////// 后续再改为全打开位
-        else
-            clampLifterAction |= eByteCLampClose;
+        else {
+            if (GetSequenceType() == eSequencePark) {
+                if (TargetIsEntrance(action.spacenode())) {
+                    clampLifterAction |= eByteClampFullyOpen;
+                } else {
+                    clampLifterAction |= eByteCLampClose;
+                }
+
+            } else {
+                if (TargetIsEntrance(action.spacenode())) {
+                    clampLifterAction |= eByteCLampClose;
+                } else {
+                    clampLifterAction |= eByteClampFullyOpen;
+                }
+            }
+        }
+
 
         printf(" clampLifter_statu2 :%d\n", clampLifterAction);
         if (!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode()))
@@ -1171,8 +1271,12 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         else {
             // 整车进 出入口速度调整为与巡线一致
             if (move_mode_ == eDouble) {
-                limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
-                limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+                if (GetSequenceType() == eSequencePark) {
+
+                } else if (GetSequenceType() == eSequencePick) {
+                    limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
+                    limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+                }
             }
         }
         bool retMove = MoveToTarget(new_target, limit_v, limit_w, true, false, clampLifterAction);
@@ -1184,6 +1288,16 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
 
     } else if (action.type() == 2) {
         //出库,起点为spaceNode
+        if (GetSequenceType() == eSequencePick && TargetIsExport(action.spacenode())) {
+            Pose2d current_AGV = timedPose_.Get();
+            double new_street_x = (action.streetnode().y() - current_AGV.y()) * cos(current_AGV.theta())
+                                  / sin(current_AGV.theta()) + current_AGV.x();
+
+            action.mutable_spacenode()->set_x(current_AGV.x());
+            action.mutable_spacenode()->set_y(current_AGV.y());
+
+            action.mutable_streetnode()->set_x(new_street_x);
+        }
         // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿
         Pose2d vec(action.streetnode().x() - action.spacenode().x(),
                    action.streetnode().y() - action.spacenode().y(), 0);
@@ -1193,8 +1307,10 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(), current.y(), 0), space));
         // 整车从出入口出库速度调整为与巡线一致
         if (move_mode_ == eDouble && (TargetIsExport(action.spacenode()) || TargetIsExport(action.spacenode()))) {
-            limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
-            limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+            if (GetSequenceType() == eSequencePark) {
+                limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
+                limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+            }
         }
         if (fabs(diff.x()) < 0.15 || fabs(diff.y()) < 0.10) {
             //出库，移动到马路点，允许自由方向，不允许中途旋转
@@ -1214,7 +1330,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         isInSpace_ = false;
 
         // 存车流程离开入口，掉头
-        printf("SequenceType: %d\n",GetSequenceType());
+        printf("SequenceType: %d\n", GetSequenceType());
         if (GetSequenceType() == eSequencePark) {
             if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
                 Pose2d current_pose = timedPose_.Get();
@@ -1259,7 +1375,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
     statu[3] = line_velocity;
     statu[4] = angular_velocity;
 
-    NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
+    NavParameter::R_mpc_parameter mpc_parameter = parameter_.r_mpc_parameter();
     double obs_w = 0.95;//0.85;
     double obs_h = 1.55;//1.45;
 
@@ -1271,8 +1387,8 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
         brother_in_self = Pose2d(pose.x() + 100, pose.y() + 100, 0);
     }
     RotateMPC MPC(brother_in_self, obs_w, obs_h, limit_wmg.min, limit_wmg.max);
-    RotateMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
-                                          mpc_parameter.acc_velocity(), mpc_parameter.acc_angular()};
+    RotateMPC::MPC_parameter parameter = {0, mpc_parameter.dt(),
+                                          0, mpc_parameter.acc_angular()};
     ret = MPC.solve(current_to_target, statu, parameter, out);
     if (ret == no_solution) {
         printf(" Rotation_mpc solve no solution set v/w = 0\n");
@@ -1297,7 +1413,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, int& mpc_result, 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;
@@ -1363,27 +1479,49 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     mpc_result = ret;
 
     //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
-    if (ret == no_solution) {
-        printf(" mpc solve no solution set v/w = 0\n");
-        out.clear();
-        out.push_back(0);
-        out.push_back(0);
-        out.push_back(0);
-        out.push_back(0);
-        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();
+    double current_v = timedV_.Get(), current_w = timedA_.Get();
+    if (ret != success) {
         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);
+        int v_bool_flag;
+        if (current_v > 0) {
+            v_bool_flag = 1;
+        } else if (current_v < 0) {
+            v_bool_flag = -1;
+        } else {
+            v_bool_flag = 0;
+        }
+        double new_v = current_v - v_bool_flag * mpc_parameter.acc_velocity()/2 * mpc_parameter.dt();
+        if ((current_v * new_v) < 0){
+            new_v = 0;
+        }
+
+        int w_bool_flag;
+        if (current_w > 0) {
+            w_bool_flag = 1;
+        } else if (current_w < 0) {
+            w_bool_flag = -1;
+        } else {
+            w_bool_flag = 0;
+        }
+        double new_w = current_w - w_bool_flag * mpc_parameter.acc_angular() * mpc_parameter.dt();
+        if ((current_w * new_w) < 0){
+            new_w = 0;
+        }
+
+        out.push_back(new_v);
+        out.push_back(new_w);
+        out.push_back(new_v);
+        out.push_back(new_w);
+        out.push_back(new_v);
+        out.push_back(new_w);
+        if (ret == unknown) {
+            printf(" mpc solve unknown set v/w = current-acc*dt. cur_v:%f, down_v:%f\n",current_v, new_v);
+        } else if (ret == no_solution) {
+            printf(" mpc solve no solution set v/w = 0\n");
+        } else {
+            printf(" mpc solve failed set v/w = 0\n");
+        }
     }
     //
 //    diff_yaw.push_back(0);
@@ -1391,7 +1529,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
 
     predict_traj_.reset(optimize_trajectory, 1);
     selected_traj_.reset(selected_trajectory, 1);
-    return ret == success || ret == no_solution;
+    return ret == success;
 }
 
 bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
@@ -1419,7 +1557,7 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
     double b = (x - tx) * sin(theta) + (y - ty) * cos(theta);
 
     if (pow(a / l, 8) + pow(b / w, 8) <= 1) {
-        if (fabs(timedV_.Get()) < 0.06 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(timedV_.Get()) < 0.1 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" Arrived target: %f  %f l:%f w:%f theta:%f\n", tx, ty, l, w, theta);
             return true;
         }
@@ -1885,7 +2023,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         }, "mpc_once");
 
         if (ret == false) {
-            if (mpc_result == unknown){
+            if (mpc_result == unknown) {
                 retry_count++;
                 if (retry_count > retry_count_limit) {
                     printf("MpcToTarget failed,ret:%d\n", ret);
@@ -2027,7 +2165,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     return eMpcFailed;
 }
 
-bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY) {
+bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY, double wheel_yaw) {
     if (timedPose_.timeout()) {
         return false;
     }
@@ -2052,7 +2190,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
 //    }
     double W = 2.5;
     double L = 1.565;
-    double minYaw = 5 * M_PI / 180.0;
+    double minYaw = wheel_yaw * M_PI / 180.0;
     if (move_mode_ == eDouble) {
         L = 1.565 + 2.78;
     }
@@ -2062,6 +2200,13 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
         W = L;
         L = t;
     }
+
+    /*Pose2d targetInCurrent=Pose2d::relativePose(Pose2d(tx,ty,theta),current);
+    if(fabs(targetInCurrent.x())>0.1 && atan(fabs(targetInCurrent.y()/targetInCurrent.x()))>30.0*M_PI/180.0 ){
+        std::cout<<"impossible to target, target in pose:"<<targetInCurrent<<", atan >20"<<std::endl;
+        return false;
+    }*/
+
     double minR = W / 2 + L / tan(minYaw);
     //printf("l:%f,w:%f\n",l,w);
     std::vector<Pose2d> poses = Pose2d::generate_rectangle_vertexs(Pose2d(tx, ty, 0), l * 2, w * 2);
@@ -2114,6 +2259,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     }
     //add 先检查当前点与起点的距离
     global_navCmd_ = cmd;
+    printf("SequenceType: %d\n", GetSequenceType());
 
     for (int i = 0; i < cmd.newactions_size(); ++i)
         newUnfinished_cations_.push(cmd.newactions(i));
@@ -2169,7 +2315,8 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 break;
             }
         } else if (act.type() == 7) {
-            if (this->clamp_fully_open() == false) {
+            ret = clamp_fully_open();
+            if (ret == false) {
                 printf("打开夹持 failed...\n");
                 break;
             }
@@ -2475,11 +2622,11 @@ NavMessage::NewAction Navigation::GetLastAction() {
 }
 
 int Navigation::GetSequenceType() {
-    if (global_navCmd_.has_sequence()){
-        return eSequenceDefault;
-    }
-    else{
+    if (global_navCmd_.has_sequence()) {
         return global_navCmd_.sequence();
+
+    } else {
+        return eSequenceDefault;
     }
 }
 

MPC/navigation.h

@@ -158,7 +158,7 @@ protected:
 
     bool IsArrived(NavMessage::PathNode targetNode);
 
-    bool PossibleToTarget(NavMessage::PathNode targetNode, bool directY);
+    bool PossibleToTarget(NavMessage::PathNode targetNode, bool directY,double wheel_yaw=5.0);
 
     /*
      * 移动到目标点，不能到达则旋转

MPC/navigation_main.cpp

@@ -326,6 +326,11 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         return eMpcSuccess;
     }
 
+    double angular_pericision = 0.5;
+    if(GetSequenceType() == eSequencePick && TargetIsExport(space)){
+        angular_pericision = 5;
+    }
+
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         Pose2d current_now = timedPose_.Get();
@@ -351,7 +356,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 3 * M_PI / 180.0) {
+        if (fabs(yawDiff) < angular_pericision * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             if(WaitClampLifterStatu(clampLifterAction)==false)

MPC/navigation_main.h

@@ -53,14 +53,14 @@ public:
             Pose2d brother_pose = timedBrotherPose_.Get();
             Pose2d brother_in_self = Pose2d::relativePose(brother_pose, self_pose);
 
-            if (brother_in_self.x() <= 0)
-                mode = mode | eMainInForward;
-            else
-                mode = mode | eMainInBackward;
-            if (brother_in_self.theta() >= -M_PI / 2 && brother_in_self.theta() <= M_PI / 2)
-                mode = mode | eChildSameToMain;
-            else
-                mode = mode | eChildOppositeToMain;
+//            if (brother_in_self.x() <= 0)
+//                mode = mode | eMainInForward;
+//            else
+//                mode = mode | eMainInBackward;
+//            if (brother_in_self.theta() >= -M_PI / 2 && brother_in_self.theta() <= M_PI / 2)
+//                mode = mode | eChildSameToMain;
+//            else
+//                mode = mode | eChildOppositeToMain;
 
             printf(" Switch MoveMode --> main(%d), wheelBase: %f\n", mode, wheelBase);
         }

MPC/parameter.pb.cc

@@ -178,6 +178,28 @@ struct AccuracyDefaultTypeInternal {
 PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
     PROTOBUF_ATTRIBUTE_INIT_PRIORITY1 AccuracyDefaultTypeInternal _Accuracy_default_instance_;
 template <typename>
+PROTOBUF_CONSTEXPR R_mpc_parameter::R_mpc_parameter(
+    ::_pbi::ConstantInitialized): _impl_{
+    /*decltype(_impl_.dt_)*/ 0
+
+  , /*decltype(_impl_.acc_angular_)*/ 0
+
+  , /*decltype(_impl_.min_)*/ 0
+
+  , /*decltype(_impl_.max_)*/ 0
+
+  , /*decltype(_impl_._cached_size_)*/{}} {}
+struct R_mpc_parameterDefaultTypeInternal {
+  PROTOBUF_CONSTEXPR R_mpc_parameterDefaultTypeInternal() : _instance(::_pbi::ConstantInitialized{}) {}
+  ~R_mpc_parameterDefaultTypeInternal() {}
+  union {
+    R_mpc_parameter _instance;
+  };
+};
+
+PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
+    PROTOBUF_ATTRIBUTE_INIT_PRIORITY1 R_mpc_parameterDefaultTypeInternal _R_mpc_parameter_default_instance_;
+template <typename>
 PROTOBUF_CONSTEXPR Navigation_parameter::Navigation_parameter(
     ::_pbi::ConstantInitialized): _impl_{
     /*decltype(_impl_._has_bits_)*/{}
@@ -194,6 +216,7 @@ PROTOBUF_CONSTEXPR Navigation_parameter::Navigation_parameter(
   , /*decltype(_impl_.inoutvlimit_)*/nullptr
   , /*decltype(_impl_.nodevelocitylimit_)*/nullptr
   , /*decltype(_impl_.nodeangularlimit_)*/nullptr
+  , /*decltype(_impl_.r_mpc_parameter_)*/nullptr
   , /*decltype(_impl_.main_agv_)*/ false
 } {}
 struct Navigation_parameterDefaultTypeInternal {
@@ -207,7 +230,7 @@ struct Navigation_parameterDefaultTypeInternal {
 PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
     PROTOBUF_ATTRIBUTE_INIT_PRIORITY1 Navigation_parameterDefaultTypeInternal _Navigation_parameter_default_instance_;
 }  // namespace NavParameter
-static ::_pb::Metadata file_level_metadata_parameter_2eproto[7];
+static ::_pb::Metadata file_level_metadata_parameter_2eproto[8];
 static constexpr const ::_pb::EnumDescriptor**
     file_level_enum_descriptors_parameter_2eproto = nullptr;
 static constexpr const ::_pb::ServiceDescriptor**
@@ -286,6 +309,18 @@ const ::uint32_t TableStruct_parameter_2eproto::offsets[] PROTOBUF_SECTION_VARIA
     ~0u,  // no sizeof(Split)
     PROTOBUF_FIELD_OFFSET(::NavParameter::Accuracy, _impl_.l_),
     PROTOBUF_FIELD_OFFSET(::NavParameter::Accuracy, _impl_.w_),
+    ~0u,  // no _has_bits_
+    PROTOBUF_FIELD_OFFSET(::NavParameter::R_mpc_parameter, _internal_metadata_),
+    ~0u,  // no _extensions_
+    ~0u,  // no _oneof_case_
+    ~0u,  // no _weak_field_map_
+    ~0u,  // no _inlined_string_donated_
+    ~0u,  // no _split_
+    ~0u,  // no sizeof(Split)
+    PROTOBUF_FIELD_OFFSET(::NavParameter::R_mpc_parameter, _impl_.dt_),
+    PROTOBUF_FIELD_OFFSET(::NavParameter::R_mpc_parameter, _impl_.acc_angular_),
+    PROTOBUF_FIELD_OFFSET(::NavParameter::R_mpc_parameter, _impl_.min_),
+    PROTOBUF_FIELD_OFFSET(::NavParameter::R_mpc_parameter, _impl_.max_),
     PROTOBUF_FIELD_OFFSET(::NavParameter::Navigation_parameter, _impl_._has_bits_),
     PROTOBUF_FIELD_OFFSET(::NavParameter::Navigation_parameter, _internal_metadata_),
     ~0u,  // no _extensions_
@@ -304,6 +339,7 @@ const ::uint32_t TableStruct_parameter_2eproto::offsets[] PROTOBUF_SECTION_VARIA
     PROTOBUF_FIELD_OFFSET(::NavParameter::Navigation_parameter, _impl_.nodevelocitylimit_),
     PROTOBUF_FIELD_OFFSET(::NavParameter::Navigation_parameter, _impl_.nodeangularlimit_),
     PROTOBUF_FIELD_OFFSET(::NavParameter::Navigation_parameter, _impl_.rpc_ipport_),
+    PROTOBUF_FIELD_OFFSET(::NavParameter::Navigation_parameter, _impl_.r_mpc_parameter_),
     ~0u,
     0,
     1,
@@ -314,6 +350,7 @@ const ::uint32_t TableStruct_parameter_2eproto::offsets[] PROTOBUF_SECTION_VARIA
     6,
     7,
     ~0u,
+    8,
 };
 
 static const ::_pbi::MigrationSchema
@@ -324,7 +361,8 @@ static const ::_pbi::MigrationSchema
         { 40, -1, -1, sizeof(::NavParameter::MPC_parameter)},
         { 52, -1, -1, sizeof(::NavParameter::SpeedLimit)},
         { 62, -1, -1, sizeof(::NavParameter::Accuracy)},
-        { 72, 90, -1, sizeof(::NavParameter::Navigation_parameter)},
+        { 72, -1, -1, sizeof(::NavParameter::R_mpc_parameter)},
+        { 84, 103, -1, sizeof(::NavParameter::Navigation_parameter)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -334,6 +372,7 @@ static const ::_pb::Message* const file_default_instances[] = {
     &::NavParameter::_MPC_parameter_default_instance_._instance,
     &::NavParameter::_SpeedLimit_default_instance_._instance,
     &::NavParameter::_Accuracy_default_instance_._instance,
+    &::NavParameter::_R_mpc_parameter_default_instance_._instance,
     &::NavParameter::_Navigation_parameter_default_instance_._instance,
 };
 const char descriptor_table_protodef_parameter_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
@@ -351,31 +390,34 @@ const char descriptor_table_protodef_parameter_2eproto[] PROTOBUF_SECTION_VARIAB
     "(\002\022\024\n\014acc_velocity\030\003 \001(\002\022\023\n\013acc_angular\030"
     "\004 \001(\002\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013\n\003max\030"
     "\002 \001(\002\" \n\010Accuracy\022\t\n\001l\030\001 \001(\002\022\t\n\001w\030\002 \001(\002\""
-    "\331\003\n\024Navigation_parameter\022\020\n\010main_agv\030\001 \001"
-    "(\010\0221\n\010Agv_emqx\030\002 \001(\0132\037.NavParameter.AgvE"
-    "mqx_parameter\0223\n\rTerminal_emqx\030\003 \001(\0132\034.N"
-    "avParameter.Emqx_parameter\022/\n\014brother_em"
-    "qx\030\004 \001(\0132\031.NavParameter.BrotherEmqx\0224\n\017x"
-    "_mpc_parameter\030\005 \001(\0132\033.NavParameter.MPC_"
-    "parameter\0224\n\017y_mpc_parameter\030\006 \001(\0132\033.Nav"
-    "Parameter.MPC_parameter\022-\n\013InOutVLimit\030\007"
-    " \001(\0132\030.NavParameter.SpeedLimit\0223\n\021NodeVe"
-    "locityLimit\030\010 \001(\0132\030.NavParameter.SpeedLi"
-    "mit\0222\n\020NodeAngularLimit\030\t \001(\0132\030.NavParam"
-    "eter.SpeedLimit\022\022\n\nrpc_ipport\030\n \001(\tb\006pro"
-    "to3"
+    "L\n\017R_mpc_parameter\022\n\n\002dt\030\001 \001(\002\022\023\n\013acc_an"
+    "gular\030\002 \001(\002\022\013\n\003min\030\003 \001(\002\022\013\n\003max\030\004 \001(\002\"\221\004"
+    "\n\024Navigation_parameter\022\020\n\010main_agv\030\001 \001(\010"
+    "\0221\n\010Agv_emqx\030\002 \001(\0132\037.NavParameter.AgvEmq"
+    "x_parameter\0223\n\rTerminal_emqx\030\003 \001(\0132\034.Nav"
+    "Parameter.Emqx_parameter\022/\n\014brother_emqx"
+    "\030\004 \001(\0132\031.NavParameter.BrotherEmqx\0224\n\017x_m"
+    "pc_parameter\030\005 \001(\0132\033.NavParameter.MPC_pa"
+    "rameter\0224\n\017y_mpc_parameter\030\006 \001(\0132\033.NavPa"
+    "rameter.MPC_parameter\022-\n\013InOutVLimit\030\007 \001"
+    "(\0132\030.NavParameter.SpeedLimit\0223\n\021NodeVelo"
+    "cityLimit\030\010 \001(\0132\030.NavParameter.SpeedLimi"
+    "t\0222\n\020NodeAngularLimit\030\t \001(\0132\030.NavParamet"
+    "er.SpeedLimit\022\022\n\nrpc_ipport\030\n \001(\t\0226\n\017r_m"
+    "pc_parameter\030\013 \001(\0132\035.NavParameter.R_mpc_"
+    "parameterb\006proto3"
 };
 static ::absl::once_flag descriptor_table_parameter_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_parameter_2eproto = {
     false,
     false,
-    1043,
+    1177,
     descriptor_table_protodef_parameter_2eproto,
     "parameter.proto",
     &descriptor_table_parameter_2eproto_once,
     nullptr,
     0,
-    7,
+    8,
     schemas,
     file_default_instances,
     TableStruct_parameter_2eproto::offsets,
@@ -2324,6 +2366,311 @@ void Accuracy::InternalSwap(Accuracy* other) {
 }
 // ===================================================================
 
+class R_mpc_parameter::_Internal {
+ public:
+};
+
+R_mpc_parameter::R_mpc_parameter(::PROTOBUF_NAMESPACE_ID::Arena* arena)
+  : ::PROTOBUF_NAMESPACE_ID::Message(arena) {
+  SharedCtor(arena);
+  // @@protoc_insertion_point(arena_constructor:NavParameter.R_mpc_parameter)
+}
+R_mpc_parameter::R_mpc_parameter(const R_mpc_parameter& from)
+  : ::PROTOBUF_NAMESPACE_ID::Message(), _impl_(from._impl_) {
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(
+      from._internal_metadata_);
+  // @@protoc_insertion_point(copy_constructor:NavParameter.R_mpc_parameter)
+}
+
+inline void R_mpc_parameter::SharedCtor(::_pb::Arena* arena) {
+  (void)arena;
+  new (&_impl_) Impl_{
+      decltype(_impl_.dt_) { 0 }
+
+    , decltype(_impl_.acc_angular_) { 0 }
+
+    , decltype(_impl_.min_) { 0 }
+
+    , decltype(_impl_.max_) { 0 }
+
+    , /*decltype(_impl_._cached_size_)*/{}
+  };
+}
+
+R_mpc_parameter::~R_mpc_parameter() {
+  // @@protoc_insertion_point(destructor:NavParameter.R_mpc_parameter)
+  if (auto *arena = _internal_metadata_.DeleteReturnArena<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>()) {
+  (void)arena;
+    return;
+  }
+  SharedDtor();
+}
+
+inline void R_mpc_parameter::SharedDtor() {
+  ABSL_DCHECK(GetArenaForAllocation() == nullptr);
+}
+
+void R_mpc_parameter::SetCachedSize(int size) const {
+  _impl_._cached_size_.Set(size);
+}
+
+void R_mpc_parameter::Clear() {
+// @@protoc_insertion_point(message_clear_start:NavParameter.R_mpc_parameter)
+  ::uint32_t cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  ::memset(&_impl_.dt_, 0, static_cast<::size_t>(
+      reinterpret_cast<char*>(&_impl_.max_) -
+      reinterpret_cast<char*>(&_impl_.dt_)) + sizeof(_impl_.max_));
+  _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+const char* R_mpc_parameter::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) {
+#define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  while (!ctx->Done(&ptr)) {
+    ::uint32_t tag;
+    ptr = ::_pbi::ReadTag(ptr, &tag);
+    switch (tag >> 3) {
+      // float dt = 1;
+      case 1:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 13)) {
+          _impl_.dt_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float acc_angular = 2;
+      case 2:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 21)) {
+          _impl_.acc_angular_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float min = 3;
+      case 3:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 29)) {
+          _impl_.min_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float max = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 37)) {
+          _impl_.max_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      default:
+        goto handle_unusual;
+    }  // switch
+  handle_unusual:
+    if ((tag == 0) || ((tag & 7) == 4)) {
+      CHK_(ptr);
+      ctx->SetLastTag(tag);
+      goto message_done;
+    }
+    ptr = UnknownFieldParse(
+        tag,
+        _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(),
+        ptr, ctx);
+    CHK_(ptr != nullptr);
+  }  // while
+message_done:
+  return ptr;
+failure:
+  ptr = nullptr;
+  goto message_done;
+#undef CHK_
+}
+
+::uint8_t* R_mpc_parameter::_InternalSerialize(
+    ::uint8_t* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const {
+  // @@protoc_insertion_point(serialize_to_array_start:NavParameter.R_mpc_parameter)
+  ::uint32_t cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  // float dt = 1;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_dt = this->_internal_dt();
+  ::uint32_t raw_dt;
+  memcpy(&raw_dt, &tmp_dt, sizeof(tmp_dt));
+  if (raw_dt != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        1, this->_internal_dt(), target);
+  }
+
+  // float acc_angular = 2;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_acc_angular = this->_internal_acc_angular();
+  ::uint32_t raw_acc_angular;
+  memcpy(&raw_acc_angular, &tmp_acc_angular, sizeof(tmp_acc_angular));
+  if (raw_acc_angular != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        2, this->_internal_acc_angular(), target);
+  }
+
+  // float min = 3;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_min = this->_internal_min();
+  ::uint32_t raw_min;
+  memcpy(&raw_min, &tmp_min, sizeof(tmp_min));
+  if (raw_min != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        3, this->_internal_min(), target);
+  }
+
+  // float max = 4;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_max = this->_internal_max();
+  ::uint32_t raw_max;
+  memcpy(&raw_max, &tmp_max, sizeof(tmp_max));
+  if (raw_max != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        4, this->_internal_max(), target);
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    target = ::_pbi::WireFormat::InternalSerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:NavParameter.R_mpc_parameter)
+  return target;
+}
+
+::size_t R_mpc_parameter::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:NavParameter.R_mpc_parameter)
+  ::size_t total_size = 0;
+
+  ::uint32_t cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  // float dt = 1;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_dt = this->_internal_dt();
+  ::uint32_t raw_dt;
+  memcpy(&raw_dt, &tmp_dt, sizeof(tmp_dt));
+  if (raw_dt != 0) {
+    total_size += 5;
+  }
+
+  // float acc_angular = 2;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_acc_angular = this->_internal_acc_angular();
+  ::uint32_t raw_acc_angular;
+  memcpy(&raw_acc_angular, &tmp_acc_angular, sizeof(tmp_acc_angular));
+  if (raw_acc_angular != 0) {
+    total_size += 5;
+  }
+
+  // float min = 3;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_min = this->_internal_min();
+  ::uint32_t raw_min;
+  memcpy(&raw_min, &tmp_min, sizeof(tmp_min));
+  if (raw_min != 0) {
+    total_size += 5;
+  }
+
+  // float max = 4;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_max = this->_internal_max();
+  ::uint32_t raw_max;
+  memcpy(&raw_max, &tmp_max, sizeof(tmp_max));
+  if (raw_max != 0) {
+    total_size += 5;
+  }
+
+  return MaybeComputeUnknownFieldsSize(total_size, &_impl_._cached_size_);
+}
+
+const ::PROTOBUF_NAMESPACE_ID::Message::ClassData R_mpc_parameter::_class_data_ = {
+    ::PROTOBUF_NAMESPACE_ID::Message::CopyWithSourceCheck,
+    R_mpc_parameter::MergeImpl
+};
+const ::PROTOBUF_NAMESPACE_ID::Message::ClassData*R_mpc_parameter::GetClassData() const { return &_class_data_; }
+
+
+void R_mpc_parameter::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBUF_NAMESPACE_ID::Message& from_msg) {
+  auto* const _this = static_cast<R_mpc_parameter*>(&to_msg);
+  auto& from = static_cast<const R_mpc_parameter&>(from_msg);
+  // @@protoc_insertion_point(class_specific_merge_from_start:NavParameter.R_mpc_parameter)
+  ABSL_DCHECK_NE(&from, _this);
+  ::uint32_t cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_dt = from._internal_dt();
+  ::uint32_t raw_dt;
+  memcpy(&raw_dt, &tmp_dt, sizeof(tmp_dt));
+  if (raw_dt != 0) {
+    _this->_internal_set_dt(from._internal_dt());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_acc_angular = from._internal_acc_angular();
+  ::uint32_t raw_acc_angular;
+  memcpy(&raw_acc_angular, &tmp_acc_angular, sizeof(tmp_acc_angular));
+  if (raw_acc_angular != 0) {
+    _this->_internal_set_acc_angular(from._internal_acc_angular());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_min = from._internal_min();
+  ::uint32_t raw_min;
+  memcpy(&raw_min, &tmp_min, sizeof(tmp_min));
+  if (raw_min != 0) {
+    _this->_internal_set_min(from._internal_min());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_max = from._internal_max();
+  ::uint32_t raw_max;
+  memcpy(&raw_max, &tmp_max, sizeof(tmp_max));
+  if (raw_max != 0) {
+    _this->_internal_set_max(from._internal_max());
+  }
+  _this->_internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+}
+
+void R_mpc_parameter::CopyFrom(const R_mpc_parameter& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:NavParameter.R_mpc_parameter)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool R_mpc_parameter::IsInitialized() const {
+  return true;
+}
+
+void R_mpc_parameter::InternalSwap(R_mpc_parameter* other) {
+  using std::swap;
+  _internal_metadata_.InternalSwap(&other->_internal_metadata_);
+  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
+      PROTOBUF_FIELD_OFFSET(R_mpc_parameter, _impl_.max_)
+      + sizeof(R_mpc_parameter::_impl_.max_)
+      - PROTOBUF_FIELD_OFFSET(R_mpc_parameter, _impl_.dt_)>(
+          reinterpret_cast<char*>(&_impl_.dt_),
+          reinterpret_cast<char*>(&other->_impl_.dt_));
+}
+
+::PROTOBUF_NAMESPACE_ID::Metadata R_mpc_parameter::GetMetadata() const {
+  return ::_pbi::AssignDescriptors(
+      &descriptor_table_parameter_2eproto_getter, &descriptor_table_parameter_2eproto_once,
+      file_level_metadata_parameter_2eproto[6]);
+}
+// ===================================================================
+
 class Navigation_parameter::_Internal {
  public:
   using HasBits = decltype(std::declval<Navigation_parameter>()._impl_._has_bits_);
@@ -2361,6 +2708,10 @@ class Navigation_parameter::_Internal {
   static void set_has_nodeangularlimit(HasBits* has_bits) {
     (*has_bits)[0] |= 128u;
   }
+  static const ::NavParameter::R_mpc_parameter& r_mpc_parameter(const Navigation_parameter* msg);
+  static void set_has_r_mpc_parameter(HasBits* has_bits) {
+    (*has_bits)[0] |= 256u;
+  }
 };
 
 const ::NavParameter::AgvEmqx_parameter&
@@ -2395,6 +2746,10 @@ const ::NavParameter::SpeedLimit&
 Navigation_parameter::_Internal::nodeangularlimit(const Navigation_parameter* msg) {
   return *msg->_impl_.nodeangularlimit_;
 }
+const ::NavParameter::R_mpc_parameter&
+Navigation_parameter::_Internal::r_mpc_parameter(const Navigation_parameter* msg) {
+  return *msg->_impl_.r_mpc_parameter_;
+}
 Navigation_parameter::Navigation_parameter(::PROTOBUF_NAMESPACE_ID::Arena* arena)
   : ::PROTOBUF_NAMESPACE_ID::Message(arena) {
   SharedCtor(arena);
@@ -2416,6 +2771,7 @@ Navigation_parameter::Navigation_parameter(const Navigation_parameter& from)
     , decltype(_impl_.inoutvlimit_){nullptr}
     , decltype(_impl_.nodevelocitylimit_){nullptr}
     , decltype(_impl_.nodeangularlimit_){nullptr}
+    , decltype(_impl_.r_mpc_parameter_){nullptr}
     , decltype(_impl_.main_agv_) {}
   };
 
@@ -2451,6 +2807,9 @@ Navigation_parameter::Navigation_parameter(const Navigation_parameter& from)
   if ((from._impl_._has_bits_[0] & 0x00000080u) != 0) {
     _this->_impl_.nodeangularlimit_ = new ::NavParameter::SpeedLimit(*from._impl_.nodeangularlimit_);
   }
+  if ((from._impl_._has_bits_[0] & 0x00000100u) != 0) {
+    _this->_impl_.r_mpc_parameter_ = new ::NavParameter::R_mpc_parameter(*from._impl_.r_mpc_parameter_);
+  }
   _this->_impl_.main_agv_ = from._impl_.main_agv_;
   // @@protoc_insertion_point(copy_constructor:NavParameter.Navigation_parameter)
 }
@@ -2470,6 +2829,7 @@ inline void Navigation_parameter::SharedCtor(::_pb::Arena* arena) {
     , decltype(_impl_.inoutvlimit_){nullptr}
     , decltype(_impl_.nodevelocitylimit_){nullptr}
     , decltype(_impl_.nodeangularlimit_){nullptr}
+    , decltype(_impl_.r_mpc_parameter_){nullptr}
     , decltype(_impl_.main_agv_) { false }
 
   };
@@ -2499,6 +2859,7 @@ inline void Navigation_parameter::SharedDtor() {
   if (this != internal_default_instance()) delete _impl_.inoutvlimit_;
   if (this != internal_default_instance()) delete _impl_.nodevelocitylimit_;
   if (this != internal_default_instance()) delete _impl_.nodeangularlimit_;
+  if (this != internal_default_instance()) delete _impl_.r_mpc_parameter_;
 }
 
 void Navigation_parameter::SetCachedSize(int size) const {
@@ -2547,6 +2908,10 @@ void Navigation_parameter::Clear() {
       _impl_.nodeangularlimit_->Clear();
     }
   }
+  if (cached_has_bits & 0x00000100u) {
+    ABSL_DCHECK(_impl_.r_mpc_parameter_ != nullptr);
+    _impl_.r_mpc_parameter_->Clear();
+  }
   _impl_.main_agv_ = false;
   _impl_._has_bits_.Clear();
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
@@ -2651,6 +3016,15 @@ const char* Navigation_parameter::_InternalParse(const char* ptr, ::_pbi::ParseC
           goto handle_unusual;
         }
         continue;
+      // .NavParameter.R_mpc_parameter r_mpc_parameter = 11;
+      case 11:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 90)) {
+          ptr = ctx->ParseMessage(_internal_mutable_r_mpc_parameter(), ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
       default:
         goto handle_unusual;
     }  // switch
@@ -2753,6 +3127,13 @@ failure:
     target = stream->WriteStringMaybeAliased(10, _s, target);
   }
 
+  // .NavParameter.R_mpc_parameter r_mpc_parameter = 11;
+  if (cached_has_bits & 0x00000100u) {
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(11, _Internal::r_mpc_parameter(this),
+        _Internal::r_mpc_parameter(this).GetCachedSize(), target, stream);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::_pbi::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -2834,6 +3215,13 @@ failure:
     }
 
   }
+  // .NavParameter.R_mpc_parameter r_mpc_parameter = 11;
+  if (cached_has_bits & 0x00000100u) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *_impl_.r_mpc_parameter_);
+  }
+
   // bool main_agv = 1;
   if (this->_internal_main_agv() != 0) {
     total_size += 2;
@@ -2895,6 +3283,10 @@ void Navigation_parameter::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, c
           from._internal_nodeangularlimit());
     }
   }
+  if (cached_has_bits & 0x00000100u) {
+    _this->_internal_mutable_r_mpc_parameter()->::NavParameter::R_mpc_parameter::MergeFrom(
+        from._internal_r_mpc_parameter());
+  }
   if (from._internal_main_agv() != 0) {
     _this->_internal_set_main_agv(from._internal_main_agv());
   }
@@ -2931,7 +3323,7 @@ void Navigation_parameter::InternalSwap(Navigation_parameter* other) {
 ::PROTOBUF_NAMESPACE_ID::Metadata Navigation_parameter::GetMetadata() const {
   return ::_pbi::AssignDescriptors(
       &descriptor_table_parameter_2eproto_getter, &descriptor_table_parameter_2eproto_once,
-      file_level_metadata_parameter_2eproto[6]);
+      file_level_metadata_parameter_2eproto[7]);
 }
 // @@protoc_insertion_point(namespace_scope)
 }  // namespace NavParameter
@@ -2960,6 +3352,10 @@ template<> PROTOBUF_NOINLINE ::NavParameter::Accuracy*
 Arena::CreateMaybeMessage< ::NavParameter::Accuracy >(Arena* arena) {
   return Arena::CreateMessageInternal< ::NavParameter::Accuracy >(arena);
 }
+template<> PROTOBUF_NOINLINE ::NavParameter::R_mpc_parameter*
+Arena::CreateMaybeMessage< ::NavParameter::R_mpc_parameter >(Arena* arena) {
+  return Arena::CreateMessageInternal< ::NavParameter::R_mpc_parameter >(arena);
+}
 template<> PROTOBUF_NOINLINE ::NavParameter::Navigation_parameter*
 Arena::CreateMaybeMessage< ::NavParameter::Navigation_parameter >(Arena* arena) {
   return Arena::CreateMessageInternal< ::NavParameter::Navigation_parameter >(arena);

MPC/parameter.pb.h

@@ -69,6 +69,9 @@ extern MPC_parameterDefaultTypeInternal _MPC_parameter_default_instance_;
 class Navigation_parameter;
 struct Navigation_parameterDefaultTypeInternal;
 extern Navigation_parameterDefaultTypeInternal _Navigation_parameter_default_instance_;
+class R_mpc_parameter;
+struct R_mpc_parameterDefaultTypeInternal;
+extern R_mpc_parameterDefaultTypeInternal _R_mpc_parameter_default_instance_;
 class SpeedLimit;
 struct SpeedLimitDefaultTypeInternal;
 extern SpeedLimitDefaultTypeInternal _SpeedLimit_default_instance_;
@@ -87,6 +90,8 @@ template <>
 template <>
 ::NavParameter::Navigation_parameter* Arena::CreateMaybeMessage<::NavParameter::Navigation_parameter>(Arena*);
 template <>
+::NavParameter::R_mpc_parameter* Arena::CreateMaybeMessage<::NavParameter::R_mpc_parameter>(Arena*);
+template <>
 ::NavParameter::SpeedLimit* Arena::CreateMaybeMessage<::NavParameter::SpeedLimit>(Arena*);
 PROTOBUF_NAMESPACE_CLOSE
 
@@ -1373,6 +1378,197 @@ class Accuracy final :
   friend struct ::TableStruct_parameter_2eproto;
 };// -------------------------------------------------------------------
 
+class R_mpc_parameter final :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavParameter.R_mpc_parameter) */ {
+ public:
+  inline R_mpc_parameter() : R_mpc_parameter(nullptr) {}
+  ~R_mpc_parameter() override;
+  template<typename = void>
+  explicit PROTOBUF_CONSTEXPR R_mpc_parameter(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized);
+
+  R_mpc_parameter(const R_mpc_parameter& from);
+  R_mpc_parameter(R_mpc_parameter&& from) noexcept
+    : R_mpc_parameter() {
+    *this = ::std::move(from);
+  }
+
+  inline R_mpc_parameter& operator=(const R_mpc_parameter& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline R_mpc_parameter& operator=(R_mpc_parameter&& from) noexcept {
+    if (this == &from) return *this;
+    if (GetOwningArena() == from.GetOwningArena()
+  #ifdef PROTOBUF_FORCE_COPY_IN_MOVE
+        && GetOwningArena() != nullptr
+  #endif  // !PROTOBUF_FORCE_COPY_IN_MOVE
+    ) {
+      InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  inline const ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance);
+  }
+  inline ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return default_instance().GetMetadata().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return default_instance().GetMetadata().reflection;
+  }
+  static const R_mpc_parameter& default_instance() {
+    return *internal_default_instance();
+  }
+  static inline const R_mpc_parameter* internal_default_instance() {
+    return reinterpret_cast<const R_mpc_parameter*>(
+               &_R_mpc_parameter_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    6;
+
+  friend void swap(R_mpc_parameter& a, R_mpc_parameter& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(R_mpc_parameter* other) {
+    if (other == this) return;
+  #ifdef PROTOBUF_FORCE_COPY_IN_SWAP
+    if (GetOwningArena() != nullptr &&
+        GetOwningArena() == other->GetOwningArena()) {
+   #else  // PROTOBUF_FORCE_COPY_IN_SWAP
+    if (GetOwningArena() == other->GetOwningArena()) {
+  #endif  // !PROTOBUF_FORCE_COPY_IN_SWAP
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(R_mpc_parameter* other) {
+    if (other == this) return;
+    ABSL_DCHECK(GetOwningArena() == other->GetOwningArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  R_mpc_parameter* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final {
+    return CreateMaybeMessage<R_mpc_parameter>(arena);
+  }
+  using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom;
+  void CopyFrom(const R_mpc_parameter& from);
+  using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom;
+  void MergeFrom( const R_mpc_parameter& from) {
+    R_mpc_parameter::MergeImpl(*this, from);
+  }
+  private:
+  static void MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBUF_NAMESPACE_ID::Message& from_msg);
+  public:
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  ::size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::uint8_t* _InternalSerialize(
+      ::uint8_t* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _impl_._cached_size_.Get(); }
+
+  private:
+  void SharedCtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(R_mpc_parameter* other);
+
+  private:
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::absl::string_view FullMessageName() {
+    return "NavParameter.R_mpc_parameter";
+  }
+  protected:
+  explicit R_mpc_parameter(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  static const ClassData _class_data_;
+  const ::PROTOBUF_NAMESPACE_ID::Message::ClassData*GetClassData() const final;
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kDtFieldNumber = 1,
+    kAccAngularFieldNumber = 2,
+    kMinFieldNumber = 3,
+    kMaxFieldNumber = 4,
+  };
+  // float dt = 1;
+  void clear_dt() ;
+  float dt() const;
+  void set_dt(float value);
+
+  private:
+  float _internal_dt() const;
+  void _internal_set_dt(float value);
+
+  public:
+  // float acc_angular = 2;
+  void clear_acc_angular() ;
+  float acc_angular() const;
+  void set_acc_angular(float value);
+
+  private:
+  float _internal_acc_angular() const;
+  void _internal_set_acc_angular(float value);
+
+  public:
+  // float min = 3;
+  void clear_min() ;
+  float min() const;
+  void set_min(float value);
+
+  private:
+  float _internal_min() const;
+  void _internal_set_min(float value);
+
+  public:
+  // float max = 4;
+  void clear_max() ;
+  float max() const;
+  void set_max(float value);
+
+  private:
+  float _internal_max() const;
+  void _internal_set_max(float value);
+
+  public:
+  // @@protoc_insertion_point(class_scope:NavParameter.R_mpc_parameter)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  struct Impl_ {
+    float dt_;
+    float acc_angular_;
+    float min_;
+    float max_;
+    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  };
+  union { Impl_ _impl_; };
+  friend struct ::TableStruct_parameter_2eproto;
+};// -------------------------------------------------------------------
+
 class Navigation_parameter final :
     public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavParameter.Navigation_parameter) */ {
  public:
@@ -1429,7 +1625,7 @@ class Navigation_parameter final :
                &_Navigation_parameter_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    6;
+    7;
 
   friend void swap(Navigation_parameter& a, Navigation_parameter& b) {
     a.Swap(&b);
@@ -1510,6 +1706,7 @@ class Navigation_parameter final :
     kInOutVLimitFieldNumber = 7,
     kNodeVelocityLimitFieldNumber = 8,
     kNodeAngularLimitFieldNumber = 9,
+    kRMpcParameterFieldNumber = 11,
     kMainAgvFieldNumber = 1,
   };
   // string rpc_ipport = 10;
@@ -1644,6 +1841,20 @@ class Navigation_parameter final :
   void unsafe_arena_set_allocated_nodeangularlimit(
       ::NavParameter::SpeedLimit* nodeangularlimit);
   ::NavParameter::SpeedLimit* unsafe_arena_release_nodeangularlimit();
+  // .NavParameter.R_mpc_parameter r_mpc_parameter = 11;
+  bool has_r_mpc_parameter() const;
+  void clear_r_mpc_parameter() ;
+  const ::NavParameter::R_mpc_parameter& r_mpc_parameter() const;
+  PROTOBUF_NODISCARD ::NavParameter::R_mpc_parameter* release_r_mpc_parameter();
+  ::NavParameter::R_mpc_parameter* mutable_r_mpc_parameter();
+  void set_allocated_r_mpc_parameter(::NavParameter::R_mpc_parameter* r_mpc_parameter);
+  private:
+  const ::NavParameter::R_mpc_parameter& _internal_r_mpc_parameter() const;
+  ::NavParameter::R_mpc_parameter* _internal_mutable_r_mpc_parameter();
+  public:
+  void unsafe_arena_set_allocated_r_mpc_parameter(
+      ::NavParameter::R_mpc_parameter* r_mpc_parameter);
+  ::NavParameter::R_mpc_parameter* unsafe_arena_release_r_mpc_parameter();
   // bool main_agv = 1;
   void clear_main_agv() ;
   bool main_agv() const;
@@ -1673,6 +1884,7 @@ class Navigation_parameter final :
     ::NavParameter::SpeedLimit* inoutvlimit_;
     ::NavParameter::SpeedLimit* nodevelocitylimit_;
     ::NavParameter::SpeedLimit* nodeangularlimit_;
+    ::NavParameter::R_mpc_parameter* r_mpc_parameter_;
     bool main_agv_;
   };
   union { Impl_ _impl_; };
@@ -2548,6 +2760,90 @@ inline void Accuracy::_internal_set_w(float value) {
 
 // -------------------------------------------------------------------
 
+// R_mpc_parameter
+
+// float dt = 1;
+inline void R_mpc_parameter::clear_dt() {
+  _impl_.dt_ = 0;
+}
+inline float R_mpc_parameter::dt() const {
+  // @@protoc_insertion_point(field_get:NavParameter.R_mpc_parameter.dt)
+  return _internal_dt();
+}
+inline void R_mpc_parameter::set_dt(float value) {
+  _internal_set_dt(value);
+  // @@protoc_insertion_point(field_set:NavParameter.R_mpc_parameter.dt)
+}
+inline float R_mpc_parameter::_internal_dt() const {
+  return _impl_.dt_;
+}
+inline void R_mpc_parameter::_internal_set_dt(float value) {
+  ;
+  _impl_.dt_ = value;
+}
+
+// float acc_angular = 2;
+inline void R_mpc_parameter::clear_acc_angular() {
+  _impl_.acc_angular_ = 0;
+}
+inline float R_mpc_parameter::acc_angular() const {
+  // @@protoc_insertion_point(field_get:NavParameter.R_mpc_parameter.acc_angular)
+  return _internal_acc_angular();
+}
+inline void R_mpc_parameter::set_acc_angular(float value) {
+  _internal_set_acc_angular(value);
+  // @@protoc_insertion_point(field_set:NavParameter.R_mpc_parameter.acc_angular)
+}
+inline float R_mpc_parameter::_internal_acc_angular() const {
+  return _impl_.acc_angular_;
+}
+inline void R_mpc_parameter::_internal_set_acc_angular(float value) {
+  ;
+  _impl_.acc_angular_ = value;
+}
+
+// float min = 3;
+inline void R_mpc_parameter::clear_min() {
+  _impl_.min_ = 0;
+}
+inline float R_mpc_parameter::min() const {
+  // @@protoc_insertion_point(field_get:NavParameter.R_mpc_parameter.min)
+  return _internal_min();
+}
+inline void R_mpc_parameter::set_min(float value) {
+  _internal_set_min(value);
+  // @@protoc_insertion_point(field_set:NavParameter.R_mpc_parameter.min)
+}
+inline float R_mpc_parameter::_internal_min() const {
+  return _impl_.min_;
+}
+inline void R_mpc_parameter::_internal_set_min(float value) {
+  ;
+  _impl_.min_ = value;
+}
+
+// float max = 4;
+inline void R_mpc_parameter::clear_max() {
+  _impl_.max_ = 0;
+}
+inline float R_mpc_parameter::max() const {
+  // @@protoc_insertion_point(field_get:NavParameter.R_mpc_parameter.max)
+  return _internal_max();
+}
+inline void R_mpc_parameter::set_max(float value) {
+  _internal_set_max(value);
+  // @@protoc_insertion_point(field_set:NavParameter.R_mpc_parameter.max)
+}
+inline float R_mpc_parameter::_internal_max() const {
+  return _impl_.max_;
+}
+inline void R_mpc_parameter::_internal_set_max(float value) {
+  ;
+  _impl_.max_ = value;
+}
+
+// -------------------------------------------------------------------
+
 // Navigation_parameter
 
 // bool main_agv = 1;
@@ -3313,6 +3609,93 @@ inline void Navigation_parameter::set_allocated_rpc_ipport(std::string* value) {
   // @@protoc_insertion_point(field_set_allocated:NavParameter.Navigation_parameter.rpc_ipport)
 }
 
+// .NavParameter.R_mpc_parameter r_mpc_parameter = 11;
+inline bool Navigation_parameter::has_r_mpc_parameter() const {
+  bool value = (_impl_._has_bits_[0] & 0x00000100u) != 0;
+  PROTOBUF_ASSUME(!value || _impl_.r_mpc_parameter_ != nullptr);
+  return value;
+}
+inline void Navigation_parameter::clear_r_mpc_parameter() {
+  if (_impl_.r_mpc_parameter_ != nullptr) _impl_.r_mpc_parameter_->Clear();
+  _impl_._has_bits_[0] &= ~0x00000100u;
+}
+inline const ::NavParameter::R_mpc_parameter& Navigation_parameter::_internal_r_mpc_parameter() const {
+  const ::NavParameter::R_mpc_parameter* p = _impl_.r_mpc_parameter_;
+  return p != nullptr ? *p : reinterpret_cast<const ::NavParameter::R_mpc_parameter&>(
+      ::NavParameter::_R_mpc_parameter_default_instance_);
+}
+inline const ::NavParameter::R_mpc_parameter& Navigation_parameter::r_mpc_parameter() const {
+  // @@protoc_insertion_point(field_get:NavParameter.Navigation_parameter.r_mpc_parameter)
+  return _internal_r_mpc_parameter();
+}
+inline void Navigation_parameter::unsafe_arena_set_allocated_r_mpc_parameter(
+    ::NavParameter::R_mpc_parameter* r_mpc_parameter) {
+  if (GetArenaForAllocation() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(_impl_.r_mpc_parameter_);
+  }
+  _impl_.r_mpc_parameter_ = r_mpc_parameter;
+  if (r_mpc_parameter) {
+    _impl_._has_bits_[0] |= 0x00000100u;
+  } else {
+    _impl_._has_bits_[0] &= ~0x00000100u;
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavParameter.Navigation_parameter.r_mpc_parameter)
+}
+inline ::NavParameter::R_mpc_parameter* Navigation_parameter::release_r_mpc_parameter() {
+  _impl_._has_bits_[0] &= ~0x00000100u;
+  ::NavParameter::R_mpc_parameter* temp = _impl_.r_mpc_parameter_;
+  _impl_.r_mpc_parameter_ = nullptr;
+#ifdef PROTOBUF_FORCE_COPY_IN_RELEASE
+  auto* old =  reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(temp);
+  temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  if (GetArenaForAllocation() == nullptr) { delete old; }
+#else  // PROTOBUF_FORCE_COPY_IN_RELEASE
+  if (GetArenaForAllocation() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+#endif  // !PROTOBUF_FORCE_COPY_IN_RELEASE
+  return temp;
+}
+inline ::NavParameter::R_mpc_parameter* Navigation_parameter::unsafe_arena_release_r_mpc_parameter() {
+  // @@protoc_insertion_point(field_release:NavParameter.Navigation_parameter.r_mpc_parameter)
+  _impl_._has_bits_[0] &= ~0x00000100u;
+  ::NavParameter::R_mpc_parameter* temp = _impl_.r_mpc_parameter_;
+  _impl_.r_mpc_parameter_ = nullptr;
+  return temp;
+}
+inline ::NavParameter::R_mpc_parameter* Navigation_parameter::_internal_mutable_r_mpc_parameter() {
+  _impl_._has_bits_[0] |= 0x00000100u;
+  if (_impl_.r_mpc_parameter_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavParameter::R_mpc_parameter>(GetArenaForAllocation());
+    _impl_.r_mpc_parameter_ = p;
+  }
+  return _impl_.r_mpc_parameter_;
+}
+inline ::NavParameter::R_mpc_parameter* Navigation_parameter::mutable_r_mpc_parameter() {
+  ::NavParameter::R_mpc_parameter* _msg = _internal_mutable_r_mpc_parameter();
+  // @@protoc_insertion_point(field_mutable:NavParameter.Navigation_parameter.r_mpc_parameter)
+  return _msg;
+}
+inline void Navigation_parameter::set_allocated_r_mpc_parameter(::NavParameter::R_mpc_parameter* r_mpc_parameter) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArenaForAllocation();
+  if (message_arena == nullptr) {
+    delete _impl_.r_mpc_parameter_;
+  }
+  if (r_mpc_parameter) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+        ::PROTOBUF_NAMESPACE_ID::Arena::InternalGetOwningArena(r_mpc_parameter);
+    if (message_arena != submessage_arena) {
+      r_mpc_parameter = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, r_mpc_parameter, submessage_arena);
+    }
+    _impl_._has_bits_[0] |= 0x00000100u;
+  } else {
+    _impl_._has_bits_[0] &= ~0x00000100u;
+  }
+  _impl_.r_mpc_parameter_ = r_mpc_parameter;
+  // @@protoc_insertion_point(field_set_allocated:NavParameter.Navigation_parameter.r_mpc_parameter)
+}
+
 #ifdef __GNUC__
 #pragma GCC diagnostic pop
 #endif  // __GNUC__

MPC/rotate_mpc.cpp

@@ -124,9 +124,10 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     /// 定义最大最小角度,角速度,角加速度(不区分方向)
     double max_angular = M_PI;
-    double max_angular_velocity = max_velocity_ / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
+//    double max_angular_velocity = max_velocity_ / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
+    double max_angular_velocity = max_velocity_ / (1 + exp(-4 * (fabs(current_to_target.theta()) - 0.57*max_velocity_)));
     double min_angular_velocity = min_velocity_ ;
-    double max_angular_acceleration_velocity = mpc_param.acc_angular;
+    double max_angular_acceleration_velocity = mpc_param.acc_angular/ (1 + exp(-7 * (fabs(current_to_target.theta()) + 0.1*mpc_param.acc_angular)));
     //double max_angular_acceleration_velocity=0.44;///(1+exp(-36.*(angular_velocity-0.15)))+0.085;
     /// 调整输入
     if (angular_velocity > max_angular_velocity) angular_velocity = max_angular_velocity;

parameter.proto

@@ -45,6 +45,14 @@ message Accuracy{
   float w=2;
 }
 
+message R_mpc_parameter
+{
+  float dt = 1;
+  float acc_angular = 2;
+  float min = 3;
+  float max = 4;
+}
+
 message Navigation_parameter
 {
   bool main_agv=1;   //是否是两车整体
@@ -58,5 +66,8 @@ message Navigation_parameter
   SpeedLimit NodeVelocityLimit=8; //马路点MPC速度限制
   SpeedLimit NodeAngularLimit=9;  //马路点原地旋转速度限制
   string rpc_ipport=10;
+  R_mpc_parameter r_mpc_parameter = 11; //整车旋转参数
 }
 
+
+