1.优化巡线MPC约束

2.优化旋转MPC约束
3.添加4种整车模式数据结构，及其调用。(未修改业务逻辑)

MPC/custom_type.h

@@ -6,6 +6,57 @@
 #ifndef NAVIGATION_CUSTOM_TYPE_H
 #define NAVIGATION_CUSTOM_TYPE_H
 
+//////////////////////////////////////////////////
+/// 封装的enum模板类,解决原生enum无法参与位运算问题
+template<typename Enum>
+class MyEnum {
+    using Self = MyEnum<Enum>;
+    using EnumType = Enum;
+public:
+    MyEnum(const MyEnum &other)
+            : i(other.i) {
+
+    }
+
+    MyEnum(const Enum &e)
+            : i((int) e) {
+
+    }
+
+    MyEnum(const int ival)
+            : i(ival) {
+
+    }
+
+    operator int() const {
+        return i;
+    }
+
+    MyEnum operator|(const MyEnum &another) {
+        MyEnum g;
+        g.i = another.i | Self::i;
+        return g;
+    }
+
+    MyEnum operator|(const EnumType f) {
+        return *this | MyEnum(f);
+    }
+
+    MyEnum operator&(const MyEnum &another) {
+        MyEnum g;
+        g.i = another.i & Self::i;
+        return g;
+    }
+
+    MyEnum operator&(const EnumType f) {
+        return *this & MyEnum(f);
+    }
+
+private:
+    int i;
+};
+//////////////////////////////////////////////////
+
 //////////////////////////////////////////////////
 /// MPC返回值
 enum MpcError {
@@ -17,12 +68,37 @@ enum MpcError {
 
 //////////////////////////////////////////////////
 /// 基准可选方向
-enum Direction{
+enum Direction {
     eForward = 0x0001,
-    eBackward= 0x0002,
-    eLeft    = 0x0004,
-    eRight   = 0x0008,
+    eBackward = 0x0002,
+    eLeft = 0x0004,
+    eRight = 0x0008
 };
 //////////////////////////////////////////////////
 
+//////////////////////////////////////////////////
+/// AGV运动模式
+enum ActionMode {
+    eSingle = 0x0000,
+    eDouble = 0x0001,
+    eMainInForward = 0x0000,
+    eMainInBackward = 0x0008,
+    eChildSameToMain = 0x0000,
+    eChildOppositeToMain = 0x0002
+};
+//using ActionModes = MyEnum<ActionMode>;
+//////////////////////////////////////////////////
+
+//////////////////////////////////////////////////
+/// AGV指令类型
+enum ActionType {
+    eReady = 0,
+    eStop = 0,
+    eRotation = 1,
+    eHorizontal = 2,
+    eVertical = 3,
+    eClampClose = 5,
+    eClampOpen = 6
+};
+//////////////////////////////////////////////////
 #endif //NAVIGATION_CUSTOM_TYPE_H

MPC/loaded_mpc.cpp

@@ -7,7 +7,7 @@
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
 
-size_t N = 25;                  //优化考虑后面多少步
+size_t N = 10;                  //优化考虑后面多少步
 
 size_t nx = 0;
 size_t ny = nx + N;
@@ -105,26 +105,45 @@ public:
             fg[1 + ndlt + t] = (vars[ndlt + t] - vars[ndlt + t - 1]) / dt;
         }
 
-
-        if (m_statu.size() == 2 + 16) {
-            //与障碍物的距离
-            for (int i = 0; i < 8; ++i) {
-                double ox = m_statu[2 + i * 2];
-                double oy = m_statu[2 + i * 2 + 1];
-                for (int t = 0; t < N; ++t) {
-                    /*第i点的矩形方程为:   {[(x-xi)cos(Θi)-(y-yi)sin(Θi)]/1.35}**8  +
-                     *                  {[(x-xi)sin(Θi)+(y-yi)cos(Θi)]/0.75}**8  = 1.0
-                     *
-                     * */
-                    CppAD::AD<double> ra = (ox - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
-                                           (oy - vars[ny + t]) * CppAD::sin(vars[nth + t]);
-                    CppAD::AD<double> rb = (ox - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
-                                           (oy - vars[ny + t]) * CppAD::cos(vars[nth + t]);
-                    fg[1 + nobs + N * i + t] = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
+        if (m_statu.size() == 2 + 3) {
+            float obs_x = m_statu[2];
+            float obs_y = m_statu[3];
+            float obs_theta = m_statu[4];
+            Pose2d obs_relative_pose(obs_x, obs_y, obs_theta);
+            std::vector<Pose2d> obs_vertexes = Pose2d::generate_rectangle_vertexs(obs_relative_pose, obs_w * 2,
+                                                                                  obs_h * 2);
+            for (auto t = 0; t < N; ++t) {
+                CppAD::AD<double> min_distance = 1e19;
+                for (auto i = 0; i < 8; ++i) {
+                    CppAD::AD<double> ra = (obs_vertexes[i].x() - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
+                                           (obs_vertexes[i].y() - vars[ny + t]) * CppAD::sin(vars[nth + t]);
+                    CppAD::AD<double> rb = (obs_vertexes[i].x() - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
+                                           (obs_vertexes[i].y() - vars[ny + t]) * CppAD::cos(vars[nth + t]);
+                    CppAD::AD<double> distance = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
+                    if (distance < min_distance) min_distance = distance;
                 }
+                fg[nobs + t] = min_distance;
             }
+//            std::cout << " fg[n_obs]: " << fg[n_obs] << " | __LINE__:" << __LINE__ << std::endl;
         }
-
+//        if (m_statu.size() == 2 + 3) {
+//            //与障碍物的距离
+//            for (int i = 0; i < 8; ++i) {
+//                double ox = m_statu[2 + i * 2];
+//                double oy = m_statu[2 + i * 2 + 1];
+//                for (int t = 0; t < N; ++t) {
+//                    /*第i点的矩形方程为:   {[(x-xi)cos(Θi)-(y-yi)sin(Θi)]/1.35}**8  +
+//                     *                  {[(x-xi)sin(Θi)+(y-yi)cos(Θi)]/0.75}**8  = 1.0
+//                     *
+//                     * */
+//                    CppAD::AD<double> ra = (ox - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
+//                                           (oy - vars[ny + t]) * CppAD::sin(vars[nth + t]);
+//                    CppAD::AD<double> rb = (ox - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
+//                                           (oy - vars[ny + t]) * CppAD::cos(vars[nth + t]);
+//                    fg[1 + nobs + N * i + t] = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
+//                }
+//            }
+//        }
     }
 };
 
@@ -221,17 +240,13 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     }
 
     // Lower and upper limits for the constraints
-    size_t n_constraints = N * 5;
     /*
      * 障碍物是否进入 碰撞检测范围内
      */
     float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
-    bool find_obs = false;
-    if (distance < 20) {
-        //printf(" mpc find obs ,w=%f,h=%f\n",obs_w_,obs_h_);
-        find_obs = true;
-    }
-    if (find_obs) n_constraints = N * (5 + 8);
+    bool find_obs = distance < 20;
+    if (find_obs) printf(" find obs ..............................\n");
+    size_t n_constraints = 5 * N + find_obs * N;
 
     Dvector constraints_lowerbound(n_constraints);
     Dvector constraints_upperbound(n_constraints);
@@ -252,7 +267,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     // 与障碍物保持距离的约束
     if (find_obs) {
-        for (int i = nobs; i < nobs + 8 * N; ++i) {
+        for (int i = nobs; i < nobs + N; ++i) {
             constraints_lowerbound[i] = 1.0;
             constraints_upperbound[i] = 1e19;
         }
@@ -278,19 +293,14 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
         angular = -max_dlt;
     }
 
-    Eigen::VectorXd statu_velocity(2);
-    if (find_obs) {
-        statu_velocity = Eigen::VectorXd(2 + 16);
-
-        std::vector<Pose2d> vertexs = Pose2d::generate_rectangle_vertexs(obs_relative_pose_, obs_w_ * 2, obs_h_ * 2);
-        for (int i = 0; i < vertexs.size(); ++i) {
-            //std::cout<<"vetex:"<<vertexs[i]<<std::endl;
-            statu_velocity[2 + i * 2] = vertexs[i].x();
-            statu_velocity[2 + i * 2 + 1] = vertexs[i].y();
-        }
-    }
+    Eigen::VectorXd statu_velocity(2 + 3*find_obs);
     statu_velocity[0] = line_velocity;
     statu_velocity[1] = angular;
+    if (find_obs) {
+        statu_velocity[2]= obs_relative_pose_.x();
+        statu_velocity[3] = obs_relative_pose_.y();
+        statu_velocity[4] = obs_relative_pose_.theta();
+    }
 
     Eigen::VectorXd condition(6);
     condition << dt, ref_velocity, obs_w_, obs_h_, targetPoseInAGV.x(), targetPoseInAGV.y();
@@ -316,7 +326,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     double time =
             double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
 
-    solution.status == CppAD::ipopt::solve_result<Dvector>::success; // invalid_number_detected
+    // invalid_number_detected
     if (solution.status != CppAD::ipopt::solve_result<Dvector>::success) {
         printf(" mpc failed statu : %d  input: %.4f  %.5f(%.5f) max_v:%f\n", solution.status, line_velocity,
                wmg, angular, max_velocity_);

MPC/monitor/monitor_emqx.cpp

@@ -24,13 +24,13 @@ void Monitor_emqx::set_speedcmd_topic(std::string speedcmd)
   speedcmd_topic_=speedcmd;
 }
 
-void Monitor_emqx::clamp_close(ActionMode mode)
+void Monitor_emqx::clamp_close(int mode)
 {
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
   speed.set_h(heat_);
-  speed.set_t(eClampClode);
+  speed.set_t(eClampClose);
 
   speed.set_m(mode);
   speed.set_end(1);
@@ -38,7 +38,7 @@ void Monitor_emqx::clamp_close(ActionMode mode)
   Publish(speedcmd_topic_,msg);
 }
 
-void Monitor_emqx::clamp_open(ActionMode mode)
+void Monitor_emqx::clamp_open(int mode)
 {
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
@@ -52,9 +52,9 @@ void Monitor_emqx::clamp_open(ActionMode mode)
   Publish(speedcmd_topic_,msg);
 }
 
-void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a,double L)
+void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a,double L)
 {
-  if(mode==eMain && type==eVertical &&fabs(L)<1e-3)
+  if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
   {
     printf(" Main agv mpc must set wheelBase\n ");
     return;
@@ -76,7 +76,7 @@ void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a,d
   Publish(speedcmd_topic_,msg);
 
 }
-void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a)
+void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a)
 {
   double w=fabs(a)>0.0001?a:0.0;
   MqttMsg msg;
@@ -90,7 +90,6 @@ void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a)
   speed.set_end(1);
   msg.fromProtoMessage(speed);
   Publish(speedcmd_topic_,msg);
-
 }
 
 void Monitor_emqx::stop()

MPC/monitor/monitor_emqx.h

@@ -7,36 +7,22 @@
 
 #include <mutex>
 #include "terminator_emqx.h"
-
+#include "../custom_type.h"
 
 class Monitor_emqx : public Terminator_emqx
 {
-public:
-    enum ActionMode{
-        eSingle=1,
-        eMain
-    };
-    enum ActionType{
-        eStop=0,
-        eRotate=1,
-        eHorizontal=2,
-        eVertical=3,
-        eClampClode=5,
-        eClampOpen=6
-    };
  public:
     Monitor_emqx(std::string nodeId);
     ~Monitor_emqx();
     void set_speedcmd_topic(std::string speedcmd);
-    void set_speed(ActionMode mode,ActionType type,double v,double a);
-    void set_speed(ActionMode mode,ActionType type,double v,double a,double L);
-    void clamp_close(ActionMode mode);
-    void clamp_open(ActionMode mode);
+    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 clamp_close(int mode);
+    void clamp_open(int mode);
     void stop();
 
  protected:
 
-
     std::string speedcmd_topic_;
 
  public:

MPC/navigation.cpp

@@ -292,11 +292,11 @@ bool Navigation::SlowlyStop() {
             if (v > 0) {
                 double new_v = v - acc * dt;
                 new_v = new_v > 0 ? new_v : 0;
-                SendMoveCmd(move_mode_, Monitor_emqx::ActionType(actionType_), new_v, 0);
+                SendMoveCmd(move_mode_, actionType_, new_v, 0);
             } else {
                 double new_v = v + acc * dt;
                 new_v = new_v < 0 ? new_v : 0;
-                SendMoveCmd(move_mode_, Monitor_emqx::ActionType(actionType_), new_v, 0);
+                SendMoveCmd(move_mode_, actionType_, new_v, 0);
             }
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
         } else {
@@ -312,7 +312,7 @@ bool Navigation::SlowlyStop() {
 Navigation::Navigation() {
     isInSpace_ = false; //是否在车位或者正在进入车位
     RWheel_position_ = eUnknow;
-    move_mode_ = Monitor_emqx::eSingle;
+    move_mode_ = eSingle;
     monitor_ = nullptr;
     terminator_ = nullptr;
 
@@ -386,7 +386,7 @@ bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate,
             double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
             double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);
 
-            SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, limit_angular);
+            SendMoveCmd(move_mode_, eRotation, 0, limit_angular);
             actionType_ = eRotation;
             printf(" Rotate | input angular:%f,next angular:%f,down:%f diff:%f anyDirect:%d\n",
                    timedA_.Get(), angular, limit_angular, minYawdiff, anyDirect);
@@ -475,8 +475,8 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
         }
 
         //下发速度
-        if (fabs(target_yaw_diff) > 1 * M_PI / 180.0) {
-            SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, out[0]);
+        if (fabs(target_yaw_diff) > 2 * M_PI / 180.0) {
+            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
             actionType_ = eRotation;
             printf(" MPC_rotate | input angular_v:%f, down:%f, diff:%f autoDirect:%d\n",
                    timedA_.Get(), out[0], target_yaw_diff, directions);
@@ -619,7 +619,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
     target.set_id(space.id());
 
     //整车协调的时候，保持前后与车位一致即可
-    if (move_mode_ == Monitor_emqx::eMain) {
+    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)) {
@@ -645,7 +645,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 
         //下发速度
         if (fabs(yawDiff) > 1 * M_PI / 180.0) {
-            SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, out[0]);
+            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
             actionType_ = eRotation;
             printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f diff:%f anyDirect:false\n",
                    timedA_.Get(), out[0], yawDiff);
@@ -661,7 +661,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 //            double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
 //            double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);
 //
-//            SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, limit_angular);
+//            SendMoveCmd(move_mode_, Monitor_emqx::eRotation, 0, limit_angular);
 //            actionType_ = eRotation;
 //            printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
 //                   timedA_.Get(), angular, limit_angular, yawDiff);
@@ -774,9 +774,9 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
 
     MpcError ret = failed;
     Pose2d brother = timedBrotherPose_.Get();
-    Pose2d relative2brother = Pose2d::relativePose(brother, pose);
-//    std::cout<<" relative2brother: "<<relative2brother<<std::endl;
-    RotateMPC MPC(relative2brother, obs_w, obs_h, limit_wmg.min, limit_wmg.max);
+    Pose2d brother_in_self = Pose2d::relativePose(brother, pose);
+//    std::cout<<" brother_in_self: "<<brother_in_self<<std::endl;
+    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()};
     ret = MPC.solve(current_to_target, statu, parameter, out);
@@ -836,7 +836,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
         relative = Pose2d::relativePose(brother.rotate(brother.x(), brother.y(), M_PI / 2),
                                         pose.rotate(pose.x(), pose.y(), M_PI / 2));//计算另一节在当前小车坐标系下的坐标
     //std::cout<<"pose:"<<pose<<",  brother:"<<brother<<", relative:"<<relative<<std::endl;
-    if (move_mode_ == Monitor_emqx::ActionMode::eMain)
+    if (move_mode_ == ActionMode::eDouble)
         relative = Pose2d(-1e8, -1e8, 0);
 
     MpcError ret = failed;
@@ -892,15 +892,15 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
 
 }
 
-void Navigation::SendMoveCmd(Monitor_emqx::ActionMode mode, Monitor_emqx::ActionType type,
+void Navigation::SendMoveCmd(int mode, ActionType type,
                              double v, double angular) {
     if (monitor_) {
         monitor_->set_speed(mode, type, v, angular);
-        if (mode == ActionType::eRotation)
+        if (type == eRotation)
             RWheel_position_ = eR;
-        if (mode == ActionType::eVertical)
+        if (type == eVertical)
             RWheel_position_ = eX;
-        if (mode == ActionType::eHorizon)
+        if (type == eHorizontal)
             RWheel_position_ = eY;
     }
 }
@@ -1024,11 +1024,12 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
                 out[0] = -limit_v.min;
         }
         //下发速度
+        printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
         if (directY == false)
-            SendMoveCmd(move_mode_, Monitor_emqx::eVertical, out[0], out[1]);
+            SendMoveCmd(move_mode_, eVertical, out[0], out[1]);
         else
-            SendMoveCmd(move_mode_, Monitor_emqx::eHorizontal, out[0], out[1]);
-        actionType_ = directY ? eHorizon : eVertical;
+            SendMoveCmd(move_mode_, eHorizontal, out[0], out[1]);
+        actionType_ = directY ? eHorizontal : eVertical;
         /*
          * 判断车辆是否在终点附近徘徊，无法到达终点
          */
@@ -1059,8 +1060,8 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
 
     double W = 2.45;
     double L = 1.3;
-    double minYaw = 5 * M_PI / 180.0;
-    if (move_mode_ == Monitor_emqx::eMain) {
+    double minYaw = 3 * M_PI / 180.0;
+    if (move_mode_ == eDouble) {
         L = 1.3 + 2.7;
 
     }
@@ -1164,7 +1165,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 break;
             }
         } else if (act.type() == 8) { //切换模式
-            SwitchMode(Monitor_emqx::ActionMode(act.changedmode()), act.wheelbase());
+            SwitchMode(ActionMode(act.changedmode()), act.wheelbase());
         } else {
             printf(" action type invalid not handled !!\n");
             break;
@@ -1195,7 +1196,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     running_ = false;
 }
 
-void Navigation::SwitchMode(Monitor_emqx::ActionMode mode, float wheelBase) {
+void Navigation::SwitchMode(int mode, float wheelBase) {
     printf("Child AGV can not Switch Mode\n");
 }
 
@@ -1220,22 +1221,22 @@ void Navigation::ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::N
     switch (cmd.operation_type()) {
         case 1: // 旋转
             velocity = 2.0 * M_PI / 180.0 * symbol;
-            SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, velocity);
+            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_, Monitor_emqx::eVertical, velocity, 0);
+            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_, Monitor_emqx::eHorizontal, velocity, 0);
-            actionType_ = eHorizon;
+            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;

MPC/navigation.h

@@ -21,14 +21,6 @@ public:
         double min;
         double max;
     } stLimit;
-    enum ActionType {
-        eReady = 0,
-        eRotation,
-        eHorizon,
-        eVertical,
-        eClampOpen,
-        eClampClose
-    };
     //舵轮位置
     enum RWheelPosition {
         eUnknow = 0,
@@ -86,15 +78,14 @@ public:
 
     bool SlowlyStop();
 
-    virtual void SwitchMode(Monitor_emqx::ActionMode mode, float wheelBase);
+    virtual void SwitchMode(int mode, float wheelBase);
 
     bool PoseTimeout();
 
 protected:
     virtual void HandleAgvStatu(const MqttMsg &msg);
 
-    virtual void SendMoveCmd(Monitor_emqx::ActionMode mode, Monitor_emqx::ActionType type,
-                             double v, double angular);
+    virtual void SendMoveCmd(int mode, ActionType type, double v, double angular);
 
     static void RobotPoseCallback(const MqttMsg &msg, void *context);
 
@@ -140,7 +131,8 @@ protected:
      * space:输入车位点,
      * target:输出目标点
      */
-    virtual Navigation::MpcResult RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
+    virtual Navigation::MpcResult
+    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
 
     virtual bool clamp_close();
 
@@ -195,7 +187,7 @@ protected:
     bool pause_ = false;
     bool cancel_ = false;
 
-    Monitor_emqx::ActionMode move_mode_ = Monitor_emqx::eSingle;
+    int move_mode_ = eSingle;
 
     bool isInSpace_; //是否在车位或者正在进入车位
     std::string space_id_;

MPC/navigation_main.cpp

@@ -7,7 +7,7 @@
 
 
 NavigationMain::NavigationMain() {
-    move_mode_ = Monitor_emqx::eSingle;
+    move_mode_ = eSingle;
     wheelBase_ = 0;
 }
 
@@ -16,7 +16,7 @@ NavigationMain::~NavigationMain() {
 }
 
 void NavigationMain::ResetPose(const Pose2d &pose) {
-    if (move_mode_ == Monitor_emqx::eMain) {
+    if (move_mode_ == eDouble) {
         if (timedBrotherPose_.timeout() == true) {
             std::cout << "Brother pose is timeout can not set MainAGV pose" << std::endl;
             return;
@@ -47,7 +47,7 @@ void NavigationMain::publish_statu(NavMessage::NavStatu &statu) {
 
 
 void NavigationMain::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
-    /*if(move_mode_!=Monitor_emqx::eMain)
+    /*if(move_mode_!=Monitor_emqx::eDouble)
     {
       printf(" navigation mode must set main,parameter:Pose2d\n");
       return false;
@@ -57,7 +57,7 @@ void NavigationMain::Start(const NavMessage::NavCmd &cmd, NavMessage::NavRespons
 
 }
 
-void NavigationMain::SendMoveCmd(Monitor_emqx::ActionMode mode, Monitor_emqx::ActionType type,
+void NavigationMain::SendMoveCmd(int mode, ActionType type,
                                  double v, double angular) {
     if (monitor_) {
         monitor_->set_speed(mode, type, v, angular, wheelBase_);
@@ -89,7 +89,7 @@ void NavigationMain::HandleAgvStatu(const MqttMsg &msg) {
 }
 
 bool NavigationMain::clamp_close() {
-    if (move_mode_ == Monitor_emqx::eSingle)
+    if (move_mode_ == eSingle)
         return Navigation::clamp_close();
     printf("双车夹持\n");
     if (monitor_) {
@@ -112,7 +112,7 @@ bool NavigationMain::clamp_close() {
 }
 
 bool NavigationMain::clamp_open() {
-    if (move_mode_ == Monitor_emqx::eSingle)
+    if (move_mode_ == eSingle)
         return Navigation::clamp_open();
 
     if (monitor_) {
@@ -182,7 +182,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
 
         //下发速度
         if (fabs(yawDiff) > 1 * M_PI / 180.0) {
-            SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, out[0]);
+            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
             actionType_ = eRotation;
             printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f diff:%f anyDirect:false\n",
                    timedA_.Get(), out[0], yawDiff);
@@ -200,7 +200,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
 //      double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
 //      double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);
 //
-//      SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, limit_angular);
+//      SendMoveCmd(move_mode_, Monitor_emqx::eRotation, 0, limit_angular);
 //      actionType_ = eRotation;
 //      printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
 //             timedA_.Get(), angular, limit_angular, yawDiff);

MPC/navigation_main.h

@@ -4,59 +4,80 @@
 
 #ifndef NAVIGATION_NAVIGATION_MAIN_H
 #define NAVIGATION_NAVIGATION_MAIN_H
+
 #include "navigation.h"
 
-class NavigationMain:public Navigation {
+class NavigationMain : public Navigation {
 
 public:
     NavigationMain();
+
     ~NavigationMain();
 
-    virtual void ResetPose(const Pose2d& pose);
-    virtual void Start(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response);
-    virtual void publish_statu(NavMessage::NavStatu& statu);
+    virtual void ResetPose(const Pose2d &pose);
+
+    virtual void Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response);
+
+    virtual void publish_statu(NavMessage::NavStatu &statu);
 
 
     void ResetOtherClamp(ClampStatu statu);
-    virtual void HandleAgvStatu(const MqttMsg& msg);
-    virtual void SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase){
-      if(move_mode_==mode)
-        return;
-      if(mode==Monitor_emqx::eMain)
-      {
-        if(timedBrotherPose_.timeout()==true || timedPose_.timeout()==true)
-        {
-          std::cout<<"Brother/self pose is timeout can not set MainAGV pose"<<std::endl;
-          return;
-        }
-        //Pose2d transform(-wheelBase_/2.0,0,0);
-        //Navigation::ResetPose(pose * transform);
-        Pose2d brother=timedBrotherPose_.Get();
-        Pose2d pose=timedPose_.Get();
-        Pose2d diff=Pose2d::abs(Pose2d::relativePose(brother,pose));
-
-        if(diff.x()>3.4 || diff.x()<2.3 || diff.y()>0.2 || diff.theta()>5*M_PI/180.0)
-        {
-          std::cout<<" distance with two agv is too far diff: "<<diff<<std::endl;
-          return;
-        }
-        printf(" Switch MoveMode --> main\n");
-      }
-      wheelBase_=wheelBase;
-      move_mode_=mode;
 
-    }
+    virtual void HandleAgvStatu(const MqttMsg &msg);
+
+    virtual void SwitchMode(int mode, float wheelBase) {
+        if (move_mode_ == mode)
+            return;
+        if (mode == eDouble) {
+            if (timedBrotherPose_.timeout() == true || timedPose_.timeout() == true) {
+                std::cout << "Brother/self pose is timeout can not set MainAGV pose" << std::endl;
+                return;
+            }
+            //Pose2d transform(-wheelBase_/2.0,0,0);
+            //Navigation::ResetPose(pose * transform);
+            Pose2d brother = timedBrotherPose_.Get();
+            Pose2d pose = timedPose_.Get();
+            Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));
+
+            if (diff.x() > 3.4 || diff.x() < 2.3 || diff.y() > 0.2 || diff.theta() > 5 * M_PI / 180.0) {
+                std::cout << " distance with two agv is too far diff: " << diff << std::endl;
+                return;
+            }
 
+            Pose2d self_pose = timedPose_.Get();
+            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;
+
+            printf(" Switch MoveMode --> main(%d)\n", mode);
+        }
+        wheelBase_ = wheelBase;
+        move_mode_ = mode;
+    };
 
 protected:
-    virtual Navigation::MpcResult RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelbase,NavMessage::PathNode& target);
-    virtual void SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
-                             double v,double angular);
+    virtual Navigation::MpcResult
+    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
+
+    virtual void SendMoveCmd(int mode, ActionType type,
+                             double v, double angular);
+
     virtual bool clamp_close();
+
     virtual bool clamp_open();
-    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu);
+
+    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu);
+
 protected:
-    float  wheelBase_;   //两节agv的位姿与单节的位姿变换
+    float wheelBase_;   //两节agv的位姿与单节的位姿变换
     TimedLockData<int> timed_other_clamp_;
 };
 

MPC/pose2d.h

@@ -110,6 +110,7 @@ class Pose2d
     const float gridient()const;
 
     static float distance(const Pose2d& pose1,const Pose2d& pose2);
+    //前者在后者坐标系下的位姿
     static Pose2d relativePose(const Pose2d& world_pose,const Pose2d& axis_pose);
     //返回值(-PI,PI]
     static float vector2yaw(float x,float y);

MPC/rotate_mpc.cpp

@@ -55,7 +55,7 @@ public:
         int n_obs = n_angular_acceleration + N;
 
         /// 角度等式约束 1~N
-        fg[n_angular] = vars[0] - angular_velocity * dt;
+        fg[n_angular] = vars[0] - vars[N] * dt;
         for (int t = 1; t < N; ++t) {
             // State at time t + 1
             CppAD::AD<double> theta1 = vars[t];
@@ -71,7 +71,7 @@ public:
         /// 角加速度不等式约束 N+1~2N
         fg[n_angular_acceleration] = (vars[0 + N] - angular_velocity) / dt;
         for (int t = 1; t < N; ++t) {
-            fg[n_angular_acceleration + t] = (vars[1 + t + N] - vars[t + N]) / dt;
+            fg[n_angular_acceleration + t] = (vars[t + N] - vars[t + N - 1]) / dt;
         }
 
         /// 障碍物距离不等式约束 2N+1~3N
@@ -82,14 +82,13 @@ public:
             Pose2d obs_relative_pose(obs_x, obs_y, obs_theta);
             std::vector<Pose2d> obs_vertexes = Pose2d::generate_rectangle_vertexs(obs_relative_pose, obs_w * 2,
                                                                                   obs_h * 2);
-
             for (auto t = 0; t < N; ++t) {
                 CppAD::AD<double> min_distance = 1e19;
-                for (auto i = 0; i < 7; ++i) {
-                    CppAD::AD<double> ra = obs_x * CppAD::cos(vars[t]) -
-                                           obs_y * CppAD::sin(vars[t]);
-                    CppAD::AD<double> rb = obs_x * CppAD::sin(vars[t]) +
-                                           obs_y * CppAD::cos(vars[t]);
+                for (auto i = 0; i < 8; ++i) {
+                    CppAD::AD<double> ra = obs_vertexes[i].x() * CppAD::cos(vars[t]) -
+                                           obs_vertexes[i].y() * CppAD::sin(vars[t]);
+                    CppAD::AD<double> rb = obs_vertexes[i].x() * CppAD::sin(vars[t]) +
+                                           obs_vertexes[i].y() * CppAD::cos(vars[t]);
                     CppAD::AD<double> distance = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
 
                     if (distance < min_distance) min_distance = distance;
@@ -121,9 +120,9 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     /// 定义最大最小角度,角速度,角加速度(不区分方向)
     double max_angular = M_PI;
-    double max_angular_velocity = 10.0 / 180 * M_PI;
+    double max_angular_velocity = 6.0 / 180 * M_PI;
     double min_angular_velocity = 0.1 / 180 * M_PI;
-    double max_angular_acceleration_velocity = 8.0 / 180 * M_PI;//mpc_param.acc_angular; //
+    double max_angular_acceleration_velocity = 3.0 / 180 * M_PI;//mpc_param.acc_angular; //
 
     // 优化
     typedef CPPAD_TESTVECTOR(double) Dvector;
@@ -136,18 +135,17 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
     Dvector vars(n_vars); // 0-N为角度，N-2N为角速度
 
     for (int i = 0; i < n_vars; i++) vars[i] = 0.0;
-    vars[N] = angular_velocity;
 
     // 参数上下限
     Dvector vars_lowerbound(n_vars);
     Dvector vars_upperbound(n_vars);
-    for (int i = 0; i < n_vars; i++) {
+    for (int i = 0; i < n_vars; ++i) {
         vars_lowerbound[i] = -1.0e19;
         vars_upperbound[i] = 1.0e19;
     }
 
     /// limit of angular velocity
-    for (auto i = N; i < 2 * N; i++) {
+    for (auto i = N; i < 2 * N; ++i) {
         vars_lowerbound[i] = -max_angular_velocity;
         vars_upperbound[i] = max_angular_velocity;
     }
@@ -155,7 +153,8 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
     // Lower and upper limits for the constraints
     /// 障碍物是否进入 碰撞检测范围内
     float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
-    bool find_obs = distance < 5;
+    bool find_obs = distance < 20;
+    if (find_obs) printf(" find obs ..............................\n");
     size_t n_constraints = 2 * N + find_obs * N;// 0~N角度，N~2N角加速度, 2N~3N障碍物约束
     Dvector constraints_lowerbound(n_constraints);
     Dvector constraints_upperbound(n_constraints);

config/navigation.prototxt

@@ -1,6 +1,6 @@
 
-
 main_agv:false
+rpc_ipport:"192.168.0.71:9090"
 Agv_emqx
 {
 	NodeId:"agv-child"
@@ -18,7 +18,6 @@ Terminal_emqx
         port:1883
         pubStatuTopic:"agv_child/agv_statu"
         pubNavStatuTopic:"agv_child/nav_statu"
-        subNavCmdTopic:"agv_child/nav_cmd"
 }
 
 #连接主PLC的emqx

config/navigation_main.prototxt

@@ -1,5 +1,6 @@
 
 main_agv:true
+rpc_ipport:"192.168.0.70:9090"
 Agv_emqx
 {
 	NodeId:"agv-main"
@@ -17,7 +18,6 @@ Terminal_emqx
 	port:1883
 	pubStatuTopic:"agv_main/agv_statu"
 	pubNavStatuTopic:"agv_main/nav_statu"
-	subNavCmdTopic:"agv_main/nav_cmd"
 }
 
 #连接后车的emqx

config/webots/navigation.prototxt

@@ -1,11 +1,10 @@
 
-
 main_agv:false
 rpc_ipport:"127.0.0.1:9091"
 Agv_emqx
 {
 	NodeId:"agv-Child"
-	ip:"192.168.2.179"
+	ip:"127.0.0.1"
 	port:1883
 	pubSpeedTopic:"ChildPLC/speedcmd"
 	subPoseTopic:"lidar_odom/child"

config/webots/navigation_main.prototxt

@@ -4,7 +4,7 @@ rpc_ipport:"127.0.0.1:9090"
 Agv_emqx
 {
 	NodeId:"agv-main"
-	ip:"192.168.2.179"
+	ip:"127.0.0.1"
 	port:1883
 	pubSpeedTopic:"MainPLC/speedcmd"
 	subPoseTopic:"lidar_odom/main"