nav状态增加动作类型，增加单车碰撞等待条件

MPC/monitor/emqx/message.pb.cc

@@ -355,7 +355,7 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "t\"]\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t"
   "\022\021\n\twheelbase\030\003 \001(\002\022#\n\007actions\030\004 \003(\0132\022.N"
   "avMessage.Action\"\330\001\n\010NavStatu\022\r\n\005statu\030\001"
-  " \001(\010\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030\003 \001("
+  " \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030\003 \001("
   "\005\022\013\n\003key\030\004 \001(\t\022.\n\022unfinished_actions\030\005 \003"
   "(\0132\022.NavMessage.Action\022-\n\rselected_traj\030"
   "\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014predict"
@@ -2848,7 +2848,7 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
     ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
     CHK_(ptr);
     switch (tag >> 3) {
-      // bool statu = 1;
+      // int32 statu = 1;
       case 1:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 8)) {
           statu_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
@@ -2932,10 +2932,10 @@ failure:
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  // bool statu = 1;
+  // int32 statu = 1;
   if (this->statu() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(1, this->_internal_statu(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_statu(), target);
   }
 
   // bool main_agv = 2;
@@ -3028,9 +3028,11 @@ size_t NavStatu::ByteSizeLong() const {
         *predict_traj_);
   }
 
-  // bool statu = 1;
+  // int32 statu = 1;
   if (this->statu() != 0) {
-    total_size += 1 + 1;
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_statu());
   }
 
   // bool main_agv = 2;

MPC/monitor/emqx/message.pb.h

@@ -1743,13 +1743,13 @@ class NavStatu PROTOBUF_FINAL :
       ::NavMessage::Trajectory* predict_traj);
   ::NavMessage::Trajectory* unsafe_arena_release_predict_traj();
 
-  // bool statu = 1;
+  // int32 statu = 1;
   void clear_statu();
-  bool statu() const;
-  void set_statu(bool value);
+  ::PROTOBUF_NAMESPACE_ID::int32 statu() const;
+  void set_statu(::PROTOBUF_NAMESPACE_ID::int32 value);
   private:
-  bool _internal_statu() const;
-  void _internal_set_statu(bool value);
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_statu() const;
+  void _internal_set_statu(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
   // bool main_agv = 2;
@@ -1781,7 +1781,7 @@ class NavStatu PROTOBUF_FINAL :
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
   ::NavMessage::Trajectory* selected_traj_;
   ::NavMessage::Trajectory* predict_traj_;
-  bool statu_;
+  ::PROTOBUF_NAMESPACE_ID::int32 statu_;
   bool main_agv_;
   ::PROTOBUF_NAMESPACE_ID::int32 move_mode_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
@@ -3127,22 +3127,22 @@ NavCmd::actions() const {
 
 // NavStatu
 
-// bool statu = 1;
+// int32 statu = 1;
 inline void NavStatu::clear_statu() {
-  statu_ = false;
+  statu_ = 0;
 }
-inline bool NavStatu::_internal_statu() const {
+inline ::PROTOBUF_NAMESPACE_ID::int32 NavStatu::_internal_statu() const {
   return statu_;
 }
-inline bool NavStatu::statu() const {
+inline ::PROTOBUF_NAMESPACE_ID::int32 NavStatu::statu() const {
   // @@protoc_insertion_point(field_get:NavMessage.NavStatu.statu)
   return _internal_statu();
 }
-inline void NavStatu::_internal_set_statu(bool value) {
+inline void NavStatu::_internal_set_statu(::PROTOBUF_NAMESPACE_ID::int32 value) {
   
   statu_ = value;
 }
-inline void NavStatu::set_statu(bool value) {
+inline void NavStatu::set_statu(::PROTOBUF_NAMESPACE_ID::int32 value) {
   _internal_set_statu(value);
   // @@protoc_insertion_point(field_set:NavMessage.NavStatu.statu)
 }

MPC/navigation.cpp

@@ -169,12 +169,12 @@ void Navigation::pubStatuThreadFuc(void* p) {
 void Navigation::publish_statu(NavMessage::NavStatu& statu) {
 
     //发布nav状态
-    statu.set_statu(eReady); //默认ready
+    statu.set_statu(actionType_); //
+    printf(" statu:%d\n",actionType_);
     statu.set_move_mode(move_mode_);
     NavMessage::Action *current_action = nullptr;
     if (running_)
     {
-      statu.set_statu(eRunning);
       statu.set_key(global_navCmd_.key());
       std::queue<NavMessage::Action> actios = unfinished_cations_;
       while (actios.empty() == false) {
@@ -448,6 +448,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
           double limit_angular=limit(angular,limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
 
           SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, limit_angular);
+          actionType_=eRotation;
           printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f\n",timedA_.Get(),angular,limit_angular,theta);
           continue;
         }
@@ -457,6 +458,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
           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;
         }
@@ -466,6 +468,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
           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\n",timedV_.Get(),velocity);
           continue;
         }
@@ -533,14 +536,16 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
 
 void Navigation::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
                              double v,double angular){
-  if(monitor_)
-    monitor_->set_speed(mode,type,v,angular);
+  if(monitor_) {
+    monitor_->set_speed(mode, type, v, angular);
+  }
 }
 
 bool Navigation::clamp_close()
 {
   if(monitor_) {
     monitor_->clamp_close(move_mode_);
+    actionType_=eClampClose;
     while (cancel_ == false) {
       if (timed_clamp_.timeout()) {
         printf("timed clamp is timeout\n");
@@ -550,6 +555,7 @@ bool Navigation::clamp_close()
         return true;
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
       monitor_->clamp_close(move_mode_);
+      actionType_=eClampClose;
     }
     return false;
   }
@@ -558,6 +564,7 @@ bool Navigation::clamp_close()
 bool Navigation::clamp_open() {
   if(monitor_) {
     monitor_->clamp_open(move_mode_);
+    actionType_=eClampOpen;
     while(cancel_==false)
     {
       if(timed_clamp_.timeout())
@@ -569,6 +576,7 @@ bool Navigation::clamp_open() {
         return true;
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
       monitor_->clamp_open(move_mode_);
+      actionType_=eClampOpen;
     }
     return false;
   }
@@ -609,27 +617,28 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     /*
      * 判断周边是否有障碍物，半径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();
-        obs_stoped = true;
-        continue;
-      }
-    } else {
-      //障碍解除
-      if(distance>1.2*r)
-      {
-        std::cout << "obs release continue MPC" << std::endl;
-        obs_stoped = false;
-      }else{
-        continue;
-      }
+    if(move_mode_==Monitor_emqx::eSingle) {
+      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();
+          obs_stoped = true;
+          continue;
+        }
+      } else {
+        //障碍解除
+        if (distance > 1.2 * r) {
+          std::cout << "obs release continue MPC" << std::endl;
+          obs_stoped = false;
+        } else {
+          continue;
+        }
 
+      }
     }
 
     //一次变速
@@ -661,6 +670,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     }
 
     SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
+    actionType_=eMPC;
     //std::this_thread::sleep_for(std::chrono::milliseconds (400));
   }
 
@@ -705,6 +715,7 @@ void Navigation::navigatting()
   pause_=false;
   cancel_=false;
   running_=true;
+  actionType_=eReady;
   printf(" navigation beg...\n");
 
   while(unfinished_cations_.empty()==false && cancel_==false)
@@ -767,7 +778,7 @@ void Navigation::navigatting()
     }
     unfinished_cations_.pop();
   }
-
+  actionType_=eReady;
   Stop();
   if(cancel_==true) {
     printf(" navigation canceled\n");

MPC/navigation.h

@@ -13,6 +13,7 @@
 #include "parameter.pb.h"
 #include <queue>
 #include <thread>
+
 class Navigation
 {
 public:
@@ -23,7 +24,12 @@ public:
     enum ActionType
     {
         eReady=0,
-        eRunning
+        eRotation,
+        eHorizon,
+        eVertical,
+        eMPC,
+        eClampOpen,
+        eClampClose
     };
     enum ClampStatu
     {
@@ -90,6 +96,7 @@ public:
  protected:
     std::mutex  mtx_;
     bool exit_=false;
+    ActionType actionType_=eReady;
     std::thread* pubthread_= nullptr;
     bool inited_=false;
     Monitor_emqx* monitor_= nullptr;

MPC/navigation_main.cpp

@@ -53,7 +53,6 @@ void NavigationMain::HandleNavCmd(const NavMessage::NavCmd& cmd)
       printf("Failed to Switch MoveMode --> main,wheelbase invalid :%f\n",cmd.wheelbase());
       return;
     }
-    printf(" Switch MoveMode --> main\n");
     SwitchMode(Monitor_emqx::eMain,cmd.wheelbase());
     return ;
   }

MPC/navigation_main.h

@@ -20,11 +20,31 @@ public:
     void ResetOtherClamp(ClampStatu statu);
     virtual void HandleAgvStatu(const MqttMsg& msg);
     virtual void SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase){
-      move_mode_=mode;
-      if(move_mode_==Monitor_emqx::eMain)
+      if(move_mode_==mode)
+        return;
+      if(mode==Monitor_emqx::eMain)
       {
-        wheelBase_=wheelBase;
+        if(timedBrotherPose_.timeout()==true || timedPose_.timeout()==true)
+        {
+          std::cout<<"Brother/self pose is timeout can not set MainAGV pose"<<std::endl;
+          return;
+        }
+        //Pose2d transform(-wheelBase_/2.0,0,0);
+        //Navigation::ResetPose(pose * transform);
+        Pose2d brother=timedBrotherPose_.Get();
+        Pose2d pose=timedPose_.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;
+        }
+        printf(" Switch MoveMode --> main\n");
       }
+      wheelBase_=wheelBase;
+      move_mode_=mode;
+
     }
 
 

message.proto

@@ -66,7 +66,7 @@ message NavCmd
 
 message NavStatu
 {
-  bool statu = 1; //0:ready  1:running
+  int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关
   bool main_agv=2; //是否是两节控制plc
   int32 move_mode=3; //运动模式，1：single，2：双车
   string key = 4; // 任务唯一码