增加前后车碰撞等待

MPC/loaded_mpc.cpp

@@ -250,7 +250,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     constraints_upperbound[i] = max_acc_dlt;
   }
   // 与障碍物保持距离
-  double dobs=2.2;
+  double dobs=1.6;
   for(int i=nobs;i<nobs+N;++i)
   {
     constraints_lowerbound[i] = dobs*dobs;//pow(float(nobs+N-i)/float(N)*dobs,2);

MPC/navigation.cpp

@@ -102,11 +102,13 @@ bool Navigation::Init(const Navigation_parameter& parameter)
     if (brotherEmqx_ == nullptr) {
       brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid());
 
-      if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == false) {
+      if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
+        brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback, this);
+      }else{
         printf(" brother emqx connected failed\n");
-        return false;
+       // return false;
       }
-      brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback, this);
+
     }
   }
   inited_=true;
@@ -497,6 +499,8 @@ bool Navigation::mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<dou
 
   Pose2d brother=timedBrotherPose_.Get();
   Pose2d relative=Pose2d::relativePose(brother,pose);//计算另一节在当前小车坐标系下的坐标
+  if(move_mode_==Monitor_emqx::ActionMode::eMain)
+    relative=Pose2d(-1e8,-1e8,0);
   LoadedMPC MPC(relative,limit_v.min, limit_v.max);
   Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态
 
@@ -571,26 +575,23 @@ bool Navigation::clamp_open() {
   return false;
 }
 
-bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v)
-{
-  if(inited_==false) {
+bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v) {
+  if (inited_ == false) {
     printf(" navigation has not inited\n");
     return false;
   }
-  if (timedPose_.timeout() ) {
+  if (timedPose_.timeout()) {
     printf(" navigation Error:Pose is timeout \n");
     return false;
   }
   //生成轨迹
-  Trajectory traj=Trajectory::create_line_trajectory(begin,target);
-  if(traj.size()==0)
+  Trajectory traj = Trajectory::create_line_trajectory(begin, target);
+  if (traj.size() == 0)
     return true;
 
-  while(cancel_==false)
-  {
+  while (cancel_ == false) {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
-    if(pause_==true)
-    {
+    if (pause_ == true) {
       //发送暂停消息
       continue;
     }
@@ -599,47 +600,69 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       return false;
     }
     //判断是否到达终点
-    if(IsArrived(timedPose_.Get(),timedV_.Get(),timedA_.Get(),target,target_diff))
-    {
-        if(Stop()) {
-          printf(" exec along completed !!!\n");
-          return true;
-        }
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff)) {
+      if (Stop()) {
+        printf(" exec along completed !!!\n");
+        return true;
+      }
     }
-      //一次变速
-      std::vector<double> out;
-      bool ret;
-      TimerRecord::Execute([&,this](){
-        ret=mpc_once(traj,limit_v,out);
-      },"mpc_once");
-      if(ret==false)
-      {
+    /*
+     * 判断周边是否有障碍物，半径r，当障碍物进入r内，停下等待，当障碍物远离 1.2*r，恢复
+     * */
+    double r = 2.8;
+    static bool obs_stoped = false;
+    Pose2d obs_relative = Pose2d::relativePose(timedBrotherPose_.Get(), timedPose_.Get());
+    double distance = sqrt(obs_relative.x() * obs_relative.x() + obs_relative.y() * obs_relative.y());
+    if (obs_stoped == false){
+      if (distance < r && fabs(obs_relative.x())>0.2) {
+        std::cout << "find obs stop to wait...." << std::endl;
         Stop();
-        return false;
+        obs_stoped = true;
+        continue;
       }
-      /*
-       * 判断车辆是否在终点附近徘徊，无法到达终点
-       */
-      if(mpc_possible_to_end(target,target_diff)==false)
+    } else {
+      //障碍解除
+      if(distance>1.2*r)
       {
-        printf(" --------------MPC  imposible to target -----------------------\n");
-        Stop();
-        //调整到目标点
-        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;
-        if(execute_adjust_action(target,target_diff,limitv,limitv,limitR))
-          return true;
-        else
-          return false;
+        std::cout << "obs release continue MPC" << std::endl;
+        obs_stoped = false;
+      }else{
+        continue;
       }
 
-      SendMoveCmd(move_mode_,Monitor_emqx::eMPC,out[0],out[1]);
-      //std::this_thread::sleep_for(std::chrono::milliseconds (400));
     }
 
+    //一次变速
+    std::vector<double> out;
+    bool ret;
+    TimerRecord::Execute([&, this]() {
+        ret = mpc_once(traj, limit_v, out);
+    }, "mpc_once");
+    if (ret == false) {
+      Stop();
+      return false;
+    }
+    /*
+     * 判断车辆是否在终点附近徘徊，无法到达终点
+     */
+    if (mpc_possible_to_end(target, target_diff) == false) {
+      printf(" --------------MPC  imposible to target -----------------------\n");
+      Stop();
+      //调整到目标点
+      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;
+      if (execute_adjust_action(target, target_diff, limitv, limitv, limitR))
+        return true;
+      else
+        return false;
+    }
+
+    SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
+    //std::this_thread::sleep_for(std::chrono::milliseconds (400));
+  }
 
   return false;
 }

MPC/navigation_main.cpp

@@ -23,6 +23,13 @@ void NavigationMain::ResetPose(const Pose2d& pose){
     //Pose2d transform(-wheelBase_/2.0,0,0);
     //Navigation::ResetPose(pose * transform);
     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)
+    {
+      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());
 
     Navigation::ResetPose(agv);

MPC/pose2d.h

@@ -87,6 +87,11 @@ class Pose2d
       return Pose2d(x()+rpos.x(),y()+rpos.y(),rpos.theta());
     }
 
+    bool operator<=(const Pose2d& target)const
+    {
+      return x()<=target.x() && y()<target.y() && theta()<target.theta();
+    }
+
     /*
      * 将点顺时针旋转theta
      */

config/navigation.prototxt

@@ -19,7 +19,6 @@ Terminal_emqx
         pubStatuTopic:"agv_child/agv_statu"
         pubNavStatuTopic:"agv_child/nav_statu"
         subNavCmdTopic:"agv_child/nav_cmd"
-        subBrotherStatuTopic:"agv_main/agv_statu"
 }
 
 #连接主PLC的emqx