增加后车配置

CMakeLists.txt

@@ -23,7 +23,6 @@ project(navigation)
 find_package (Eigen3 REQUIRED CONFIG QUIET)
 #find_package(PCL REQUIRED)
 FIND_PACKAGE(Protobuf REQUIRED)
-FIND_PACKAGE(Glog REQUIRED)
 
 #---------------------------------------------------------------------------------------
 # Set default build to release
@@ -42,6 +41,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 
 include_directories(
 		define
+		/usr/include
 		/usr/local/include
 		${EIGEN3_INCLUDE_DIR}
 		)

MPC/navigation.cpp

@@ -199,18 +199,21 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
 NavMessage::RobotStatu Navigation::CreateRobotStatuMsg()
 {
   NavMessage::RobotStatu robotStatu;
-  if (timedPose_.timeout() == false &&
-      (timedV_.timeout() == false && timedA_.timeout() == false)) {
+  //printf(" %d %d %d \n",timedPose_.timeout(),timedV_.timeout(),timedA_.timeout());
+  if (timedPose_.timeout() == false) {
 
     Pose2d pose = timedPose_.Get();
     robotStatu.set_x(pose.x());
     robotStatu.set_y(pose.y());
     robotStatu.set_theta(pose.theta());
-    NavMessage::AgvStatu plc;
-    plc.set_v(timedV_.Get());
-    plc.set_w(timedA_.Get());
-    plc.set_clamp(timed_clamp_.Get());
-    robotStatu.mutable_agvstatu()->CopyFrom(plc);
+    if((timedV_.timeout() == false && timedA_.timeout() == false)) {
+      NavMessage::AgvStatu plc;
+      plc.set_v(timedV_.Get());
+      plc.set_w(timedA_.Get());
+      plc.set_clamp(timed_clamp_.Get());
+      //printf(" timed_clamp_:%d\n ",timed_clamp_.Get());
+      robotStatu.mutable_agvstatu()->CopyFrom(plc);
+    }
   }
   return robotStatu;
 }
@@ -262,10 +265,33 @@ void Navigation::Cancel()
   cancel_=true;
 
 }
+
+bool Navigation::Stop() {
+  if(monitor_)
+  {
+    monitor_->stop();
+    while(cancel_==false)
+    {
+      if(timedV_.timeout()==false && timedA_.timeout()==false)
+      {
+        if(fabs(timedV_.Get())<1e-6 && fabs(timedA_.Get())<1e-6)
+          return true;
+        else
+          monitor_->stop();
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      }else{
+        printf("Stop failed V/A timeout\n");
+        return false;
+      }
+    }
+  }
+  return false;
+}
+
 void Navigation::Pause()
 {
-  monitor_->stop();
-  pause_=true;
+  if(Stop())
+    pause_=true;
 }
 
 Navigation::Navigation()
@@ -298,17 +324,17 @@ void Navigation::HandleNavCmd(const NavMessage::NavCmd& cmd)
 {
   if(cmd.action()==3)
   {
-    printf(" Nav cancel");
+    printf(" Nav cancel\n");
     Cancel();
     return ;
   }
   if(cmd.action()==2) {
-    printf(" Nav continue");
+    printf(" Nav continue\n");
     pause_=false;
     return ;
   }
   if(cmd.action()==1) {
-    printf(" Nav pause");
+    printf(" Nav pause\n");
     Pause();
     return ;
   }
@@ -357,9 +383,11 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
     }
     //判断是否到达目标点
     if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff)) {
-      monitor_->stop();
-      printf("adjust completed\n");
-      return true;
+      if(Stop()) {
+        printf("adjust completed\n");
+        return true;
+      }
+      return false;
     }
     
     //计算目标点在当前pose下的pose
@@ -412,9 +440,10 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
           continue;
         }
       }
-      monitor_->stop();
-      printf(" monitor type :%d completed!!! reset action type!!!\n", action);
-      action = 0;
+      if(Stop()) {
+        printf(" monitor type :%d completed!!! reset action type!!!\n", action);
+        action = 0;
+      }
     }
   }
   return false;
@@ -480,7 +509,7 @@ bool Navigation::clamp_close()
 {
   if(monitor_) {
     monitor_->clamp_close(move_mode_);
-    while (exit_ == false) {
+    while (cancel_ == false) {
       if (timed_clamp_.timeout()) {
         printf("timed clamp is timeout\n");
         return false;
@@ -497,7 +526,7 @@ bool Navigation::clamp_close()
 bool Navigation::clamp_open() {
   if(monitor_) {
     monitor_->clamp_open(move_mode_);
-    while(exit_==false)
+    while(cancel_==false)
     {
       if(timed_clamp_.timeout())
       {
@@ -544,9 +573,10 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     //判断是否到达终点
     if(IsArrived(timedPose_.Get(),timedV_.Get(),timedA_.Get(),target,target_diff))
     {
-        monitor_->stop();
-        printf(" exec along completed !!!\n");
-        return true;
+        if(Stop()) {
+          printf(" exec along completed !!!\n");
+          return true;
+        }
     }
       //一次变速
       std::vector<double> out;
@@ -556,9 +586,28 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       },"mpc_once");
       if(ret==false)
       {
-        monitor_->stop();
+        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));
     }
@@ -567,6 +616,35 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
   return false;
 }
 
+bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_diff)
+{
+  if(timedPose_.timeout()==false) {
+    Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
+    if (fabs(diff.y()) < target_diff.y() && fabs(diff.x()) < target_diff.x()) { //xy在精度内
+      if (timedV_.timeout() == true)
+        return false;
+      if(fabs(timedV_.Get())<0.05 && fabs(diff.theta())>target_diff.theta())
+        return false;
+      return true;
+    } else {  //xy不在精度内
+      if (fabs(diff.x()) < 1e-3)
+        return false; //x已到达，Y未到达，则不可能到达目标点
+      double theta = fabs(atan(diff.y() / diff.x()));
+      if (theta > 5 * M_PI / 180.0) {
+        if (timedV_.timeout() == true)
+          return false;
+        if (fabs(timedV_.Get()) < 0.05)
+          return false;
+        else
+          return true;
+      } else {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
 void Navigation::navigatting()
 {
   if(inited_==false) {
@@ -639,7 +717,7 @@ void Navigation::navigatting()
     unfinished_cations_.pop();
   }
 
-  monitor_->stop();
+  Stop();
   if(cancel_==true) {
     printf(" navigation canceled\n");
     while(unfinished_cations_.empty()==false)

MPC/navigation.h

@@ -53,6 +53,7 @@ public:
     virtual bool Start(const NavMessage::NavCmd& cmd);
     void Cancel();
     void Pause();
+    bool Stop();
 
 
  protected:
@@ -74,6 +75,7 @@ public:
     virtual bool clamp_close();
     virtual bool clamp_open();
     bool mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<double>& out);
+    bool mpc_possible_to_end(const Pose2d& target,const Pose2d& diff);
     void navigatting();
 
     virtual NavMessage::RobotStatu CreateRobotStatuMsg();

config/navigation.prototxt

@@ -1,20 +1,23 @@
+
+
+main_agv:false
 Agv_emqx
 {
-	NodeId:"agv"
-	ip:"127.0.0.1"
+	NodeId:"agv-main"
+	ip:"192.168.0.71"
 	port:1883
-	pubSpeedTopic:"monitor/speedcmd"
-	subPoseTopic:"monitor/statu"
-	subSpeedTopic:"monitor/speed"
+	pubSpeedTopic:"monitor_child/speedcmd"
+	subPoseTopic:"monitor_child/statu"
+	subSpeedTopic:"monitor_child/speed"
 }
 
 Terminal_emqx
 {
-	NodeId:"agv1-nav"
-	ip:"127.0.0.1"
-	port:1883
-	pubStatuTopic:"agv1/agv_statu"
-	pubNavStatuTopic:"agv1/nav_statu"
-	subNavCmdTopic:"agv1/nav_cmd"
-	subBrotherStatuTopic:"agv1_child/agv_statu"
+	NodeId:"agv1-child-nav"
+        ip:"192.168.0.71"
+        port:1883
+        pubStatuTopic:"agv_child/agv_statu"
+        pubNavStatuTopic:"agv_child/nav_statu"
+        subNavCmdTopic:"agv_child/nav_cmd"
+        subBrotherStatuTopic:"agv_main/agv_statu"
 }

config/navigation_main.prototxt

@@ -12,7 +12,7 @@ Agv_emqx
 
 Terminal_emqx
 {
-	NodeId:"agv1-child-nav"
+	NodeId:"agv1-main-nav"
 	ip:"192.168.0.70"
 	port:1883
 	pubStatuTopic:"agv_main/agv_statu"