1.优化巡线MPC约束

3.调整4种整车模式数据结构，及其调用

MPC/custom_type.h

@@ -77,8 +77,8 @@ enum Direction {
 //////////////////////////////////////////////////
 
 //////////////////////////////////////////////////
-/// AGV运动模式
-enum ActionMode {
+/// AGV运动模式(最低1位：0单车，1整车。低2位：0同向，1反向。低3位：0主车在前，1主车在后)
+enum ActionMode{
     eSingle = 0x0000,
     eDouble = 0x0001,
     eMainInForward = 0x0000,

MPC/loaded_mpc.cpp

@@ -244,7 +244,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
      * 障碍物是否进入 碰撞检测范围内
      */
     float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
-    bool find_obs = distance < 20;
+    bool find_obs = distance < 10;
     if (find_obs) printf(" find obs ..............................\n");
     size_t n_constraints = 5 * N + find_obs * N;
 
@@ -293,18 +293,18 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
         angular = -max_dlt;
     }
 
-    Eigen::VectorXd statu_velocity(2 + 3*find_obs);
-    statu_velocity[0] = line_velocity;
-    statu_velocity[1] = angular;
+    Eigen::VectorXd status(2 + 3 * find_obs);
+    status[0] = line_velocity;
+    status[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();
+        status[2]= obs_relative_pose_.x();
+        status[3] = obs_relative_pose_.y();
+        status[4] = obs_relative_pose_.theta();
     }
 
     Eigen::VectorXd condition(6);
     condition << dt, ref_velocity, obs_w_, obs_h_, targetPoseInAGV.x(), targetPoseInAGV.y();
-    FG_eval_half_agv fg_eval(coef, statu_velocity, condition);
+    FG_eval_half_agv fg_eval(coef, status, condition);
 
     // options for IPOPT solver
     std::string options;

MPC/navigation.cpp

@@ -416,7 +416,7 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
     printf(" exec MPC_rotate autoDirect:%d\n", directions);
 
     while (cancel_ == false) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        std::this_thread::sleep_for(std::chrono::milliseconds(30));
         if (pause_ == true) {
             //发送暂停消息
             continue;
@@ -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_ == ActionMode::eDouble)
+    if (move_mode_ == eDouble)
         relative = Pose2d(-1e8, -1e8, 0);
 
     MpcError ret = failed;
@@ -1090,12 +1090,12 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
                    minR,ax,ay);*/
             return true;
         } else if (nagY && posiY) {
-            std::cout << "nagY && posiY" << std::endl;
+//            std::cout << "nagY && posiY" << std::endl;
             return true;
         }
-        printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n", directY,
-               relative.x(), relative.y(), targetNode.l(), targetNode.w(), targetNode.theta(),
-               minR, ax, ay);
+//        printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n", directY,
+//               relative.x(), relative.y(), targetNode.l(), targetNode.w(), targetNode.theta(),
+//               minR, ax, ay);
     }
 
 
@@ -1165,7 +1165,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 break;
             }
         } else if (act.type() == 8) { //切换模式
-            SwitchMode(ActionMode(act.changedmode()), act.wheelbase());
+            SwitchMode(act.changedmode(), act.wheelbase());
         } else {
             printf(" action type invalid not handled !!\n");
             break;

MPC/rotate_mpc.cpp

@@ -153,7 +153,7 @@ 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 < 20;
+    bool find_obs = distance < 5;
     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);

config/navigation.prototxt

@@ -23,7 +23,7 @@ Terminal_emqx
 #连接主PLC的emqx
 brother_emqx
 {
-	NodeId:"agv-child"
+	NodeId:"agv-brother-main"
 	ip:"192.168.0.70"
 	port:1883
 	subBrotherStatuTopic:"agv_main/nav_statu"

config/navigation_main.prototxt

@@ -1,4 +1,4 @@
-
+#127.0.0.1
 main_agv:true
 rpc_ipport:"192.168.0.70:9090"
 Agv_emqx
@@ -6,9 +6,9 @@ Agv_emqx
 	NodeId:"agv-main"
 	ip:"192.168.0.70"
 	port:1883
-	pubSpeedTopic:"monitor_child/speedcmd"
-	subPoseTopic:"monitor_child/statu"
-	subSpeedTopic:"monitor_child/speed"
+	pubSpeedTopic:"monitor_main/speedcmd"
+	subPoseTopic:"monitor_main/statu"
+	subSpeedTopic:"monitor_main/speed"
 }
 
 Terminal_emqx
@@ -23,7 +23,7 @@ Terminal_emqx
 #连接后车的emqx
 brother_emqx
 {
-	NodeId:"agv-main"
+	NodeId:"agv-brother-child"
 	ip:"192.168.0.71"
 	port:1883
 	subBrotherStatuTopic:"agv_child/nav_statu"

config/webots/navigation.prototxt

@@ -1,10 +1,10 @@
 
 main_agv:false
-rpc_ipport:"127.0.0.1:9091"
+rpc_ipport:"192.168.0.100:9091"
 Agv_emqx
 {
 	NodeId:"agv-Child"
-	ip:"127.0.0.1"
+	ip:"192.168.0.100"
 	port:1883
 	pubSpeedTopic:"ChildPLC/speedcmd"
 	subPoseTopic:"lidar_odom/child"
@@ -14,7 +14,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-child-nav"
-        ip:"127.0.0.1"
+        ip:"192.168.0.100"
         port:1883
         pubStatuTopic:"agv_child/agv_statu"
         pubNavStatuTopic:"agv_child/nav_statu"
@@ -24,7 +24,7 @@ Terminal_emqx
 brother_emqx
 {
 	NodeId:"agv-brother-child"
-	ip:"127.0.0.1"
+	ip:"192.168.0.100"
 	port:1883
 	subBrotherStatuTopic:"agv_main/nav_statu"
 }

config/webots/navigation_main.prototxt

@@ -1,10 +1,10 @@
 
 main_agv:true
-rpc_ipport:"127.0.0.1:9090"
+rpc_ipport:"192.168.0.100:9090"
 Agv_emqx
 {
 	NodeId:"agv-main"
-	ip:"127.0.0.1"
+	ip:"192.168.0.100"
 	port:1883
 	pubSpeedTopic:"MainPLC/speedcmd"
 	subPoseTopic:"lidar_odom/main"
@@ -14,7 +14,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-main-nav"
-	ip:"127.0.0.1"
+	ip:"192.168.0.100"
 	port:1883
 	pubStatuTopic:"agv_main/agv_statu"
 	pubNavStatuTopic:"agv_main/nav_statu"
@@ -24,7 +24,7 @@ Terminal_emqx
 brother_emqx
 {
 	NodeId:"agv-brother-main"
-	ip:"127.0.0.1"
+	ip:"192.168.0.100"
 	port:1883
 	subBrotherStatuTopic:"agv_child/nav_statu"
 }