完成原动作兼容，横移巡线待测试

MPC/loaded_mpc.cpp

@@ -63,8 +63,13 @@ class FG_eval_half_agv {
         fg[0] += y_cost_weight * CppAD::pow(vars[ny+t]-fx, 2);
 
         //朝向loss
-        CppAD::AD<double> fth = CppAD::atan(m_coeffs[1] + 2*m_coeffs[2]*xt + 3*m_coeffs[3]*pow(xt,2));
-        fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
+        //CppAD::AD<double> fth = CppAD::atan(m_coeffs[1] + 2*m_coeffs[2]*xt + 3*m_coeffs[3]*pow(xt,2));
+        //fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
+        CppAD::AD<double> gy = CppAD::atan(m_coeffs[1] + 2*m_coeffs[2]*xt + 3*m_coeffs[3]*pow(xt,2));
+        CppAD::AD<double> cx=CppAD::cos(vars[nth + t]);
+        CppAD::AD<double> cy=CppAD::sin(vars[nth + t]);
+        CppAD::AD<double> dot=(cx+cy*gy)/CppAD::sqrt(gy*gy+1);
+        fg[0] += th_cost_weight * CppAD::pow(CppAD::acos(dot),2);
 
         //目标速度loss
         fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-ref_v,2);
@@ -196,7 +201,7 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
 
   //printf("min_v:%f   max_v:%f\n",min_velocity_,max_velocity_);
   double max_dlt=max_wmg;//5*M_PI/180.0;
-  double max_acc_line_velocity=0.5;
+  double max_acc_line_velocity=1;
   double max_acc_dlt=10.*M_PI/180.0;
 
   size_t n_vars = N * 5;
@@ -413,36 +418,3 @@ Eigen::VectorXd LoadedMPC::fit_path(const std::vector<Pose2d>& trajectory)
   auto result = Q.solve(yvals);
   return result;
 }
-
-
-MPC_Y::MPC_Y(const Pose2d& obs,double min_velocity,double max_velocity)
-  :LoadedMPC(obs,min_velocity,max_velocity) {
-}
-MPC_Y::~MPC_Y(){}
-bool MPC_Y::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
-                   std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory){
-  //将轨迹点，绕当前点旋转-pi/2 即可
-  Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
-  Trajectory newTraj;
-  for(int i=0;i<trajectory.size();++i)
-  {
-    Pose2d rotated=trajectory[i].rotate(pose_agv.x(),pose_agv.y(),-M_PI/2.0);
-    newTraj.push_point(rotated);
-  }
-  Pose2d newTaget=target.rotate(pose_agv.x(),pose_agv.y(),-M_PI/2.0);
-  if(LoadedMPC::solve(newTraj,newTaget,statu,out,select_traj,optimize_trajectory))
-  {
-    //将选中的轨迹和优化预测轨迹绕当前点旋转回来
-    for(int i=0;i<select_traj.size();++i)
-    {
-      select_traj[i]=select_traj[i].rotate(pose_agv.x(),pose_agv.y(),M_PI/2.0);
-    }
-    for(int i=0;i<optimize_trajectory.size();++i)
-    {
-      optimize_trajectory[i]=optimize_trajectory[i].rotate(pose_agv.x(),pose_agv.y(),M_PI/2.0);
-    }
-    return true;
-  }
-  return false;
-}
-

MPC/loaded_mpc.h

@@ -33,15 +33,4 @@ class LoadedMPC
 
 };
 
-class MPC_Y:public LoadedMPC
-{
-public:
-    MPC_Y(const Pose2d& obs,double min_velocity,double max_velocity);
-    virtual ~MPC_Y();
-    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
-                       std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
-
-protected:
-};
-
 #endif //LIO_LIVOX_CPP_MPC_LOADED_MPC_H_

MPC/monitor/emqx/message.pb.cc

@@ -16,6 +16,7 @@
 #include <google/protobuf/port_def.inc>
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_Action_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_AgvStatu_message_2eproto;
+extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NewAction_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_PathNode_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Pose2d_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_SpeedLimit_message_2eproto;
@@ -125,9 +126,10 @@ static void InitDefaultsscc_info_NavCmd_message_2eproto() {
   ::NavMessage::NavCmd::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_NavCmd_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
-      &scc_info_Action_message_2eproto.base,}};
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NavCmd_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
+      &scc_info_Action_message_2eproto.base,
+      &scc_info_NewAction_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_NavStatu_message_2eproto() {
   GOOGLE_PROTOBUF_VERIFY_VERSION;
@@ -340,6 +342,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::NewAction, adjustvelocitylimit_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NewAction, adjusthorizonlimit_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NewAction, wheelbase_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NewAction, changedmode_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -349,6 +352,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, key_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, wheelbase_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, actions_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, newactions_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -381,9 +385,9 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 53, -1, sizeof(::NavMessage::Trajectory)},
   { 59, -1, sizeof(::NavMessage::Action)},
   { 71, -1, sizeof(::NavMessage::NewAction)},
-  { 87, -1, sizeof(::NavMessage::NavCmd)},
-  { 96, -1, sizeof(::NavMessage::NavStatu)},
-  { 108, -1, sizeof(::NavMessage::RobotStatu)},
+  { 88, -1, sizeof(::NavMessage::NavCmd)},
+  { 98, -1, sizeof(::NavMessage::NavStatu)},
+  { 110, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -421,7 +425,7 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "it\030\005 \001(\0132\026.NavMessage.SpeedLimit\022-\n\rangu"
   "lar_limit\030\006 \001(\0132\026.NavMessage.SpeedLimit\022"
   ",\n\014horize_limit\030\007 \001(\0132\026.NavMessage.Speed"
-  "Limit\"\311\003\n\tNewAction\022\014\n\004type\030\001 \001(\005\022%\n\007beg"
+  "Limit\"\336\003\n\tNewAction\022\014\n\004type\030\001 \001(\005\022%\n\007beg"
   "Node\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010pass"
   "Node\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\ntarg"
   "etNode\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tpa"
@@ -432,17 +436,19 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "NavMessage.SpeedLimit\0223\n\023adjustVelocityl"
   "imit\030\t \001(\0132\026.NavMessage.SpeedLimit\0222\n\022ad"
   "justHorizonLimit\030\n \001(\0132\026.NavMessage.Spee"
-  "dLimit\022\021\n\twheelbase\030\013 \001(\002\"]\n\006NavCmd\022\016\n\006a"
-  "ction\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.NavMessage.Action"
-  "\"\330\001\n\010NavStatu\022\r\n\005statu\030\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.NavMessa"
-  "ge.Trajectory\022,\n\014predict_traj\030\007 \001(\0132\026.Na"
-  "vMessage.Trajectory\"Y\n\nRobotStatu\022\t\n\001x\030\001"
-  " \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022&\n\010agvSta"
-  "tu\030\004 \001(\0132\024.NavMessage.AgvStatub\006proto3"
+  "dLimit\022\021\n\twheelbase\030\013 \001(\002\022\023\n\013changedMode"
+  "\030\014 \001(\005\"\210\001\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.NavMessage.Action\022)\n\nnewActions\030\005 \003"
+  "(\0132\025.NavMessage.NewAction\"\330\001\n\010NavStatu\022\r"
+  "\n\005statu\030\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_ac"
+  "tions\030\005 \003(\0132\022.NavMessage.Action\022-\n\rselec"
+  "ted_traj\030\006 \001(\0132\026.NavMessage.Trajectory\022,"
+  "\n\014predict_traj\030\007 \001(\0132\026.NavMessage.Trajec"
+  "tory\"Y\n\nRobotStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002"
+  "\022\r\n\005theta\030\003 \001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavM"
+  "essage.AgvStatub\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -462,7 +468,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 1638,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 1703,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 12, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 12, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -2890,16 +2896,16 @@ NewAction::NewAction(const NewAction& from)
     adjusthorizonlimit_ = nullptr;
   }
   ::memcpy(&type_, &from.type_,
-    static_cast<size_t>(reinterpret_cast<char*>(&wheelbase_) -
-    reinterpret_cast<char*>(&type_)) + sizeof(wheelbase_));
+    static_cast<size_t>(reinterpret_cast<char*>(&changedmode_) -
+    reinterpret_cast<char*>(&type_)) + sizeof(changedmode_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.NewAction)
 }
 
 void NewAction::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NewAction_message_2eproto.base);
   ::memset(&begnode_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&wheelbase_) -
-      reinterpret_cast<char*>(&begnode_)) + sizeof(wheelbase_));
+      reinterpret_cast<char*>(&changedmode_) -
+      reinterpret_cast<char*>(&begnode_)) + sizeof(changedmode_));
 }
 
 NewAction::~NewAction() {
@@ -2975,8 +2981,8 @@ void NewAction::Clear() {
   }
   adjusthorizonlimit_ = nullptr;
   ::memset(&type_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&wheelbase_) -
-      reinterpret_cast<char*>(&type_)) + sizeof(wheelbase_));
+      reinterpret_cast<char*>(&changedmode_) -
+      reinterpret_cast<char*>(&type_)) + sizeof(changedmode_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -3070,6 +3076,13 @@ const char* NewAction::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
+      // int32 changedMode = 12;
+      case 12:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 96)) {
+          changedmode_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -3182,6 +3195,12 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(11, this->_internal_wheelbase(), target);
   }
 
+  // int32 changedMode = 12;
+  if (this->changedmode() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(12, this->_internal_changedmode(), target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -3273,6 +3292,13 @@ size_t NewAction::ByteSizeLong() const {
     total_size += 1 + 4;
   }
 
+  // int32 changedMode = 12;
+  if (this->changedmode() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_changedmode());
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -3335,6 +3361,9 @@ void NewAction::MergeFrom(const NewAction& from) {
   if (!(from.wheelbase() <= 0 && from.wheelbase() >= 0)) {
     _internal_set_wheelbase(from._internal_wheelbase());
   }
+  if (from.changedmode() != 0) {
+    _internal_set_changedmode(from._internal_changedmode());
+  }
 }
 
 void NewAction::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -3360,8 +3389,8 @@ void NewAction::InternalSwap(NewAction* other) {
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   pathnodes_.InternalSwap(&other->pathnodes_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(NewAction, wheelbase_)
-      + sizeof(NewAction::wheelbase_)
+      PROTOBUF_FIELD_OFFSET(NewAction, changedmode_)
+      + sizeof(NewAction::changedmode_)
       - PROTOBUF_FIELD_OFFSET(NewAction, begnode_)>(
           reinterpret_cast<char*>(&begnode_),
           reinterpret_cast<char*>(&other->begnode_));
@@ -3382,14 +3411,16 @@ class NavCmd::_Internal {
 
 NavCmd::NavCmd(::PROTOBUF_NAMESPACE_ID::Arena* arena)
   : ::PROTOBUF_NAMESPACE_ID::Message(arena),
-  actions_(arena) {
+  actions_(arena),
+  newactions_(arena) {
   SharedCtor();
   RegisterArenaDtor(arena);
   // @@protoc_insertion_point(arena_constructor:NavMessage.NavCmd)
 }
 NavCmd::NavCmd(const NavCmd& from)
   : ::PROTOBUF_NAMESPACE_ID::Message(),
-      actions_(from.actions_) {
+      actions_(from.actions_),
+      newactions_(from.newactions_) {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
   if (!from._internal_key().empty()) {
@@ -3443,6 +3474,7 @@ void NavCmd::Clear() {
   (void) cached_has_bits;
 
   actions_.Clear();
+  newactions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::memset(&action_, 0, static_cast<size_t>(
       reinterpret_cast<char*>(&wheelbase_) -
@@ -3493,6 +3525,18 @@ const char* NavCmd::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::int
           } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<34>(ptr));
         } else goto handle_unusual;
         continue;
+      // repeated .NavMessage.NewAction newActions = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
+          ptr -= 1;
+          do {
+            ptr += 1;
+            ptr = ctx->ParseMessage(_internal_add_newactions(), ptr);
+            CHK_(ptr);
+            if (!ctx->DataAvailable(ptr)) break;
+          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<42>(ptr));
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -3551,6 +3595,14 @@ failure:
       InternalWriteMessage(4, this->_internal_actions(i), target, stream);
   }
 
+  // repeated .NavMessage.NewAction newActions = 5;
+  for (unsigned int i = 0,
+      n = static_cast<unsigned int>(this->_internal_newactions_size()); i < n; i++) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(5, this->_internal_newactions(i), target, stream);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -3574,6 +3626,13 @@ size_t NavCmd::ByteSizeLong() const {
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(msg);
   }
 
+  // repeated .NavMessage.NewAction newActions = 5;
+  total_size += 1UL * this->_internal_newactions_size();
+  for (const auto& msg : this->newactions_) {
+    total_size +=
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(msg);
+  }
+
   // string key = 2;
   if (this->key().size() > 0) {
     total_size += 1 +
@@ -3625,6 +3684,7 @@ void NavCmd::MergeFrom(const NavCmd& from) {
   (void) cached_has_bits;
 
   actions_.MergeFrom(from.actions_);
+  newactions_.MergeFrom(from.newactions_);
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
@@ -3658,6 +3718,7 @@ void NavCmd::InternalSwap(NavCmd* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   actions_.InternalSwap(&other->actions_);
+  newactions_.InternalSwap(&other->newactions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
       PROTOBUF_FIELD_OFFSET(NavCmd, wheelbase_)

MPC/monitor/emqx/message.pb.h

@@ -1664,6 +1664,7 @@ class NewAction PROTOBUF_FINAL :
     kAdjustHorizonLimitFieldNumber = 10,
     kTypeFieldNumber = 1,
     kWheelbaseFieldNumber = 11,
+    kChangedModeFieldNumber = 12,
   };
   // repeated .NavMessage.PathNode pathNodes = 5;
   int pathnodes_size() const;
@@ -1845,6 +1846,15 @@ class NewAction PROTOBUF_FINAL :
   void _internal_set_wheelbase(float value);
   public:
 
+  // int32 changedMode = 12;
+  void clear_changedmode();
+  ::PROTOBUF_NAMESPACE_ID::int32 changedmode() const;
+  void set_changedmode(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_changedmode() const;
+  void _internal_set_changedmode(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
   // @@protoc_insertion_point(class_scope:NavMessage.NewAction)
  private:
   class _Internal;
@@ -1863,6 +1873,7 @@ class NewAction PROTOBUF_FINAL :
   ::NavMessage::SpeedLimit* adjusthorizonlimit_;
   ::PROTOBUF_NAMESPACE_ID::int32 type_;
   float wheelbase_;
+  ::PROTOBUF_NAMESPACE_ID::int32 changedmode_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -1982,6 +1993,7 @@ class NavCmd PROTOBUF_FINAL :
 
   enum : int {
     kActionsFieldNumber = 4,
+    kNewActionsFieldNumber = 5,
     kKeyFieldNumber = 2,
     kActionFieldNumber = 1,
     kWheelbaseFieldNumber = 3,
@@ -2004,6 +2016,24 @@ class NavCmd PROTOBUF_FINAL :
   const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::Action >&
       actions() const;
 
+  // repeated .NavMessage.NewAction newActions = 5;
+  int newactions_size() const;
+  private:
+  int _internal_newactions_size() const;
+  public:
+  void clear_newactions();
+  ::NavMessage::NewAction* mutable_newactions(int index);
+  ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::NewAction >*
+      mutable_newactions();
+  private:
+  const ::NavMessage::NewAction& _internal_newactions(int index) const;
+  ::NavMessage::NewAction* _internal_add_newactions();
+  public:
+  const ::NavMessage::NewAction& newactions(int index) const;
+  ::NavMessage::NewAction* add_newactions();
+  const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::NewAction >&
+      newactions() const;
+
   // string key = 2;
   void clear_key();
   const std::string& key() const;
@@ -2046,6 +2076,7 @@ class NavCmd PROTOBUF_FINAL :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::Action > actions_;
+  ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::NewAction > newactions_;
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
   ::PROTOBUF_NAMESPACE_ID::int32 action_;
   float wheelbase_;
@@ -4397,6 +4428,26 @@ inline void NewAction::set_wheelbase(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.NewAction.wheelbase)
 }
 
+// int32 changedMode = 12;
+inline void NewAction::clear_changedmode() {
+  changedmode_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 NewAction::_internal_changedmode() const {
+  return changedmode_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 NewAction::changedmode() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NewAction.changedMode)
+  return _internal_changedmode();
+}
+inline void NewAction::_internal_set_changedmode(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  changedmode_ = value;
+}
+inline void NewAction::set_changedmode(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_changedmode(value);
+  // @@protoc_insertion_point(field_set:NavMessage.NewAction.changedMode)
+}
+
 // -------------------------------------------------------------------
 
 // NavCmd
@@ -4542,6 +4593,45 @@ NavCmd::actions() const {
   return actions_;
 }
 
+// repeated .NavMessage.NewAction newActions = 5;
+inline int NavCmd::_internal_newactions_size() const {
+  return newactions_.size();
+}
+inline int NavCmd::newactions_size() const {
+  return _internal_newactions_size();
+}
+inline void NavCmd::clear_newactions() {
+  newactions_.Clear();
+}
+inline ::NavMessage::NewAction* NavCmd::mutable_newactions(int index) {
+  // @@protoc_insertion_point(field_mutable:NavMessage.NavCmd.newActions)
+  return newactions_.Mutable(index);
+}
+inline ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::NewAction >*
+NavCmd::mutable_newactions() {
+  // @@protoc_insertion_point(field_mutable_list:NavMessage.NavCmd.newActions)
+  return &newactions_;
+}
+inline const ::NavMessage::NewAction& NavCmd::_internal_newactions(int index) const {
+  return newactions_.Get(index);
+}
+inline const ::NavMessage::NewAction& NavCmd::newactions(int index) const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavCmd.newActions)
+  return _internal_newactions(index);
+}
+inline ::NavMessage::NewAction* NavCmd::_internal_add_newactions() {
+  return newactions_.Add();
+}
+inline ::NavMessage::NewAction* NavCmd::add_newactions() {
+  // @@protoc_insertion_point(field_add:NavMessage.NavCmd.newActions)
+  return _internal_add_newactions();
+}
+inline const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::NewAction >&
+NavCmd::newactions() const {
+  // @@protoc_insertion_point(field_list:NavMessage.NavCmd.newActions)
+  return newactions_;
+}
+
 // -------------------------------------------------------------------
 
 // NavStatu

MPC/navigation.cpp

@@ -176,7 +176,7 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
     if (running_)
     {
       statu.set_key(global_navCmd_.key());
-      std::queue<NavMessage::Action> actios = unfinished_cations_;
+      /*std::queue<NavMessage::Action> actios = unfinished_cations_;
       while (actios.empty() == false) {
         //保存当前动作
         NavMessage::Action *act = statu.add_unfinished_actions();
@@ -185,11 +185,9 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
         act->CopyFrom(actios.front());
         actios.pop();
 
-      }
+      }*/
     }
     //发布MPC信息
-    if (current_action != nullptr) {
-      delete current_action;
       //发布mpc 预选点
       if (selected_traj_.timeout() == false) {
         for (int i = 0; i < selected_traj_.Get().size(); ++i) {
@@ -199,6 +197,7 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
           pose->set_y(pt.y());
           pose->set_theta(pt.theta());
         }
+
       }
       //发布 mpc 预测点
       if (predict_traj_.timeout() == false) {
@@ -210,24 +209,26 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
           pose->set_theta(pt.theta());
         }
       }
-    }
+      //std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;
+
     MqttMsg msg;
     msg.fromProtoMessage(statu);
     if(terminator_)
       terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(),msg);
 
     //发布位姿  ------robot状态--------------------------
-    NavMessage::RobotStatu robot=CreateRobotStatuMsg();
-      if(terminator_) {
+    NavMessage::RobotStatu robot;
+    if(CreateRobotStatuMsg(robot)) {
+      if (terminator_) {
         MqttMsg msg;
         msg.fromProtoMessage(robot);
         terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg);
       }
+    }
 }
 
-NavMessage::RobotStatu Navigation::CreateRobotStatuMsg()
+bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu)
 {
-  NavMessage::RobotStatu robotStatu;
   //printf(" %d %d %d \n",timedPose_.timeout(),timedV_.timeout(),timedA_.timeout());
   if (timedPose_.timeout() == false) {
 
@@ -243,8 +244,9 @@ NavMessage::RobotStatu Navigation::CreateRobotStatuMsg()
       //printf(" timed_clamp_:%d\n ",timed_clamp_.Get());
       robotStatu.mutable_agvstatu()->CopyFrom(plc);
     }
+    return true;
   }
-  return robotStatu;
+  return false;
 }
 
 void Navigation::ResetStatu(double v,double a)
@@ -283,8 +285,14 @@ bool Navigation::Start(const NavMessage::NavCmd& cmd)
 
     //add 先检查当前点与起点的距离
     global_navCmd_=cmd;
-    for(int i=0;i<cmd.actions_size();++i)
+  if(cmd.action()==6)
+  {
+    for (int i = 0; i < cmd.newactions_size(); ++i)
+      newUnfinished_cations_.push(cmd.newactions(i));
+  }else {
+    for (int i = 0; i < cmd.actions_size(); ++i)
       unfinished_cations_.push(cmd.actions(i));
+  }
     thread_=new std::thread(&Navigation::navigatting,this);
     return true;
 
@@ -367,7 +375,7 @@ void Navigation::HandleNavCmd(const NavMessage::NavCmd& cmd)
     Pause();
     return ;
   }
-  if(cmd.action()==0)
+  if(cmd.action()==0||cmd.action()==6)
   {
     Start(cmd);
   }
@@ -388,16 +396,17 @@ 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 nearest_yaw)
+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 tarhet:"<<target<<"  diff:"<<target_diff<<std::endl;
+  std::cout<<" adjust target:"<<target<<" target diff:"<<target_diff<<std::endl;
   if(inited_==false) {
     printf(" navigation has not inited\n");
     return false;
   }
 
-  Pose2d thresh=Pose2d::abs(target_diff);
+  Pose2d accuracy=target_diff;
+  Pose2d thresh=Pose2d::abs(accuracy);
 
   int action=0; //1 原地旋转，2横移，3前进
   while(cancel_==false) {
@@ -411,20 +420,20 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       break;
     }
     //判断是否到达目标点
-    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff,nearest_yaw)) {
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, accuracy,anyDitect)) {
       if(Stop()) {
-        printf("adjust completed nearest yaw:%d\n",nearest_yaw);
+        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;
@@ -444,7 +453,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       if (action == 1) {
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
           float diff_yaw=diff.theta();
-          if(nearest_yaw==true)
+          if(anyDitect==true)
           {
             int n=int(diff_yaw/(M_PI/4.0));
             if (n%2==1)
@@ -457,14 +466,14 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
               diff_yaw=diff_yaw-float(n/2)*M_PI/2.0;
             }
           }
-          double theta = limit_gause(diff_yaw,limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
+          double theta = limit_gause(diff_yaw,limit_rotate.min,limit_rotate.max);
           double angular=next_speed(timedA_.Get(),theta,acc_angular,dt);
-          double limit_angular=limit(angular,limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
+          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 NearestYaw:%d\n",
-                 timedA_.Get(),angular,limit_angular,theta,nearest_yaw);
+          printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f anyDitect:%d\n",
+                 timedA_.Get(),angular,limit_angular,diff_yaw,anyDitect);
           continue;
         }
       }
@@ -509,8 +518,9 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
       std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl;
       return false;
     }
-    bool autoDirect=(action.type()==3);
-    //原地调整
+    bool anyDirect=(action.type()==3);
+
+    //原地调整起点
     NavMessage::PathNode first=action.pathnodes(0);
     //速度限制
     stLimit adjust_x={ action.adjustvelocitylimit().min(),action.adjustvelocitylimit().max() };
@@ -518,22 +528,40 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
     stLimit adjust_angular={ action.nodeangularlimit().min(),action.nodeangularlimit().max() };
     stLimit mpc_velocity={ action.nodevelocitylimit().min(),action.nodevelocitylimit().max() };
     //目标，精度
-    Pose2d target(first.pose().x(),first.pose().y(),first.pose().theta());
-    Pose2d accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
-    execute_adjust_action(target,accuracy,adjust_x,adjust_h,adjust_angular,autoDirect);
-    NavMessage::PathNode last_node=first;
-
+    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=1;i< action.pathnodes_size();++i)
     {
-      NavMessage::PathNode node=action.pathnodes(i);
-      /// 巡线到目标点，判断巡线方向
-      Pose2d begin(last_node.pose().x(),last_node.pose().y(),last_node.pose().theta());
-      Pose2d target(node.pose().x(),node.pose().y(),node.pose().theta());
-      Pose2d accuracy(node.accuracy().x(),node.accuracy().y(),node.accuracy().theta());
+        NavMessage::PathNode node = action.pathnodes(i);
+        /// 巡线到目标点，判断巡线方向
+
+        Pose2d target(node.pose().x(), node.pose().y(), node.pose().theta());
+        Pose2d accuracy(node.accuracy().x(), node.accuracy().y(), node.accuracy().theta());
+
+        if (execute_along_action(last_node, target, accuracy, mpc_velocity, anyDirect) == false)
+          return false;
+        //不是最后一个点，则原地当前点朝向
+        if(i!=action.pathnodes_size()-1)
+        {
+          //目标，精度
+          double adjust_yaw=timedPose_.Get().theta();
+          NavMessage::PathNode next=action.pathnodes(i+1);
+          adjust_yaw=Pose2d::vector2yaw(next.pose().x()-node.pose().x(),next.pose().y()-node.pose().y());
+
+          Pose2d adjust_target(node.pose().x(),node.pose().y(),adjust_yaw);
+          Pose2d adjust_accuracy(node.accuracy().x(),node.accuracy().y(),node.accuracy().theta());
+          execute_adjust_action(adjust_target,adjust_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
+          last_node=adjust_target;
+        }
+
 
-      if(execute_along_action(begin,target,accuracy,mpc_velocity,autoDirect)==false)
-        return false;
-      last_node=node;
     }
     return true;
   }
@@ -541,55 +569,56 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
   return false;
 }
 
-bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY)
-{
-  if(timedPose_.timeout()==true && timedV_.timeout()==false && timedA_.timeout()==false)
-  {
+bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY) {
+  if (timedPose_.timeout() == true && timedV_.timeout() == false && timedA_.timeout() == false) {
     printf(" MPC once Error:Pose/V/A is timeout \n");
     return false;
   }
-  if(traj.size()==0)
-  {
+  if (traj.size() == 0) {
     printf("traj size ==0\n");
     return false;
   }
 
-  Pose2d pose=timedPose_.Get();
-  double velocity=timedV_.Get();    //从plc获取状态
-  double angular=timedA_.Get();
+  Pose2d pose = timedPose_.Get();
+  double velocity = timedV_.Get();    //从plc获取状态
+  double angular = timedA_.Get();
 
-  Pose2d brother=timedBrotherPose_.Get();
-  Pose2d relative=Pose2d::relativePose(brother,pose);//计算另一节在当前小车坐标系下的坐标
-  if(move_mode_==Monitor_emqx::ActionMode::eMain)
-    relative=Pose2d(-1e8,-1e8,0);
+  Pose2d brother = timedBrotherPose_.Get();
+  Pose2d relative = Pose2d::relativePose(brother, pose);//计算另一节在当前小车坐标系下的坐标
+  if (move_mode_ == Monitor_emqx::ActionMode::eMain)
+    relative = Pose2d(-1e8, -1e8, 0);
 
-  Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态
+  Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态
 
-  statu[0]=pose.x();
-  statu[1]=pose.y();
-  statu[2]=pose.theta();
-  statu[3]=velocity;
-  statu[4]=angular;
+  statu[0] = pose.x();
+  statu[1] = pose.y();
+  statu[2] = pose.theta();
+  statu[3] = velocity;
+  statu[4] = angular;
 
   Trajectory optimize_trajectory;
   Trajectory selected_trajectory;
 
-  bool ret=false;
-  if(directY==false) {
-    LoadedMPC MPC(relative, limit_v.min, limit_v.max);
-    ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
-  }else{
-    MPC_Y MPC(relative, limit_v.min, limit_v.max);
-    ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
+  if (directY == true) {
+    //将车身朝向旋转90°，车身朝向在(-pi,pi]
+    statu[2] += M_PI / 2;
+    if (statu[2] > M_PI)
+      statu[2] -= 2 * M_PI;
   }
-  predict_traj_.reset(optimize_trajectory,1);
-  selected_traj_.reset(selected_trajectory,1);
+  bool ret = false;
+  LoadedMPC MPC(relative, limit_v.min, limit_v.max);
+  ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
+
+  //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
+  predict_traj_.reset(optimize_trajectory, 1);
+  selected_traj_.reset(selected_trajectory, 1);
   return ret;
 }
 
 bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
                            const Pose2d& target,const Pose2d& diff,bool nearest_yaw)
 {
+  //std::cout<<cur<<","<<target<<","<<nearest_yaw<<",v"<<velocity<<"a:"<<angular<<"diff:"<<diff<<std::endl;
   Pose2d distance=Pose2d::relativePose(target,cur);
   Pose2d absDiff=Pose2d::abs(distance);
   if(nearest_yaw==false) {
@@ -688,7 +717,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     if( fabs(target_relative.y()>fabs(target_relative.x())))  //目标轨迹点在当前点Y轴上
       directY=true;
   }
-
+  printf(" exec along autoDirect:%d\n",autoDirect);
   while (cancel_ == false) {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
     if (pause_ == true) {
@@ -743,6 +772,11 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       Stop();
       return false;
     }
+    if(directY==false)
+      SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
+    else
+      SendMoveCmd(move_mode_, Monitor_emqx::eHorizontal, out[0], out[1]);
+    actionType_=eMPC;
     /*
      * 判断车辆是否在终点附近徘徊，无法到达终点
      */
@@ -755,14 +789,13 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       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))
+      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;
     }
 
-    SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
-    actionType_=eMPC;
     //std::this_thread::sleep_for(std::chrono::milliseconds (400));
   }
 
@@ -810,6 +843,52 @@ void Navigation::navigatting()
   actionType_=eReady;
   printf(" navigation beg...\n");
 
+  if(global_navCmd_.action()==6)
+  {
+    printf("exec new nav cmd ...\n");
+
+    while(newUnfinished_cations_.empty()==false && cancel_==false)
+    {
+      std::cout<<"unfinished size:"<<newUnfinished_cations_.size()<<std::endl;
+      NavMessage::NewAction act=newUnfinished_cations_.front();
+      if(act.type()==5)  {//夹持
+        if(this->clamp_close()==false){
+          printf("夹持failed ...\n");
+          break;
+        }
+      }else if(act.type()==6){
+        if(this->clamp_open()==false){
+          printf("打开夹持 failed...\n");
+          break;
+        }
+      }else if(act.type()==3 || act.type()==4){
+        if(!execute_nodes(act))
+          break;
+      }else{
+        printf(" action type 1/2 not handled !!\n");
+      }
+      newUnfinished_cations_.pop();
+    }
+    actionType_=eReady;
+    Stop();
+    if(cancel_==true) {
+      printf(" navigation canceled\n");
+      while(newUnfinished_cations_.empty()==false)
+        newUnfinished_cations_.pop();
+    }else {
+      if (newUnfinished_cations_.empty())
+        printf("navigation completed!!!\n");
+      else{
+        printf(" navigation Failed\n");
+        while(newUnfinished_cations_.empty()==false)
+          newUnfinished_cations_.pop();
+      }
+    }
+    running_=false;
+    return ;
+  }
+
+  //---------------------
   while(unfinished_cations_.empty()==false && cancel_==false)
   {
     NavMessage::Action act=unfinished_cations_.front();
@@ -827,7 +906,7 @@ void Navigation::navigatting()
       limit_angular.max=act.angular_limit().max();
       limint_horize.min=act.horize_limit().min();
       limint_horize.max=act.horize_limit().max();
-      if(execute_adjust_action(target,target_diff,limit_velocity,limint_horize,limit_angular)==false)
+      if(execute_adjust_action(target,target_diff,limit_velocity,limint_horize,limit_angular,false)==false)
       {
         printf("execute adjust action failed\n");
         break;
@@ -842,7 +921,7 @@ void Navigation::navigatting()
       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(begin,target,target_diff,limit_velocity)==false) {
+      if(execute_along_action(begin,target,target_diff,limit_velocity,false)==false) {
         printf("execute along action failed\n");
         break;
       }

MPC/navigation.h

@@ -80,12 +80,12 @@ public:
      * autoDirect:自动选择 纵向还是横向巡线
      */
     bool execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,
-                              stLimit limit_v,bool autoDirect=false);
+                              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 nearest_yaw=false);
+                            stLimit limit_h,stLimit limit_rotate,bool anyDitect);
     /*
      * 按照路径点导航
      */
@@ -96,7 +96,7 @@ public:
     bool mpc_possible_to_end(const Pose2d& target,const Pose2d& diff);
     void navigatting();
 
-    virtual NavMessage::RobotStatu CreateRobotStatuMsg();
+    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu);
 
     /*
      * 发布导航模块状态
@@ -125,6 +125,7 @@ public:
 
     NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
     std::queue<NavMessage::Action> unfinished_cations_; //未完成的动作
+    std::queue<NavMessage::NewAction> newUnfinished_cations_; //未完成的动作
     std::thread* thread_= nullptr;
     bool running_=false;
     bool pause_=false;

MPC/navigation_main.cpp

@@ -93,12 +93,15 @@ void NavigationMain::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::Act
 
 }
 
-NavMessage::RobotStatu NavigationMain::CreateRobotStatuMsg()
+bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu)
 {
-  NavMessage::RobotStatu agvStatu=Navigation::CreateRobotStatuMsg();
-  agvStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
+  NavMessage::RobotStatu agvStatu;
+  if(Navigation::CreateRobotStatuMsg(agvStatu)) {
+    agvStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
+    return true;
+  }
   //std::cout<<agvStatu.DebugString()<<std::endl;
-  return agvStatu;
+  return false;
 }
 
 void NavigationMain::ResetOtherClamp(ClampStatu statu)

MPC/navigation_main.h

@@ -54,7 +54,7 @@ protected:
     virtual void HandleNavCmd(const NavMessage::NavCmd& cmd);
     virtual bool clamp_close();
     virtual bool clamp_open();
-    virtual NavMessage::RobotStatu CreateRobotStatuMsg();
+    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu);
 protected:
     float  wheelBase_;   //两节agv的位姿与单节的位姿变换
     TimedLockData<int> timed_other_clamp_;

MPC/pose2d.h

@@ -104,6 +104,7 @@ class Pose2d
     const float x() const {return m_x;};
     const float y() const {return m_y;};
     const float theta() const {return m_theta;}
+    float& mutable_theta(){ return m_theta;}
 
     const float gridient()const;
 
@@ -113,9 +114,9 @@ class Pose2d
     static float vector2yaw(float x,float y);
 
  protected:
-    std::atomic<float>                      m_x;
-    std::atomic<float>                      m_y;
-    std::atomic<float>                      m_theta;        //梯度角，与x轴正方形逆时针为正，单位弧度
+    float                     m_x;
+    float                     m_y;
+    float                      m_theta;        //梯度角，与x轴正方形逆时针为正，单位弧度
 };
 
 

message.proto

@@ -61,9 +61,11 @@ message Action
   SpeedLimit horize_limit=7;
 }
 
+
+//----------------------------
 message NewAction //进库，出库，轨迹导航，夹持，松夹持
 {
-  int32 type =1; //1,进库，2，出库，3，马路最优动作导航，4，马路严格保证agv朝前导航，5，夹持，6，松夹持，7，切换模式
+  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，5，夹持，6，松夹持，7，切换模式
   PathNode begNode = 2; //进出库起点
   PathNode passNode=3; //进出库途径点
   PathNode targetNode = 4; //进出库终点
@@ -74,15 +76,17 @@ message NewAction //进库，出库，轨迹导航，夹持，松夹持
   SpeedLimit adjustVelocitylimit=9; //马路点原地调整x速度
   SpeedLimit adjustHorizonLimit=10;  //马路点原地横移速度限制
   float wheelbase=11;		//切换模式，轴距信息
+  int32 changedMode=12; //1:切换单车模式，2:切换双车模式
 }
-
+//-----------------------------
 
 message NavCmd
 {
-  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式
+  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,6,新导航模式
   string key=2;
   float wheelbase=3;		//轴距
   repeated Action actions=4;
+  repeated NewAction newActions=5;
 }
 
 message NavStatu