1.马路点巡航时，同时进行提升机构动作

2.优化巡航参数

MPC/loaded_mpc.cpp

@@ -225,7 +225,9 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     double max_acc_line_velocity = mpc_param.acc_velocity*acc_sigmoid;
     double max_acc_dlt = mpc_param.acc_angular;
 
-    double ref_velocity = 0.5*dis*dis+min_velocity_;//max_velocity_/(1+exp(-4.*(dis-0.8)))+min_velocity_;
+    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 ref_velocity = std::min(ref1,ref2);//max_velocity_/(1+exp(-4.*(dis-0.8)))+min_velocity_;
 
     Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);
     //std::cout<<"target:"<<target<<", agv:"<<pose_agv<<", relative:"<<targetPoseInAGV<<std::endl;

MPC/navigation.cpp

@@ -662,8 +662,6 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
         int clampLifterAction = 0;
         if (move_mode_ == eSingle) {
             clampLifterAction |= eByteClampHalfOpen;
-        } else {
-            clampLifterAction |= eByteLifterDown;
         }
         ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
@@ -712,6 +710,18 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
                 }
             }*/
 
+            //提升机构提前动作
+            if (GetRemainActionsCount() > 1){
+                NavMessage::NewAction last_action = GetLastAction();
+                if (IsUperSpace(last_action.spacenode())){
+                    if ((i+1) == action.pathnodes_size()){
+                        clampLifterAction |= eByteLifterUp;
+                    }
+                } else{
+                    clampLifterAction |= eByteLifterDown;
+                }
+            }
+
             if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true, clampLifterAction) == false)
                 return false;
             else
@@ -2449,6 +2459,7 @@ int Navigation::GetPortIDBySpace(NavMessage::PathNode node) {
     return -1;
 }
 
+
 int Navigation::GetRemainActionsCount() {
     return newUnfinished_cations_.size();
 }
@@ -2457,5 +2468,8 @@ NavMessage::NewAction Navigation::GetCurrentAction() {
     return newUnfinished_cations_.front();
 }
 
+NavMessage::NewAction Navigation::GetLastAction() {
+    return newUnfinished_cations_.back();
+}
 
 

MPC/navigation.h

@@ -129,6 +129,7 @@ public:
     // 获取当前未完成动作数量
     virtual int GetRemainActionsCount();
     virtual NavMessage::NewAction GetCurrentAction();
+    virtual NavMessage::NewAction GetLastAction();
 
 protected:
     static void anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes);