brother_pose默认值为(-100,0,0)

MPC/loaded_mpc.cpp

@@ -153,7 +153,6 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     if (wmg < 0) angular = -angular;
   }
   double max_wmg=fabs(line_velocity) / radius;
-  double min_wmg=-max_wmg;
 
   std::vector<Pose2d> filte_poses;
   if (filte_Path(pose_agv, target, trajectory, filte_poses, 50) == false)
@@ -197,8 +196,6 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   double max_acc_dlt=20.*M_PI/180.0;
 
   size_t n_vars = N * 5;
-
-
   Dvector vars(n_vars);
   for (int i = 0; i < n_vars; i++)
   {
@@ -207,7 +204,6 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
 
   Dvector vars_lowerbound(n_vars);
   Dvector vars_upperbound(n_vars);
-
   for (int i = 0; i < n_vars; i++)
   {
     vars_lowerbound[i] = -1.0e19;

MPC/navigation.cpp

@@ -209,6 +209,8 @@ Navigation::Navigation()
   thread_= nullptr;
   monitor_= nullptr;
   terminator_= nullptr;
+
+  timedBrotherPose_.reset(Pose2d(-100,0,0),0.1);
 }
 
 void Navigation::BrotherAgvStatuCallback(const MqttMsg& msg,void* context)
@@ -308,7 +310,7 @@ bool Navigation::exec_adjust_action(const Pose2d& target,const Pose2d& target_di
     {
       if (action == 1) {
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
-          double theta = limit(diff.theta(),5*M_PI/180.0,40*M_PI/180.0);
+          double theta = limit(diff.theta(),1*M_PI/180.0,5*M_PI/180.0);
           monitor_->set_speed(Monitor_emqx::eRotate, 0, theta);
           printf(" Ratate :%f\n",theta);
           continue;

projects/controllers/agv_child/AGV_controller.cpp

@@ -94,7 +94,7 @@ void Vrtical(double velocity,double& R1,double& R2,double& R3,double& R4,
 
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV-child","monitor_child/speedcmd"))
+  if(false==init_mqtt("192.168.0.70",1883,"webots-AGV-child","monitor_child/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;
@@ -262,7 +262,7 @@ int main(int argc, char **argv) {
 
             if(BaseR<0.05)
               BaseR=0.05;
-            printf("BaseR : %f \n",BaseR);
+            //printf("BaseR : %f \n",BaseR);
 
             if (BaseR > 0) {
               if (cmd_vth * cmd_v >= 0) {
@@ -293,10 +293,9 @@ int main(int argc, char **argv) {
       }
     }
 
-    printf(" R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",
+    printf("Child R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",
             R1,R2,R3,R4,L1,L2,L3,L4,v,vth);
      
-     
     RM1->setPosition(R1);
     RM2->setPosition(R2);
     RM3->setPosition(R3);