1.下发plc指令协议修改，下发三组线角速度

2.MPC最大角速度改为，由线速度和最小转弯半径计算得出
3.MPC参考速度采用sigmoid计算
4.各流程开启掉线判断
5.提高各导航节点精度要求
6.马路点导航流程开始时，添加与下一马路点对正动作
7.取消预入库动作
8.出车位点流程修复误判不在车位点导致无法动作bug
9.MPC添加日志打印
10.取消入库不最后一米纠偏限制
11.修复整车提升和夹持时，取消指令不生效bug
12.优化整车入库，正反向均可入库
13.旋转MPC添加sigmoid动态角速度限制

MPC/loaded_mpc.cpp

@@ -8,7 +8,8 @@
 #include <cppad/ipopt/solve.hpp>
 
 size_t N = 15;                  //优化考虑后面多少步
-size_t delay = 2;  // 发送到执行的延迟，即几个周期的时间
+size_t delay = 2;  // 预判发送到执行的延迟，即几个周期的时间
+size_t down_count = 3;//下发前多少步
 
 size_t nx = 0;
 size_t ny = nx + N;
@@ -67,8 +68,14 @@ public:
         }
 
         for (int t = 0; t < N - 1; t++) {
-            //速度，加速度，前轮角 weight loss
+            //角速度，加速度，角加速度 weight loss
             fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
+            if(t > N / 3){//前中后三段角速度权重1：2：3
+                fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
+            }
+            if(t > N / 3 * 2){//前中后三段角速度权重1：2：3
+                fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
+            }
             fg[0] += a_cost_weight * CppAD::pow(vars[nv + t + 1] - vars[nv + t], 2);
             fg[0] += ath_cost_weight * CppAD::pow(vars[ndlt + t + 1] - vars[ndlt + t], 2);
         }
@@ -173,10 +180,10 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
         angular = fabs(line_velocity) / radius;
         if (wmg < 0) angular = -angular;
     }
-    double max_wmg = 0.5 / radius;//fabs(line_velocity) / radius;
+    double max_wmg = fabs(line_velocity) / radius;//0.5 / radius;
 
     std::vector<Pose2d> filte_poses;
-    if (filte_Path(pose_agv, target, trajectory, filte_poses, 10) == false) {
+    if (filte_Path(pose_agv, target, trajectory, filte_poses, 20) == false) {
         printf("filte path failed ...\n");
         return failed;
     }
@@ -196,11 +203,9 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     typedef CPPAD_TESTVECTOR(double) Dvector;
 
     //根据当前点和目标点距离,计算目标速度
+    double dis=Pose2d::distance(pose_agv, target);
+    double ref_velocity = 1.0/(1+exp(-4.*(dis-1)));
 
-    double ref_velocity = Pose2d::distance(pose_agv, target) / 2.5;
-//    std::cout<<"ref_velocity----"<<pose_agv<<" "<<target<<std::endl;
-    if (ref_velocity < 0.1)
-        ref_velocity = 0.1;
     //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0
 
     Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);
@@ -353,27 +358,33 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     out.clear();
 
-    double solve_velocity = solution.x[nv+delay];
-    double solve_angular = solution.x[ndlt+delay];
+    double solve_velocity[down_count];
+    double solve_angular[down_count];
 
-    //纠正角速度/线速度，使其满足最小转弯半径
-    double correct_angular = solve_angular;
-    if ((solve_velocity * solve_velocity) / (solve_angular * solve_angular + 1e-9) < (radius * radius)) {
-        correct_angular = fabs(line_velocity) / radius;
-        if (solve_angular < 0) correct_angular = -correct_angular;
+    for (int i = 0; i < down_count; ++i) {
+        solve_velocity[i] = solution.x[nv + delay + i];
+        solve_angular[i] = solution.x[ndlt + delay + i];
+
+
+        //纠正角速度/线速度，使其满足最小转弯半径
+        double correct_angular = solve_angular[i];
+        if ((solve_velocity[i] * solve_velocity[i]) / (solve_angular[i] * solve_angular[i] + 1e-9) < (radius * radius)) {
+            correct_angular = fabs(line_velocity) / radius;
+            if (solve_angular[i] < 0) correct_angular = -correct_angular;
+        }
+        ///-----
+        printf(" P[%d],input:%.4f %.5f(%.5f) output:%.4f %.5f(%.5f) ref:%.3f max_v:%f time:%.3f\n",
+               i, line_velocity, wmg, angular, solve_velocity[i], solve_angular[i], correct_angular,
+               ref_velocity, max_velocity_, time);
+
+        /*if(solve_velocity>=0 && solve_velocity<min_velocity_)
+          solve_velocity=min_velocity_;
+        if(solve_velocity<0 && solve_velocity>-min_velocity_)
+          solve_velocity=-min_velocity_;*/
+
+        out.push_back(solve_velocity[i]);
+        out.push_back(correct_angular);
     }
-    ///-----
-    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);
-
-    /*if(solve_velocity>=0 && solve_velocity<min_velocity_)
-      solve_velocity=min_velocity_;
-    if(solve_velocity<0 && solve_velocity>-min_velocity_)
-      solve_velocity=-min_velocity_;*/
-
-    out.push_back(solve_velocity);
-    out.push_back(correct_angular);
 
 
     //计算预测轨迹

MPC/monitor/monitor_emqx.cpp

@@ -29,10 +29,10 @@ void Monitor_emqx::clamp_close(int mode)
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
-  speed.set_h(heat_);
-  speed.set_t(eClampClose);
+  speed.set_h1(heat_);
+  speed.set_t1(eClampClose);
 
-  speed.set_m(mode);
+  speed.set_m1(mode);
   speed.set_end(1);
   msg.fromProtoMessage(speed);
   Publish(speedcmd_topic_,msg);
@@ -43,10 +43,10 @@ void Monitor_emqx::clamp_open(int mode)
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
-  speed.set_h(heat_);
-  speed.set_t(eClampOpen);
+  speed.set_h1(heat_);
+  speed.set_t1(eClampOpen);
 
-  speed.set_m(mode);
+  speed.set_m1(mode);
   speed.set_end(1);
   msg.fromProtoMessage(speed);
   Publish(speedcmd_topic_,msg);
@@ -57,10 +57,10 @@ void Monitor_emqx::lifter_rise(int mode)
     MqttMsg msg;
     NavMessage::ToAgvCmd speed;
     heat_=(heat_+1)%255;
-    speed.set_h(heat_);
-    speed.set_t(eLifterRise);
+    speed.set_h1(heat_);
+    speed.set_t1(eLifterRise);
 
-    speed.set_m(mode);
+    speed.set_m1(mode);
     speed.set_end(1);
     msg.fromProtoMessage(speed);
     Publish(speedcmd_topic_,msg);
@@ -71,10 +71,10 @@ void Monitor_emqx::lifter_down(int mode)
     MqttMsg msg;
     NavMessage::ToAgvCmd speed;
     heat_=(heat_+1)%255;
-    speed.set_h(heat_);
-    speed.set_t(eLifterDown);
+    speed.set_h1(heat_);
+    speed.set_t1(eLifterDown);
 
-    speed.set_m(mode);
+    speed.set_m1(mode);
     speed.set_end(1);
     msg.fromProtoMessage(speed);
     Publish(speedcmd_topic_,msg);
@@ -92,14 +92,15 @@ void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a,double L
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
-  speed.set_h(heat_);
-  speed.set_t(type);
-  speed.set_v(velocity);
-  speed.set_w(w);
-  speed.set_l(L);
+  speed.set_h1(heat_);
+  speed.set_t1(type);
+  speed.set_v1(velocity);
+  speed.set_w1(w);
+  speed.set_l1(L);
 
-  speed.set_m(mode);
+  speed.set_m1(mode);
   speed.set_end(1);
+  printf("real down(v:%f, w:%f)\n",velocity, w);
   msg.fromProtoMessage(speed);
   Publish(speedcmd_topic_,msg);
 
@@ -110,11 +111,15 @@ void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a)
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
-  speed.set_h(heat_);
-  speed.set_t(type);
-  speed.set_v(v);
-  speed.set_w(w);
-  speed.set_m(mode);
+  speed.set_h1(heat_);
+  speed.set_t1(type);
+  speed.set_v1(v);
+  speed.set_w1(w);
+  speed.set_v2(v);
+  speed.set_w2(w);
+  speed.set_v3(v);
+  speed.set_w3(w);
+  speed.set_m1(mode);
   speed.set_end(1);
   msg.fromProtoMessage(speed);
   Publish(speedcmd_topic_,msg);
@@ -125,35 +130,49 @@ void Monitor_emqx::stop()
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
-  speed.set_h(heat_);
-  speed.set_t(eStop);
-  speed.set_v(0);
-  speed.set_w(0);
-  speed.set_end(1);
+  speed.set_h1(heat_);
+  speed.set_t1(eStop);
+  speed.set_v1(0);
+  speed.set_w1(0);
+  speed.set_v2(0);
+  speed.set_w2(0);
+  speed.set_v3(0);
+  speed.set_w3(0);
+
+    speed.set_end(1);
   msg.fromProtoMessage(speed);
 
   Publish(speedcmd_topic_,msg);
 }
 
-void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v, double w, double L, int P, double D) {
+void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[], double L, int P, double D) {
 //    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
 //    {
 //        printf(" Main agv mpc must set wheelBase\n ");
 //        return;
 //    }
-    double w_correct=fabs(w)>0.001?w:0.0;
-    double velocity=fabs(v)>0.001?v:0;
+    int count = 3;
+    double w_correct[count];
+    double velocity[count];
+    for (int i = 0; i < count; ++i) {
+        w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
+        velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
+    }
     MqttMsg msg;
     NavMessage::ToAgvCmd agvCmd;
     heat_=(heat_+1)%255;
-    agvCmd.set_h(heat_);
-    agvCmd.set_m(mode);
-    agvCmd.set_t(type);
-    agvCmd.set_v(velocity);
-    agvCmd.set_w(w_correct);
-    agvCmd.set_l(L);
-    agvCmd.set_p(P);
-    agvCmd.set_d(D);
+    agvCmd.set_h1(heat_);
+    agvCmd.set_m1(mode);
+    agvCmd.set_t1(type);
+    agvCmd.set_v1(velocity[0]);
+    agvCmd.set_w1(w_correct[0]);
+    agvCmd.set_v2(velocity[1]);
+    agvCmd.set_w2(w_correct[1]);
+    agvCmd.set_v3(velocity[2]);
+    agvCmd.set_w3(w_correct[2]);
+    agvCmd.set_l1(L);
+    agvCmd.set_p1(P);
+    agvCmd.set_d1(D);
 
     agvCmd.set_end(1);
     msg.fromProtoMessage(agvCmd);

MPC/monitor/monitor_emqx.h

@@ -17,7 +17,7 @@ class Monitor_emqx : public Terminator_emqx
     void set_speedcmd_topic(std::string speedcmd);
     void set_speed(int mode,ActionType type,double v,double a);
     void set_speed(int mode,ActionType type,double v,double a,double L);
-    void set_ToAgvCmd(int mode,ActionType type,double v,double w,double L=0,int P=0,double D=0);
+    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double L=0,int P=0,double D=0);
     void clamp_close(int mode);
     void clamp_open(int mode);
     void lifter_rise(int mode);

MPC/navigation.cpp

@@ -428,14 +428,14 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
             //发送暂停消息
             continue;
         }
-//        if (PoseTimeout()) {
-//            printf(" MPC_rotate Error:Pose is timeout \n");
-//            return eMpcFailed;
-//        }
-//        if (timedA_.timeout()) {
-//            printf(" MPC_rotate Error:v/a is timeout \n");
-//            return eMpcFailed;
-//        }
+        if (PoseTimeout()) {
+            printf(" MPC_rotate Error:Pose is timeout \n");
+            return eMpcFailed;
+        }
+        if (timedA_.timeout()) {
+            printf(" MPC_rotate Error:v/a is timeout \n");
+            return eMpcFailed;
+        }
 
         Pose2d current = timedPose_.Get();
         Pose2d target(node.x(), node.y(), node.theta());
@@ -466,7 +466,7 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
                 if (fabs(yaw_diffs[i]) < fabs(target_yaw_diff))
                     target_yaw_diff = yaw_diffs[i];
             }
-            std::cout << std::endl;
+//            std::cout << std::endl;
 //            std::cout << std::endl << " min_diff: " << current_to_target.theta() << std::endl;
         }
 
@@ -482,15 +482,30 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
         }
 
         //下发速度
-        if (fabs(target_yaw_diff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(target_yaw_diff) < 0.2 * 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]);
+            const int down_count = 3;
+            double v[down_count] = {0,0,0};
+            double w[down_count] = {out[0], out[1], out[2]};
+//            if(out[0]>=0){
+//                w[0]=1.*M_PI/180.0;
+//                w[1]=1.*M_PI/180.0;
+//                w[2]=1.*M_PI/180.0;
+//            }else{
+//                w[0]=-1.*M_PI/180.0;
+//                w[1]=-1.*M_PI/180.0;
+//                w[2]=-1.*M_PI/180.0;
+//            }
+            SendMoveCmd(move_mode_, eRotation, v, w);
             actionType_ = eRotation;
-            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);
+            double input_w = timedA_.Get();
+            for (int i = 0; i < down_count; ++i) {
+                printf("  MPC_rotate |P[%d], input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n",
+                       i, input_w, w[i], w[i]/M_PI*180, target_yaw_diff, directions);
+            }
         }
         continue;
 
@@ -511,6 +526,7 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
 
 bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w,
                               bool anyDirect, bool enable_rotate) {
+//    LogWriter("MoveToTarget beg");
     if (IsArrived(node)) {
         return true;
     }
@@ -581,7 +597,7 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
         bool anyDirect = (action.type() == 3);
 
         //速度限制
-        stLimit adjust_angular = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+        stLimit wmg_limit = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
         stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
 
         for (int i = 0; i < action.pathnodes_size(); ++i) {
@@ -590,15 +606,24 @@ 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.10);
+                node.set_l(0.05);
                 node.set_w(0.10);
-
             } else {
                 node.set_theta(0);
+                node.set_l(0.03);
                 node.set_w(0.10);
-                node.set_l(0.10);
             }
-            if (MoveToTarget(node, mpc_velocity, adjust_angular, anyDirect, true) == false)
+            int directions = Direction::eForward;
+            if (anyDirect)
+                directions = Direction::eForward | Direction::eBackward |
+                             Direction::eLeft | Direction::eRight;
+            if (i > 0){
+                if (RotateReferToTarget(node, wmg_limit, directions) != eMpcSuccess){
+                    printf(" before move node  RotateReferToNone Failed!!\n");
+                    return false;
+                }
+            }
+            if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true) == false)
                 return false;
             else
                 isInSpace_ = false;
@@ -609,8 +634,55 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
     return false;
 }
 
+Navigation::MpcResult Navigation::RotateAfterOutSpace(){
+    if(move_mode_!=eDouble)
+        return eMpcSuccess;
+    Pose2d init = timedPose_.Get();
+    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};
+    while (cancel_ == false) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        Pose2d current = timedPose_.Get();
+        double yawDiff = (target - current.theta());
+
+
+
+        //一次变速
+        std::vector<double> out;
+        bool ret;
+        TimerRecord::Execute([&, this]() {
+            ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
+        }, "Rotation_mpc_once");
+        if (ret == false) {
+            Stop();
+            return eMpcFailed;
+        }
+
+        //下发速度
+        if (fabs(yawDiff) < 0.3 * 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;
+        } else{
+            const int down_count = 3;
+            double v[down_count] = {0,0,0};
+            double w[down_count] = {out[0], out[1], out[2]};
+            SendMoveCmd(move_mode_, eRotation, v, w);
+            actionType_ = eRotation;
+            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
+                   timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
+        }
+        continue;
+
+    }
+    return eMpcFailed;
+}
+
 Navigation::MpcResult
-Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
+Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
+                                   double wheelbase, NavMessage::PathNode &target) {
 //    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
     if (timedPose_.timeout()) {
         printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
@@ -642,7 +714,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
-    target.set_l(0.05);
+    target.set_l(0.02);
     target.set_w(0.05);
     target.set_id(space.id());
     printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
@@ -673,12 +745,15 @@ 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) < 0.2 * 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;
         } else{
-            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
+            const int down_count = 3;
+            double v[down_count] = {0,0,0};
+            double w[down_count] = {out[0], out[1], out[2]};
+            SendMoveCmd(move_mode_, eRotation, v, w);
             actionType_ = eRotation;
             printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
                    timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
@@ -691,6 +766,8 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 }
 
 bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
+//    LogWriter("execute_InOut_space beg");
+
     if (action.type() != 1 && action.type() != 2) {
         printf(" Inout_space failed : msg action type must ==1\n ");
         return false;
@@ -709,10 +786,12 @@ 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) {
+//        LogWriter("In space beg-");
+
         Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
                    0);
-        action.mutable_streetnode()->set_l(0.06);
-        action.mutable_streetnode()->set_w(0.06);
+        action.mutable_streetnode()->set_l(0.05);
+        action.mutable_streetnode()->set_w(0.05);
         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());
@@ -728,61 +807,65 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             return false;
         }
 
-        //进库前提升机构上升
-        if (lifter_rise() == false){
-            printf(" Enter space | lifter rise failed. line:%d\n",__LINE__);
-        }
-
-        //移动到预入库点位，较高精度
-        NavMessage::PathNode pre_target;
-        bool jump_preenter = false;
-        if (move_mode_ == eSingle){
-            double x = action.spacenode().x();
-            double y = action.spacenode().y();
-            double theta = action.spacenode().theta();
-
-            double back_distance = 3.3 + 0.2 + 1;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
-            Pose2d current = timedPose_.Get();
-            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
-            if (diff < back_distance) jump_preenter = true;
-            x -= back_distance* cos(theta);
-            y -= back_distance* sin(theta);
-
-            pre_target.set_x(x);
-            pre_target.set_y(y);
-            pre_target.set_theta(theta);
-            pre_target.set_l(0.05);
-            pre_target.set_w(0.05);
-            pre_target.set_id("R_0");
-        }
-        else if (move_mode_ == eDouble){
-            double x = action.spacenode().x();
-            double y = action.spacenode().y();
-            double theta = action.spacenode().theta();
-
-            double back_distance = 3.3 + 0.2 + 1 + action.wheelbase()/2;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
-            Pose2d current = timedPose_.Get();
-            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
-            if (diff < back_distance) jump_preenter = true;
-            x -= back_distance* cos(theta);
-            y -= back_distance* sin(theta);
-
-            pre_target.set_x(x);
-            pre_target.set_y(y);
-            pre_target.set_theta(theta);
-            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 pre_space:%s failed. type:%d. line:%d\n", pre_target.id().data(), action.type(),
-                       __LINE__);
-                return false;
+        std::string id = action.spacenode().id().substr(1,action.spacenode().id().length()-1);
+        int space_id = stoi(id);
+        if (space_id>99 && space_id< 1000){
+            //进载车板库前提升机构上升
+            if (lifter_rise() == false){
+                printf(" Enter space | lifter rise failed. line:%d\n",__LINE__);
             }
         }
+//
+//        //移动到预入库点位，较高精度
+//        NavMessage::PathNode pre_target;
+//        bool jump_preenter = false;
+//        if (move_mode_ == eSingle){
+//            double x = action.spacenode().x();
+//            double y = action.spacenode().y();
+//            double theta = action.spacenode().theta();
+//
+//            double back_distance = 3.3 + 0.2 + 1;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
+//            Pose2d current = timedPose_.Get();
+//            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
+//            if (diff < back_distance) jump_preenter = true;
+//            x -= back_distance* cos(theta);
+//            y -= back_distance* sin(theta);
+//
+//            pre_target.set_x(x);
+//            pre_target.set_y(y);
+//            pre_target.set_theta(theta);
+//            pre_target.set_l(0.10);
+//            pre_target.set_w(0.05);
+//            pre_target.set_id("R_0");
+//        }
+//        else if (move_mode_ == eDouble){
+//            double x = action.spacenode().x();
+//            double y = action.spacenode().y();
+//            double theta = action.spacenode().theta();
+//
+//            double back_distance = 3.3 + 0.2 + 1 + action.wheelbase()/2;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
+//            Pose2d current = timedPose_.Get();
+//            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
+//            if (diff < back_distance) jump_preenter = true;
+//            x -= back_distance* cos(theta);
+//            y -= back_distance* sin(theta);
+//
+//            pre_target.set_x(x);
+//            pre_target.set_y(y);
+//            pre_target.set_theta(theta);
+//            pre_target.set_l(0.05);
+//            pre_target.set_w(0.03);
+//            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, true) == false) {
+//                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");
@@ -791,15 +874,19 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             return false;
         }
     } 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.x() < 0.05 || diff.y() < 0.05) {
+//        LogWriter("Out space beg-");
+        // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿
+        Pose2d vec(action.streetnode().x() - action.spacenode().x(),
+                   action.streetnode().y() - action.spacenode().y(), 0);
+        float theta = Pose2d::vector2yaw(vec.x(), vec.y());
+        Pose2d space(action.spacenode().x(), action.spacenode().y(), theta);
+        Pose2d current = timedPose_.Get();
+        Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(),current.y(),0),space));
+        if (diff.x() < 0.05 || diff.y() < 0.10) {
             //出库，移动到马路点，允许自由方向，不允许中途旋转
-            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);
+            action.mutable_streetnode()->set_theta(theta);
+            action.mutable_streetnode()->set_l(0.04);
+            action.mutable_streetnode()->set_w(0.06);
             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());
@@ -810,7 +897,11 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             if (lifter_down() == false){
                 printf(" Out space | lifter down failed. line:%d\n",__LINE__);
             }
-
+//            if(RotateAfterOutSpace()!=eMpcSuccess) {
+//                printf(" after out space rotate Failed!!!\n");
+//                return false;
+//            }
+//            LogWriter("Out space end-");
         } else {
             std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff
                       << std::endl;
@@ -864,15 +955,20 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
         printf(" Rotation_mpc solve no solution set v/w = 0\n");
         out.clear();
         out.push_back(0);
+        out.push_back(0);
+        out.push_back(0);
     } else {
 //        double min_angular_velocity = 1.0 / 180 * M_PI;
         double min_angular_velocity = 0.00001;
-        if (fabs(out[0]) < min_angular_velocity) {
-            if (out[0] > 0)
-                out[0] = min_angular_velocity;
-            else if (out[0] < 0)
-                out[0] = -min_angular_velocity;
-            else {}
+        const int down_count = 3;
+        for (int i = 0; i < down_count; ++i) {
+            if (fabs(out[i]) < min_angular_velocity) {
+                if (out[i] > 0)
+                    out[i] = min_angular_velocity;
+                else if (out[i] < 0)
+                    out[i] = -min_angular_velocity;
+                else {}
+            }
         }
     }
     return ret == success || ret == no_solution;
@@ -944,6 +1040,10 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
         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);
     }
     predict_traj_.reset(optimize_trajectory, 1);
     selected_traj_.reset(selected_trajectory, 1);
@@ -998,7 +1098,20 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
     }
 }
 
-void Navigation::SendMoveCmd(int mode, ActionType type, double v, double angular, int space_id, double distance) {
+void Navigation::SendMoveCmd(int mode, ActionType type,
+                             double v[], double angular[]) {
+    if (monitor_) {
+        monitor_->set_ToAgvCmd(mode, type, v, angular);
+        if (type == eRotation)
+            RWheel_position_ = eR;
+        if (type == eVertical)
+            RWheel_position_ = eX;
+        if (type == eHorizontal)
+            RWheel_position_ = eY;
+    }
+}
+
+void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance) {
     if (monitor_) {
         monitor_->set_ToAgvCmd(mode, type, v, angular, 0,space_id, distance);
         if (type == eRotation)
@@ -1095,6 +1208,8 @@ bool Navigation::lifter_down() {
 }
 
 Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect) {
+//    LogWriter("MpcToTarget beg");
+
     if (IsArrived(node)) {
         printf(" current already in target completed !!!\n");
         return eMpcSuccess;
@@ -1110,7 +1225,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     Pose2d current = timedPose_.Get();
     Pose2d target(node.x(), node.y(), 0);
     //生成轨迹
-    Trajectory traj = Trajectory::create_line_trajectory(current, target);
+    Trajectory traj = Trajectory::create_line_trajectory(current, target, 0.1);
     if (traj.size() == 0)
         return eMpcSuccess;
     //判断 巡线方向
@@ -1125,24 +1240,38 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         }
     }
     printf(" exec along autoDirect:%d\n", autoDirect);
+
+    //文件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.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(30));
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
         if (pause_ == true) {
             //发送暂停消息
             continue;
         }
         if (PoseTimeout()) {
             printf(" navigation Error:Pose is timeout \n");
+            ofs.close();//关闭文件
             return eMpcFailed;
         }
         if (timedV_.timeout() || timedA_.timeout()) {
             printf(" navigation Error:v/a is timeout | __LINE__:%d \n", __LINE__);
+            ofs.close();//关闭文件
             return eMpcFailed;
         }
         //判断是否到达终点
         if (IsArrived(node)) {
             if (Stop()) {
                 printf(" exec along completed !!!\n");
+                ofs.close();//关闭文件
                 return eMpcSuccess;
             }
         }
@@ -1155,8 +1284,12 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         }, "mpc_once");
         if (ret == false) {
             Stop();
+            ofs.close();//关闭文件
             return eMpcFailed;
         }
+        const int down_count = 3;//下发前多少步
+        double down_v[down_count] ={out[0], out[2], out[4]};//下发线速度
+        double down_w[down_count] ={out[1], out[3], out[5]};//下发角速度
         /*
          * AGV 在终点附近低速巡航时，设置最小速度0.05
          */
@@ -1165,29 +1298,35 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             target_in_agv = target_in_agv.rotate(M_PI / 2.0);
         }
         if (Pose2d::abs(target_in_agv) < Pose2d(0.2, 2, M_PI * 2)) {
-            if (out[0] >= 0 && out[0] < limit_v.min)
-                out[0] = limit_v.min;
-            if (out[0] < 0 && out[0] > -limit_v.min)
-                out[0] = -limit_v.min;
+            for (int i = 0; i < down_count; ++i) {
+                if (down_v[i] >= 0 && down_v[i] < limit_v.min)
+                    down_v[i] = limit_v.min;
+                if (down_v[i] < 0 && down_v[i] > -limit_v.min)
+                    down_v[i] = -limit_v.min;
+            }
         }
 
         //放大角速度
-        out[1] *= 10;
+//        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;
-            }
+//            if(fabs(target_in_agv.x()) < 0.4){
+//                printf("target_in_agv: %f", target_in_agv.x());
+//                for (double & i : down_w) {
+//                    i = 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){
+            if(fabs(agv_in_space.x()) < 0.3){
                 printf("agv_in_space: %f", agv_in_space.x());
-                out[1] = 0.0;
+                for (double & i : down_w) {
+                    i = 0.0;
+                }
             }
         }
 
@@ -1201,10 +1340,40 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             space_id = stoi(id);
         }
         if (directY == false)
-            SendMoveCmd(move_mode_, eVertical, out[0], out[1], space_id, distance);
+            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance);
         else
-            SendMoveCmd(move_mode_, eHorizontal, out[0], out[1], space_id, distance);
+            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance);
         actionType_ = directY ? eHorizontal : eVertical;
+
+        //日志打印
+        std::string log_inf_line = std::to_string(move_mode_);
+        log_inf_line += " ";
+
+        log_inf_line += std::to_string(parameter_.x_mpc_parameter().shortest_radius())+" "+std::to_string(parameter_.x_mpc_parameter().acc_angular());
+        log_inf_line += " ";
+
+        Pose2d cur_pose = timedPose_.Get();//自身x,y,theta
+        log_inf_line += std::to_string(cur_pose.x()) + " " + std::to_string(cur_pose.y()) + " " + std::to_string(cur_pose.theta());
+        log_inf_line += " ";
+
+        Pose2d cur_Bro_pose = timedBrotherPose_.Get();//另一节x,y,theta
+        log_inf_line += std::to_string(cur_Bro_pose.x()) + " " + std::to_string(cur_Bro_pose.y()) + " " + std::to_string(cur_Bro_pose.theta());
+        log_inf_line += " ";
+
+        log_inf_line += std::to_string(out[0]) + " " + std::to_string(out[1]);//下发速度
+        log_inf_line += " ";
+        log_inf_line += std::to_string(out[2]) + " " + std::to_string(out[3]);//下发速度
+        log_inf_line += " ";
+        log_inf_line += std::to_string(out[4]) + " " + std::to_string(out[5]);//下发速度
+        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);
+
+        ofs << log_inf_line << std::endl;  //一行日志的内容
         /*
          * 判断车辆是否在终点附近徘徊，无法到达终点
          */
@@ -1214,10 +1383,12 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
                 SlowlyStop();
             else
                 Stop();
+            ofs.close();//关闭文件
             return eImpossibleToTarget;
         }
     }
 
+    ofs.close();//关闭文件
     return eMpcFailed;
 }
 
@@ -1284,8 +1455,11 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
     }
 
 
-    printf(" impossible to target:%f %f l:%f,w:%f theta:%f\n",
+    printf(" impossible to target:%f %f l:%f,w:%f theta:%f, ",
            targetNode.x(), targetNode.y(), targetNode.l(), targetNode.w(), targetNode.theta());
+    printf(" current:%f, %f, %f\n",
+           current.x(), current.y(),current.theta());
+
     return false;
 }
 
@@ -1333,8 +1507,10 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 isInSpace_ = false;
             }
         } else if (act.type() == 3 || act.type() == 4) { //马路导航
-            if (!execute_nodes(act))
+            if (!execute_nodes(act)){
+                printf(" street nav failed\n");
                 break;
+            }
         } else if (act.type() == 5) {
             printf(" 汽车模型导航....\n");
 
@@ -1402,45 +1578,46 @@ float Navigation::compute_cuv(const Pose2d &target) {
 }
 
 void Navigation::ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::NavResponse &response) {
-    pause_ = false;
-    cancel_ = false;
-    running_ = true;
-    actionType_ = eReady;
-    printf("ManualOperation: %d | start...\n", cmd.operation_type());
-
-    float symbol = cmd.velocity() < 0 ? -1 : 1; // 判断为正方向or负方向
-    double velocity;
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    switch (cmd.operation_type()) {
-        case 1: // 旋转
-            velocity = 2.0 * M_PI / 180.0 * symbol;
-            SendMoveCmd(move_mode_, eRotation, 0, velocity);
-            actionType_ = eRotation;
-            printf(" ManualOperation: %d | input angular_v: %f, down: %f\n",
-                   cmd.operation_type(), timedA_.Get(), velocity);
-            break;
-        case 2: // X平移
-            velocity = 1.0 * symbol;
-            SendMoveCmd(move_mode_, eVertical, velocity, 0);
-            actionType_ = eVertical;
-            printf(" ManualOperation: %d | input line_v   : %f, down: %f\n",
-                   cmd.operation_type(), timedV_.Get(), velocity);
-            break;
-        case 3: // Y平移
-            velocity = 1.0 * symbol;
-            SendMoveCmd(move_mode_, eHorizontal, velocity, 0);
-            actionType_ = eHorizontal;
-            printf(" ManualOperation: %d | input line_v: %f, down: %f\n",
-                   cmd.operation_type(), timedV_.Get(), velocity);
-            break;
-        default:
-            break;
-    }
-
-    actionType_ = eReady;
-    response.set_ret(-3);
-    response.set_info("ManualOperation: %d, canceled!!!", cmd.operation_type());
-    printf("ManualOperation: %d | canceled!!!\n", cmd.operation_type());
-    running_ = false;
+    return;
+//    pause_ = false;
+//    cancel_ = false;
+//    running_ = true;
+//    actionType_ = eReady;
+//    printf("ManualOperation: %d | start...\n", cmd.operation_type());
+//
+//    float symbol = cmd.velocity() < 0 ? -1 : 1; // 判断为正方向or负方向
+//    double velocity;
+//    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//    switch (cmd.operation_type()) {
+//        case 1: // 旋转
+//            velocity = 2.0 * M_PI / 180.0 * symbol;
+//            SendMoveCmd(move_mode_, eRotation, 0, velocity);
+//            actionType_ = eRotation;
+//            printf(" ManualOperation: %d | input angular_v: %f, down: %f\n",
+//                   cmd.operation_type(), timedA_.Get(), velocity);
+//            break;
+//        case 2: // X平移
+//            velocity = 1.0 * symbol;
+//            SendMoveCmd(move_mode_, eVertical, velocity, 0);
+//            actionType_ = eVertical;
+//            printf(" ManualOperation: %d | input line_v   : %f, down: %f\n",
+//                   cmd.operation_type(), timedV_.Get(), velocity);
+//            break;
+//        case 3: // Y平移
+//            velocity = 1.0 * symbol;
+//            SendMoveCmd(move_mode_, eHorizontal, velocity, 0);
+//            actionType_ = eHorizontal;
+//            printf(" ManualOperation: %d | input line_v: %f, down: %f\n",
+//                   cmd.operation_type(), timedV_.Get(), velocity);
+//            break;
+//        default:
+//            break;
+//    }
+//
+//    actionType_ = eReady;
+//    response.set_ret(-3);
+//    response.set_info("ManualOperation: %d, canceled!!!", cmd.operation_type());
+//    printf("ManualOperation: %d | canceled!!!\n", cmd.operation_type());
+//    running_ = false;
 }
 

MPC/navigation.h

@@ -14,6 +14,8 @@
 #include "parameter.pb.h"
 #include <queue>
 #include <thread>
+#include <fstream>
+#include <time.h>
 
 class Navigation {
 public:
@@ -84,12 +86,32 @@ public:
 
     bool PoseTimeout();
 
+    // 测试日志打印
+    bool LogWriter(std::string lines){
+        {
+            std::ofstream ofs;  //创建流对象
+            std::string log_file_dir = "../data/12_12/";
+            std::string log_file_name = "12_12.txt";
+            ofs.open(log_file_dir+log_file_name,std::ios::app);  //打开的地址和方式
+
+            time_t t = time(0);
+            char tmp[32]={NULL};
+            strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S",localtime(&t));
+            std::string time = tmp;
+            std::string log_inf_line = lines + ". " + time;
+            ofs << log_inf_line << std::endl;  //一行日志的内容
+            ofs.close();
+        }
+    }
+
 protected:
     virtual void HandleAgvStatu(const MqttMsg &msg);
 
     virtual void SendMoveCmd(int mode, ActionType type, double v, double angular);
 
-    virtual void SendMoveCmd(int mode, ActionType type, double v, double angular, int space_id, double distance);
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
+
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance);
 
     static void RobotPoseCallback(const MqttMsg &msg, void *context);
 
@@ -138,6 +160,9 @@ protected:
     virtual Navigation::MpcResult
     RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
 
+    virtual Navigation::MpcResult
+    RotateAfterOutSpace();
+
     virtual bool clamp_close();
 
     virtual bool clamp_open();

MPC/navigation_main.cpp

@@ -58,9 +58,9 @@ void NavigationMain::Start(const NavMessage::NavCmd &cmd, NavMessage::NavRespons
 }
 
 void NavigationMain::SendMoveCmd(int mode, ActionType type,
-                                 double v, double angular) {
+                                 double v[], double angular[]) {
     if (monitor_) {
-        monitor_->set_speed(mode, type, v, angular, wheelBase_);
+        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_);
     }
 }
 
@@ -100,7 +100,7 @@ bool NavigationMain::clamp_close() {
     printf("双车夹持\n");
     if (monitor_) {
         monitor_->clamp_close(move_mode_);
-        while (exit_ == false) {
+        while (cancel_ == false) {
             if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
                 printf("timed clamp is timeout\n");
                 return false;
@@ -124,7 +124,7 @@ bool NavigationMain::clamp_open() {
     if (monitor_) {
         printf("双车松夹持\n");
         monitor_->clamp_open(move_mode_);
-        while (exit_ == false) {
+        while (cancel_ == false) {
             if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
                 printf("timed clamp is timeout\n");
                 return false;
@@ -148,7 +148,7 @@ bool NavigationMain::lifter_rise() {
     if (monitor_) {
         printf("双车提升机构提升\n");
         monitor_->lifter_rise(move_mode_);
-        while (exit_ == false) {
+        while (cancel_ == false) {
             if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                 printf("timed lifter is timeout\n");
                 return false;
@@ -172,7 +172,7 @@ bool NavigationMain::lifter_down() {
     if (monitor_) {
         printf("双车提升机构下降\n");
         monitor_->lifter_down(move_mode_);
-        while (exit_ == false) {
+        while (cancel_ == false) {
             if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                 printf("timed lifter is timeout\n");
                 return false;
@@ -228,11 +228,19 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
-    target.set_l(0.05);
+    target.set_l(0.02);
     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) {
+        double yawDiff1 = (rotated - timedPose_.Get()).theta();
+        double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta();
+        if (fabs(yawDiff1) > fabs(yawDiff2)) {
+            rotated = rotated + Pose2d(0, 0, M_PI);
+        }
+    }
+
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         Pose2d current = timedPose_.Get();
@@ -250,12 +258,15 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.3 * 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;
         } else{
-            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
+            const int down_count = 3;
+            double v[down_count] = {0,0,0};
+            double w[down_count] = {out[0], out[1], out[2]};
+            SendMoveCmd(move_mode_, eRotation, v, w);
             actionType_ = eRotation;
             printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
                    timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);

MPC/navigation_main.h

@@ -70,8 +70,7 @@ protected:
     virtual Navigation::MpcResult
     RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
 
-    virtual void SendMoveCmd(int mode, ActionType type,
-                             double v, double angular);
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
 
     virtual bool clamp_close();
 

MPC/pose2d.h

@@ -94,7 +94,7 @@ class Pose2d
     }
 
     /*
-     * 将点顺时针旋转theta
+     * 将点逆时针旋转theta
      */
     Pose2d rotate(float theta)const;
     //绕xy旋转

MPC/rotate_mpc.cpp

@@ -8,9 +8,11 @@
 #include <cppad/ipopt/solve.hpp>
 
 size_t mpc_rotate_steps = 20;                  //优化考虑后面多少步
-size_t mpc_rotate_delay = 2;  // 发送到执行的延迟，即几个周期的时间
+size_t mpc_rotate_delay = 0;  // 发送到执行的延迟，即几个周期的时间
+size_t mpc_rotate_down_count = 3;//下发前多少步
 #define N mpc_rotate_steps
 #define delay mpc_rotate_delay
+#define down_count mpc_rotate_down_count
 
 class FG_eval_rotate {
 public:
@@ -122,9 +124,9 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     /// 定义最大最小角度,角速度,角加速度(不区分方向)
     double max_angular = M_PI;
-    double max_angular_velocity = 15.0 / 180 * M_PI;
-    double min_angular_velocity = 0.01 / 180 * M_PI;
-    double max_angular_acceleration_velocity = 30.0 / 180 * M_PI;//mpc_param.acc_angular; //
+    double max_angular_velocity = 5.0 / 180 * M_PI  / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
+    double min_angular_velocity = 1 / 180 * M_PI;
+    double max_angular_acceleration_velocity = mpc_param.acc_angular; //30.0 / 180 * M_PI;//
     /// 调整输入
     if (angular_velocity > max_angular_velocity) angular_velocity = max_angular_velocity;
     if (angular_velocity < -max_angular_velocity) angular_velocity = -max_angular_velocity;
@@ -241,9 +243,11 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     // 输出第一步的角速度
     out.clear();
-    double solve_angular_velocity = solution.x[N+delay];
-
-    out.push_back(solve_angular_velocity);
+    double solve_angular_velocity;
+    for (int i = 0; i < down_count; ++i) {
+        solve_angular_velocity = solution.x[i + delay];
+        out.push_back(solve_angular_velocity);
+    }
 
     return success;
 }