修改两车整体朝向由两车位姿计算

MPC/navigation.cpp

@@ -170,7 +170,7 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
 
     //发布nav状态
     statu.set_statu(actionType_); //
-    printf(" statu:%d\n",actionType_);
+    //printf(" statu:%d\n",actionType_);
     statu.set_move_mode(move_mode_);
     NavMessage::Action *current_action = nullptr;
     if (running_)
@@ -527,7 +527,7 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
   Pose2d distance=Pose2d::relativePose(target,cur);
     if(Pose2d::abs(distance)<diff)
     {
-        if(fabs(velocity)<0.1 && fabs(angular)< 10*M_PI/180.0)
+        if(fabs(velocity)<0.05 && fabs(angular)< 10*M_PI/180.0)
             return true;
     }
   //std::cout<<"distance:"<<distance<<"  diff:"<<diff<<"  v:"<<velocity<<"  a:"<<angular<<std::endl;

MPC/navigation_main.cpp

@@ -25,12 +25,15 @@ void NavigationMain::ResetPose(const Pose2d& pose){
     Pose2d brother=timedBrotherPose_.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)
+    if(diff.x()>3.6 || diff.x()<2.2 || 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 agv=Pose2d((pose.x()+brother.x())/2.0,(pose.y()+brother.y())/2.0,pose.theta());
+    Pose2d abs_diff=pose-brother;
+    //计算两车朝向的方向
+    float theta=Pose2d::vector2yaw(abs_diff.x(),abs_diff.y());
+    Pose2d agv=Pose2d((pose.x()+brother.x())/2.0,(pose.y()+brother.y())/2.0,theta);
 
     Navigation::ResetPose(agv);
   }

MPC/pose2d.cpp

@@ -32,6 +32,19 @@ const float Pose2d::gridient() const
   return gradient;
 }
 
+float Pose2d::vector2yaw(float x,float y)
+{
+  float r=sqrt(x*x+y*y);
+  if(r<1e-8)
+    return 0;
+  float yaw=asin(y/r);
+  if(x>=0)  //1,4象限
+    return yaw;
+  if (x<0 ) //2 3象限
+    return M_PI-yaw;
+
+}
+
 Pose2d Pose2d::relativePose(const Pose2d& target_pose,const Pose2d& axis_pose)
 {
   Pose2d diff=target_pose-axis_pose;

MPC/pose2d.h

@@ -107,6 +107,8 @@ class Pose2d
 
     static float distance(const Pose2d& pose1,const Pose2d& pose2);
     static Pose2d relativePose(const Pose2d& world_pose,const Pose2d& axis_pose);
+    //返回值[0,2PI)
+    static float vector2yaw(float x,float y);
 
  protected:
     std::atomic<float>                      m_x;