delete adjust

MPC/navigation.cpp

@@ -393,121 +393,6 @@ void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
 }
 
 
-
-bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,stLimit limit_v,
-                                    stLimit limit_h,stLimit limit_rotate,bool anyDitect)
-{
-  std::cout<<" adjust target:"<<target<<" target diff:"<<target_diff<<std::endl;
-  if(inited_==false) {
-    printf(" navigation has not inited\n");
-    return false;
-  }
-
-  Pose2d accuracy=Pose2d::abs(target_diff.rotate(timedPose_.Get().theta()));
-  Pose2d thresh=Pose2d::abs(accuracy);
-
-  int action=0; //1 原地旋转，2横移，3前进
-  while(cancel_==false) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
-    if (pause_ == true) {
-      //发送暂停消息
-      continue;
-    }
-    if (PoseTimeout() || timedV_.timeout() || timedA_.timeout()) {
-      printf(" navigation Error:Pose/v/a is timeout \n");
-      break;
-    }
-    //判断是否到达目标点
-    int direct=0;
-    if(anyDitect) direct=2;
-    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, accuracy,direct)) {
-      if(Stop()) {
-        printf("adjust completed anyDitect :%d\n",anyDitect);
-        return true;
-      }
-      return false;
-    }
-    
-    //计算目标点在当前pose下的pose
-    Pose2d diff = Pose2d::relativePose(target,timedPose_.Get());
-    //停止状态
-    if (action == 0) {
-      //
-     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
-    {
-      double acc_v=0.6;
-      double acc_angular=10.0*M_PI/180.0;
-      double dt=0.1;
-      if (action == 1) {
-        float minYawDiff=diff.theta();
-        if(anyDitect){
-          Pose2d p0=timedPose_.Get();
-          float yawdiff[4]={0};
-          yawdiff[0]=diff.theta();
-          yawdiff[1]=Pose2d::relativePose(target,p0-Pose2d(0,0,M_PI/2)).theta();  ////使用减法，保证角度在【-pi pi】
-          yawdiff[2]=Pose2d::relativePose(target,p0-Pose2d(0,0,M_PI)).theta();
-          yawdiff[3]=Pose2d::relativePose(target,p0-Pose2d(0,0,3*M_PI/2)).theta();
-          for(int i=1;i<4;++i){
-            if(fabs(yawdiff[i])<fabs(minYawDiff)){
-              minYawDiff=yawdiff[i];
-            }
-          }
-
-        }
-        if (fabs(minYawDiff) > thresh.theta()) {
-
-          double theta = limit_gause(minYawDiff,limit_rotate.min,limit_rotate.max);
-          double angular=next_speed(timedA_.Get(),theta,acc_angular,dt);
-          double limit_angular=limit(angular,limit_rotate.min,limit_rotate.max);
-
-          SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, limit_angular);
-          actionType_=eRotation;
-          printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f anyDitect:%d\n",
-                 timedA_.Get(),angular,limit_angular,minYawDiff,anyDitect);
-          continue;
-        }
-      }
-      if (action == 2) {
-        if (Pose2d::abs(diff).y() > thresh.y()) {
-          double hv=limit_gause(diff.y(),limit_h.min,limit_h.max);
-          double velocity=limit(next_speed(timedV_.Get(),hv,acc_v,dt),limit_h.min,limit_h.max);
-          SendMoveCmd(move_mode_,Monitor_emqx::eHorizontal,velocity , 0);
-          actionType_=eHorizon;
-          printf(" Horizontal input:%f out:%f\n",timedV_.Get(),velocity);
-          continue;
-        }
-      }
-      if (action == 3) {
-        if (Pose2d::abs(diff).x() > thresh.x()) {
-          double v=limit_gause(diff.x(),limit_v.min,limit_v.max);
-          double velocity=limit(next_speed(timedV_.Get(),v,acc_v,dt),limit_v.min,limit_v.max);
-          SendMoveCmd(move_mode_,Monitor_emqx::eMPC, velocity, 0);
-          actionType_=eVertical;
-          printf(" Vrtical input:%f,out:%f diff.x:%f\n",timedV_.Get(),velocity,diff.x());
-          continue;
-        }
-      }
-      if(Stop()) {
-        printf(" monitor type :%d completed!!! reset action type!!!\n", action);
-        action = 0;
-      }
-    }
-  }
-  return false;
-
-}
-
 bool Navigation::RotateReferToTarget(const Pose2d& target,stLimit limit_rotate,bool anyDirect) {
 
   while(cancel_==false) {
@@ -586,24 +471,9 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
     bool anyDirect=(action.type()==3);
 
     //速度限制
-    stLimit adjust_x={ action.adjustvelocitylimit().min(),action.adjustvelocitylimit().max() };
-    stLimit adjust_h={ action.adjusthorizonlimit().min(),action.adjusthorizonlimit().max() };
     stLimit adjust_angular={ action.nodeangularlimit().min(),action.nodeangularlimit().max() };
     stLimit mpc_velocity={ action.nodevelocitylimit().min(),action.nodevelocitylimit().max() };
 
-    /*原地调整起点
-    NavMessage::PathNode first=action.pathnodes(0);
-
-    //目标，精度
-    double first_yaw=timedPose_.Get().theta();
-    if(1<action.pathnodes_size()){
-      NavMessage::PathNode next=action.pathnodes(1);
-      first_yaw=Pose2d::vector2yaw(next.pose().x()-first.pose().x(),next.pose().y()-first.pose().y());
-    }
-    Pose2d first_target(first.pose().x(),first.pose().y(),first_yaw);
-    Pose2d first_accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
-    execute_adjust_action(first_target,first_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
-    Pose2d last_node=first_target;*/
     for(int i=0;i< action.pathnodes_size();++i) {
       NavMessage::PathNode node = action.pathnodes(i);
       /// 巡线到目标点，判断巡线方向
@@ -641,40 +511,8 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
   return false;
 }
 
-bool Navigation::execute_vehicle_mode(NavMessage::NewAction action){
-  if(action.type()==5 ) //最优动作导航 or 导航中保证朝向严格一致
-  {
-    if (!action.has_nodevelocitylimit() || !action.has_nodeangularlimit() ||
-        !action.has_adjustvelocitylimit() || !action.has_adjusthorizonlimit()
-        || action.pathnodes_size() == 0) {
-      std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl;
-      return false;
-
-      //原地调整起点
-      NavMessage::PathNode first=action.pathnodes(0);
-      //速度限制
-      stLimit adjust_x={ action.adjustvelocitylimit().min(),action.adjustvelocitylimit().max() };
-      stLimit adjust_h={ action.adjusthorizonlimit().min(),action.adjusthorizonlimit().max() };
-      stLimit adjust_angular={ action.nodeangularlimit().min(),action.nodeangularlimit().max() };
-      stLimit mpc_velocity={ action.nodevelocitylimit().min(),action.nodevelocitylimit().max() };
-      //目标，精度
-      double first_yaw=timedPose_.Get().theta();
-      if(1<action.pathnodes_size()){
-        NavMessage::PathNode next=action.pathnodes(1);
-        first_yaw=Pose2d::vector2yaw(next.pose().x()-first.pose().x(),next.pose().y()-first.pose().y());
-      }
-      Pose2d first_target(first.pose().x(),first.pose().y(),first_yaw);
-      Pose2d first_accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
-      //execute_adjust_action(first_target,first_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
-      return false;
-    }
-  }else{
-    return false;
-  }
-}
-
 bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
-  if (action.type() != 1 && action.type() != 2) {
+  /*if (action.type() != 1 && action.type() != 2) {
     printf(" Inout_space failed : msg action type must ==1\n ");
     return false;
   }
@@ -739,7 +577,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
       return false;
 
   if (execute_along_action(begin, target, target_accuracy, mpc_velocity, true) == false)
-      return false;
+      return false;*/
   return true;
 }
 
@@ -819,7 +657,7 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
                            const Pose2d& target,const Pose2d& diff,int direct)
 {
   //std::cout<<cur<<","<<target<<","<<nearest_yaw<<",v"<<velocity<<"a:"<<angular<<"diff:"<<diff<<std::endl;
-  Pose2d distance=Pose2d::relativePose(target,cur);
+  Pose2d distance=target-cur;
   Pose2d absDiff=Pose2d::abs(distance);
 
   //std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
@@ -858,11 +696,10 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
                            const Pose2d& target,const Pose2d& diff)
 {
   //std::cout<<cur<<","<<target<<","<<nearest_yaw<<",v"<<velocity<<"a:"<<angular<<"diff:"<<diff<<std::endl;
-  Pose2d distance=Pose2d::relativePose(target,cur);
+  Pose2d distance=target-cur;
   Pose2d absDiff=Pose2d::abs(distance);
 
   //std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
-
     if(absDiff.x()<diff.x() && absDiff.y()<diff.y())
     {
       if(fabs(velocity) < 0.06 && fabs(angular) < 5 * M_PI / 180.0)
@@ -964,10 +801,8 @@ Navigation::MpcResult Navigation::execute_along_action(const Pose2d& begin,const
       return eMpcFailed;
     }
     //判断是否到达终点
-    Pose2d target_accuracy=target_diff;
-    if(directY)
-      target_accuracy=Pose2d::abs(target_diff.rotate(timedPose_.Get().theta()));
-    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, Pose2d::abs(target_accuracy),2)) {
+    Pose2d target_accuracy=Pose2d::abs(target_diff);
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, Pose2d::abs(target_accuracy))) {
       if (Stop()) {
         printf(" exec along completed !!!\n");
         return eMpcSuccess;
@@ -1007,21 +842,11 @@ Navigation::MpcResult Navigation::execute_along_action(const Pose2d& begin,const
      * 判断车辆是否在终点附近徘徊，无法到达终点
      */
     if (mpc_possible_to_end(target, target_accuracy,directY) == false) {
-      printf(" --------------MPC  imposible to target -----------------------\n");
+      printf(" --------------MPC  imposible to target -------------------------------------------\n");
       if(fabs(timedV_.Get())>0.1 || fabs(timedA_.Get())>5*M_PI/180.0)
         Stop();
       return eImpossibleToTarget;
-      //调整到目标点
-      /*stLimit limitv, limitR;
-      limitv.min = 0.05;
-      limitv.max = 0.1;
-      limitR.min = 2 * M_PI / 180.0;
-      limitR.max = 10 * M_PI / 180.0;
-      Pose2d accuracy(target_diff.x(),target_diff.y(),M_PI*2);
-      if (execute_adjust_action(target, accuracy, limitv, limitv, limitR,false))
-        return true;
-      else
-        return false;*/
+
     }
 
     //std::this_thread::sleep_for(std::chrono::milliseconds (400));
@@ -1030,32 +855,26 @@ Navigation::MpcResult Navigation::execute_along_action(const Pose2d& begin,const
   return eMpcFailed;
 }
 
-bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_diff,bool directY)
-{
-
-  if(timedPose_.timeout()==false) {
+bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_diff,bool directY) {
+  if (timedPose_.timeout() == false) {
 
     Pose2d current = timedPose_.Get();
-    Pose2d targetInPose = Pose2d::relativePose(target, current);
-    //std::cout<<" target in pose:"<<targetInPose<<", target diff:"<<target_diff<<std::endl;
     if (directY) {
-      targetInPose=targetInPose.rotate(M_PI / 2.0);
+      current = current.rotate(current.x(), current.y(), M_PI / 2.0);
     }
-    if (fabs(targetInPose.x()) < target_diff.x()) {
-      if (fabs(targetInPose.y()) > target_diff.y())
-        return false;
-      else
-        return true;
-    } else {
-      float y=fabs(targetInPose.y())-fabs(target_diff.y());
-      if(y<0) y=0;
-      Pose2d minTarget(targetInPose.x(),y,0);
-      float cuv=compute_cuv(minTarget);
-      if(cuv>15*M_PI/180.0)
-        return false;
-      return true;
+    Pose2d targetInPose = Pose2d::relativePose(target, current);
+    Pose2d accuracy = Pose2d::abs(target_diff.rotate(current.theta()));
+
+    float y = fabs(targetInPose.y()) - fabs(accuracy.y());
+    if (y < 0) y = 0;
+    Pose2d minTarget(fabs(targetInPose.x()), y, 0);
+    float cuv = compute_cuv(minTarget);
+    if (cuv > 15 * M_PI / 180.0) {
+      std::cout<<"targetInPose:"<<targetInPose<<", target diff:"<<target_diff<<", accuracy:"<<accuracy<<std::endl;
+      return false;
     }
-  }else{
+    return true;
+  } else {
     return false;
   }
 

MPC/navigation.h

@@ -99,11 +99,6 @@ public:
      */
     MpcResult execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,
                               stLimit limit_v,bool autoDirect);
-    /*
-     * 调整到目标点，nearest_yaw:xy轴线任一朝向对准目标朝向即可
-     */
-    bool execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,stLimit limit_v,
-                            stLimit limit_h,stLimit limit_rotate,bool anyDirect);
     /*
      * 旋转摆正，朝向目标点,
      * anyDirect: 任意一个朝向对准目标点
@@ -113,10 +108,6 @@ public:
      * 按照路径点导航
      */
     bool execute_nodes(NavMessage::NewAction action);
-    /*
-     * 按照汽车模型横移导航
-     */
-    bool execute_vehicle_mode(NavMessage::NewAction action);
     /*
      * 进库
      */

MPC/pose2d.cpp

@@ -12,8 +12,18 @@ Pose2d::Pose2d()
 {
 }
 Pose2d::Pose2d(float x,float y,float theta)
-        :m_x(x),m_y(y),m_theta(theta)
+        :m_x(x),m_y(y)
 {
+  m_theta=theta;
+  int n=int(m_theta/(2*M_PI));
+  if(m_theta<=-M_PI)
+  {
+    m_theta += 2*M_PI*(n+1);
+  }
+  if(m_theta>M_PI)
+  {
+    m_theta -= 2*M_PI*(n+1);
+  }
 }
 
 Pose2d::~Pose2d()
@@ -57,9 +67,9 @@ Pose2d Pose2d::relativePose(const Pose2d& target_pose,const Pose2d& axis_pose)
   Pose2d nPose=diff.rotate(-axis_pose.theta());
 
   float new_theta=diff.theta();
-  //转换到 [-pi, pi]下
+  //转换到 (-pi, pi]下
   int n=int(new_theta/(2*M_PI));
-  if(new_theta<-M_PI)
+  if(new_theta<=-M_PI)
   {
     new_theta += 2*M_PI*(n+1);
   }

MPC/pose2d.h

@@ -43,15 +43,11 @@ class Pose2d
       float y=m_y-pose.y();
       float theta=m_theta-pose.theta();
 
-      const double PI=3.14159265;
-      if(theta<0)
-        theta=theta+2*PI;
+      if(theta<=-M_PI)
+        theta+=2*M_PI;
 
-      if(theta>=2*PI)
-        theta-=2*PI;
-
-      if(theta>PI)
-        theta=theta-2*PI;
+      if(theta>M_PI)
+        theta-=2*M_PI;
 
       return Pose2d(x,y,theta);
     }
@@ -60,16 +56,13 @@ class Pose2d
       float x=m_x+pose.x();
       float y=m_y+pose.y();
       float theta=m_theta+pose.theta();
-      //转换到 [-pi/2, pi/2]下
-      int n=int(theta/(M_PI));
-      if(theta<-M_PI/2)
-      {
-        theta += M_PI*(n+1);
-      }
-      if(theta>M_PI/2)
-      {
-        theta -= M_PI*(n+1);
-      }
+      //转换到 (-PI,PI),下
+      if(theta<=-M_PI)
+        theta+=2*M_PI;
+
+      if(theta>M_PI)
+        theta-=2*M_PI;
+
       return Pose2d(x,y,theta);
     }
     static Pose2d abs(const Pose2d& pose)

config/webots/navigation.prototxt

@@ -4,7 +4,7 @@ main_agv:false
 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"
@@ -25,7 +25,7 @@ Terminal_emqx
 brother_emqx
 {
 	NodeId:"agv-brother-child"
-	ip:"127.0.0.1"
+	ip:"192.168.0.100"
 	port:1883
 	subBrotherStatuTopic:"lidar_odom/main"
 }

config/webots/navigation_main.prototxt

@@ -3,7 +3,7 @@ main_agv:true
 Agv_emqx
 {
 	NodeId:"agv-main"
-	ip:"127.0.0.1"
+	ip:"192.168.0.100"
 	port:1883
 	pubSpeedTopic:"MainPLC/speedcmd"
 	subPoseTopic:"lidar_odom/main"
@@ -13,7 +13,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:"lidar_odom/child"
 }

projects/controllers/AGV_controller/AGV_controller.cpp

@@ -140,7 +140,7 @@ void Horizontal(double velocity,double wmg,double w,double l,double& R1,double&
 
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("127.0.0.1",1883,"webots_main","MainPLC/speedcmd"))
+  if(false==init_mqtt("192.168.0.100",1883,"webots_main","MainPLC/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;

projects/controllers/agv_child/AGV_controller.cpp

@@ -139,7 +139,7 @@ void Horizontal(double velocity,double wmg,double w,double l,double& R1,double&
 
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("127.0.0.1",1883,"webots-child","ChildPLC/speedcmd"))
+  if(false==init_mqtt("192.168.0.100",1883,"webots-child","ChildPLC/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;