mpc选点数量设置为10个

MPC/loaded_mpc.cpp

@@ -158,7 +158,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   double max_wmg=0.5/radius;//fabs(line_velocity) / radius;
 
   std::vector<Pose2d> filte_poses;
-  if (filte_Path(pose_agv, target, trajectory, filte_poses, 50) == false)
+  if (filte_Path(pose_agv, target, trajectory, filte_poses, 10) == false)
   {
     printf( "filte path failed ...\n");
     return false;

MPC/navigation.cpp

@@ -332,15 +332,16 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
     //停止状态
     if (action == 0) {
       //优先进入旋转
-      if (Pose2d::abs(diff).theta() > thresh.theta()) {
-        action = 1; //原地旋转
-      } else if (Pose2d::abs(diff).x() > thresh.x()) {
+     if (Pose2d::abs(diff).x() > thresh.x()) {
         //前进
         action = 3;
       }else if (Pose2d::abs(diff).y() > thresh.y()) {
         //横移
         action = 2;
       }
+      else if (Pose2d::abs(diff).theta() > thresh.theta()) {
+       action = 1; //原地旋转
+      }
     }
     else
     {
@@ -348,6 +349,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
           double theta = limit(diff.theta(),limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
           SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, theta);
+          //std::cout<<"current:"<<timedPose_.Get()<<"  target:"<<target<<" diff:"<<diff<<std::endl;
           printf(" Ratate :%f\n",theta);
           continue;
         }

config/navigation_child.prototxt

@@ -2,7 +2,7 @@
 main_agv:true
 Agv_emqx
 {
-	NodeId:"agv-child"
+	NodeId:"agv-main"
 	ip:"192.168.0.70"
 	port:1883
 	pubSpeedTopic:"monitor_child/speedcmd"
@@ -15,8 +15,8 @@ Terminal_emqx
 	NodeId:"agv1-child-nav"
 	ip:"192.168.0.70"
 	port:1883
-	pubStatuTopic:"agv1_child/agv_statu"
-	pubNavStatuTopic:"agv1_child/nav_statu"
-	subNavCmdTopic:"agv1_child/nav_cmd"
-	subBrotherStatuTopic:"agv1/agv_statu"
+	pubStatuTopic:"agv_main/agv_statu"
+	pubNavStatuTopic:"agv_main/nav_statu"
+	subNavCmdTopic:"agv_main/nav_cmd"
+	subBrotherStatuTopic:"agv_child/agv_statu"
 }