修复相对位姿计算角度bug

MPC/loaded_mpc.cpp

@@ -157,7 +157,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   if(ref_velocity<0.05)
     ref_velocity=0.05;
   //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0
-  Pose2d targetPoseInAGV=Pose2d::PoseByPose(target,pose_agv);
+  Pose2d targetPoseInAGV=Pose2d::TargetByPose(target,pose_agv);
 
   if(targetPoseInAGV.x()<0)
     ref_velocity = -ref_velocity;

MPC/monitor/monitor_emqx.h

@@ -16,7 +16,7 @@ public:
     enum SpeedType{
         eRotate=0,
         eHorizontal,
-        evVrtical,
+        eVrtical,
         eMPC,
 
 

MPC/navigation.cpp

@@ -76,9 +76,9 @@ bool Navigation::Init(const Navigation_parameter& parameter)
 
 }
 
-void Navigation::publish_statu()
-{
-  while(exit_==false) {
+void Navigation::publish_statu() {
+  while (exit_ == false)
+  {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
     NavMessage::NavStatu statu;
     statu.set_statu(eReady); //默认ready
@@ -96,45 +96,44 @@ void Navigation::publish_statu()
         actios.pop();
 
       }
-
-      statu.set_pose_timeout(true);    //默认设置位姿超时
-      statu.set_twist_timeout(true);
-      NavMessage::AGVStatu agvStatu;
-      if (timedPose_.timeout() == false) {
-        statu.set_pose_timeout(false);
-        Pose2d pose = timedPose_.Get();
-        agvStatu.set_x(pose.x());
-        agvStatu.set_y(pose.y());
-        agvStatu.set_theta(pose.theta());
-      }
-      if (timedV_.timeout() == false && timedA_.timeout() == false) {
-        statu.set_twist_timeout(false);
-        agvStatu.set_v(timedV_.Get());
-        agvStatu.set_vth(timedA_.Get());
-      }
-      statu.mutable_current_pose()->CopyFrom(agvStatu);
-
-      if (current_action != nullptr) {
-        delete current_action;
-        //发布mpc 预选点
-        if (selected_traj_.timeout() == false) {
-          for (int i = 0; i < selected_traj_.Get().size(); ++i) {
-            Pose2d pt = selected_traj_.Get()[i];
-            NavMessage::Pose2d *pose = statu.mutable_selected_traj()->add_poses();
-            pose->set_x(pt.x());
-            pose->set_y(pt.y());
-            pose->set_theta(pt.theta());
-          }
+    }
+    statu.set_pose_timeout(true);    //默认设置位姿超时
+    statu.set_twist_timeout(true);
+    NavMessage::AGVStatu agvStatu;
+    if (timedPose_.timeout() == false) {
+      statu.set_pose_timeout(false);
+      Pose2d pose = timedPose_.Get();
+      agvStatu.set_x(pose.x());
+      agvStatu.set_y(pose.y());
+      agvStatu.set_theta(pose.theta());
+    }
+    if (timedV_.timeout() == false && timedA_.timeout() == false) {
+      statu.set_twist_timeout(false);
+      agvStatu.set_v(timedV_.Get());
+      agvStatu.set_vth(timedA_.Get());
+    }
+    statu.mutable_current_pose()->CopyFrom(agvStatu);
+
+    if (current_action != nullptr) {
+      delete current_action;
+      //发布mpc 预选点
+      if (selected_traj_.timeout() == false) {
+        for (int i = 0; i < selected_traj_.Get().size(); ++i) {
+          Pose2d pt = selected_traj_.Get()[i];
+          NavMessage::Pose2d *pose = statu.mutable_selected_traj()->add_poses();
+          pose->set_x(pt.x());
+          pose->set_y(pt.y());
+          pose->set_theta(pt.theta());
         }
-        //发布 mpc 预测点
-        if (predict_traj_.timeout() == false) {
-          for (int i = 0; i < predict_traj_.Get().size(); ++i) {
-            Pose2d pt = predict_traj_.Get()[i];
-            NavMessage::Pose2d *pose = statu.mutable_predict_traj()->add_poses();
-            pose->set_x(pt.x());
-            pose->set_y(pt.y());
-            pose->set_theta(pt.theta());
-          }
+      }
+      //发布 mpc 预测点
+      if (predict_traj_.timeout() == false) {
+        for (int i = 0; i < predict_traj_.Get().size(); ++i) {
+          Pose2d pt = predict_traj_.Get()[i];
+          NavMessage::Pose2d *pose = statu.mutable_predict_traj()->add_poses();
+          pose->set_x(pt.x());
+          pose->set_y(pt.y());
+          pose->set_theta(pt.theta());
         }
       }
     }
@@ -249,23 +248,24 @@ bool Navigation::exec_adjust_action(const Pose2d& target,const Pose2d& target_di
     //判断是否到达目标点
     if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff)) {
       monitor_->stop();
+      printf("adjust completed\n");
       return true;
     }
 
     //计算目标点在当前pose下的pose
-    Pose2d diff = Pose2d::PoseByPose(target, timedPose_.Get());
+    Pose2d diff = Pose2d::TargetByPose(target, timedPose_.Get());
 
     //停止状态
     if (action == 0) {
       //优先进入旋转
       if (Pose2d::abs(diff).theta() > thresh.theta()) {
         action = 1; //原地旋转
-      } else if (Pose2d::abs(diff).y() > thresh.y()) {
-        //横移
-        action = 2;
       } else if (Pose2d::abs(diff).x() > thresh.x()) {
         //前进
         action = 3;
+      }else if (Pose2d::abs(diff).y() > thresh.y()) {
+        //横移
+        action = 2;
       }
     }
     else
@@ -274,23 +274,26 @@ bool Navigation::exec_adjust_action(const Pose2d& target,const Pose2d& target_di
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
           double theta = limit(diff.theta(),5*M_PI/180.0,40*M_PI/180.0);
           monitor_->set_speed(Monitor_emqx::eRotate, 0, theta);
+          printf(" Ratate :%f\n",theta);
           continue;
         }
       }
       if (action == 2) {
         if (Pose2d::abs(diff).y() > thresh.y()) {
           monitor_->set_speed(Monitor_emqx::eHorizontal, limit(diff.y(),0.05,0.2), 0);
+          printf(" Horizontal :%f\n",limit(diff.y(),0.05,0.2));
           continue;
         }
       }
       if (action == 3) {
         if (Pose2d::abs(diff).x() > thresh.x()) {
-          monitor_->set_speed(Monitor_emqx::eHorizontal, limit(diff.x(),0.05,0.5), 0);
+          monitor_->set_speed(Monitor_emqx::eVrtical, limit(diff.x(),0.05,0.5), 0);
+          printf(" Vrtical :%f\n",limit(diff.x(),0.05,0.5));
           continue;
         }
       }
       monitor_->stop();
-      printf(" action :%d completed!!! reset action type!!!\n", action);
+      printf(" monitor type :%d completed!!! reset action type!!!\n", action);
       action = 0;
     }
   }
@@ -335,9 +338,10 @@ bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
 bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
                            const Pose2d& target,const Pose2d& diff)
 {
-    if(Pose2d::abs(cur-target)<diff)
+  Pose2d distance=Pose2d::TargetByPose(target,cur);
+    if(Pose2d::abs(distance)<diff)
     {
-        if(fabs(velocity)<0.1 && angular< 10*M_PI/180.0)
+        if(fabs(velocity)<0.1 && fabs(angular)< 10*M_PI/180.0)
             return true;
     }
     return false;
@@ -373,7 +377,7 @@ bool Navigation::execute_along_action(const Pose2d& target,const Pose2d& target_
     if(IsArrived(timedPose_.Get(),timedV_.Get(),timedA_.Get(),target,target_diff))
     {
         monitor_->stop();
-        printf(" edge navigation completed !!!\n");
+        printf(" exec along completed !!!\n");
         return true;
     }
       //一次变速
@@ -412,13 +416,24 @@ void Navigation::navigatting()
     Pose2d target_diff(act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
     if(act.type()==1)
     {
+      printf(" adjust to (%.3f %.3f %.3f) diff:(%.3f,%.3f,%.3f)\n",
+             act.target().x(),act.target().y(),act.target().theta(),
+             act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
       if(exec_adjust_action(target,target_diff)==false)
+      {
+        printf("execute adjust action failed\n");
         break;
+      }
     }
-    if(act.type()==2)
+    else if(act.type()==2)
     {
-      if(execute_along_action(target,target_diff)==false)
+      printf(" MPC to (%.3f %.3f %.3f) diff:(%.3f,%.3f,%.3f)\n",
+             act.target().x(),act.target().y(),act.target().theta(),
+             act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
+      if(execute_along_action(target,target_diff)==false) {
+        printf("execute along action failed\n");
         break;
+      }
     }
     else
     {
@@ -436,6 +451,11 @@ void Navigation::navigatting()
   }else {
     if (unfinished_cations_.empty())
       printf("navigation completed!!!\n");
+    else{
+      printf(" navigation Failed\n");
+      while(unfinished_cations_.empty()==false)
+        unfinished_cations_.pop();
+    }
   }
   running_=false;
 }

MPC/pose2d.cpp

@@ -32,11 +32,24 @@ const float Pose2d::gridient() const
   return gradient;
 }
 
-Pose2d Pose2d::PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose)
+Pose2d Pose2d::TargetByPose(const Pose2d& target_pose,const Pose2d& axis_pose)
 {
-  Pose2d diff=world_pose-axis_pose;
+  Pose2d diff=target_pose-axis_pose;
   Pose2d nPose=diff.rotate(-axis_pose.theta());
-  return Pose2d(nPose.x(),nPose.y(),diff.theta());
+
+  float new_theta=diff.theta();
+  //转换到 [-pi/2, pi/2]下
+  int n=int(new_theta/(M_PI));
+  if(new_theta<-M_PI/2)
+  {
+    new_theta += M_PI*(n+1);
+  }
+  if(new_theta>M_PI/2)
+  {
+    new_theta -= M_PI*(n+1);
+  }
+
+  return Pose2d(nPose.x(),nPose.y(),new_theta);
 }
 
 float Pose2d::distance(const Pose2d& pose1,const Pose2d& pose2)
@@ -53,7 +66,17 @@ Pose2d Pose2d::rotate(float theta)const
   float y=sn*this->x()+cs*this->y();
   float new_theta=this->theta()+theta;
 
-  //转换到 [-pi/2, pi/2]下
+
+  if(theta<0)
+    new_theta=new_theta+2*M_PI;
+
+  if(new_theta>=2*M_PI)
+    new_theta-=2*M_PI;
+
+  if(new_theta>M_PI)
+    new_theta=new_theta-2*M_PI;
+
+  /*//转换到 [-pi/2, pi/2]下
   int n=int(new_theta/(M_PI));
   if(new_theta<-M_PI/2)
   {
@@ -62,7 +85,7 @@ Pose2d Pose2d::rotate(float theta)const
   if(new_theta>M_PI/2)
   {
     new_theta -= M_PI*(n+1);
-  }
+  }*/
 
   return Pose2d(x,y,new_theta);
 }

MPC/pose2d.h

@@ -95,7 +95,7 @@ class Pose2d
     const float gridient()const;
 
     static float distance(const Pose2d& pose1,const Pose2d& pose2);
-    static Pose2d PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose);
+    static Pose2d TargetByPose(const Pose2d& world_pose,const Pose2d& axis_pose);
 
  protected:
     std::atomic<float>                      m_x;

message.proto

@@ -9,7 +9,7 @@ message AGVStatu {
 }
 
 message Speed {
-  int32 type=1; // 0原地旋转，1横移，2前进，3MPC巡线, 其他：停止
+  int32 type=1; // 0原地旋转，1横移，2前进，3MPC巡线, 其他/未接收到：停止
   float v=2;
   float vth=3;
 }