前后车xy方向mpc参数写入配置文件

MPC/loaded_mpc.cpp

@@ -65,11 +65,11 @@ class FG_eval_half_agv {
         //朝向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> gy = CppAD::atan(m_coeffs[1] + 2*m_coeffs[2]*xt + 3*m_coeffs[3]*pow(xt,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);
+        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);
@@ -144,8 +144,8 @@ LoadedMPC::LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity){
 }
 LoadedMPC::~LoadedMPC(){}
 
-bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,float r,std::vector<double>& out,
-                                         Trajectory& select_traj,Trajectory& optimize_trajectory)
+bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,MPC_parameter mpc_param,
+                      std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory)
 {
   auto start=std::chrono::steady_clock::now();
   // State vector holds all current values neede for vars below
@@ -155,7 +155,7 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
 
   //纠正角速度/线速度，使其满足最小转弯半径
   double angular=wmg;
-  double radius=r * (1.0/sqrt(line_velocity*line_velocity+1e-10));
+  double radius=mpc_param.shortest_radius * (1.0/sqrt(line_velocity*line_velocity+1e-10));
   if( (line_velocity*line_velocity)/(wmg*wmg+1e-9) < (radius*radius)) {
     angular = fabs(line_velocity) / radius;
     if (wmg < 0) angular = -angular;
@@ -197,12 +197,12 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
   if(targetPoseInAGV.x()<0)
     ref_velocity = -ref_velocity;
 
-  double dt = 0.1;
+  double dt = mpc_param.dt;
 
   //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=1;
-  double max_acc_dlt=10.*M_PI/180.0;
+  double max_acc_line_velocity=mpc_param.acc_velocity;
+  double max_acc_dlt=mpc_param.acc_angular*M_PI/180.0;
 
   size_t n_vars = N * 5;
   Dvector vars(n_vars);

MPC/loaded_mpc.h

@@ -10,10 +10,17 @@
 
 class LoadedMPC
 {
+public:
+    typedef struct {
+        float shortest_radius;
+        float dt;
+        float acc_velocity;
+        float acc_angular;
+    }MPC_parameter;
  public:
     LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity);
     virtual ~LoadedMPC();
-    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,float r,
+    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,MPC_parameter mpc_param,
                        std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
 
  protected:

MPC/monitor/emqx/message.pb.cc

@@ -315,7 +315,6 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, action_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, key_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, wheelbase_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, newactions_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, _internal_metadata_),
@@ -348,8 +347,8 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 53, -1, sizeof(::NavMessage::Trajectory)},
   { 59, -1, sizeof(::NavMessage::NewAction)},
   { 76, -1, sizeof(::NavMessage::NavCmd)},
-  { 85, -1, sizeof(::NavMessage::NavStatu)},
-  { 96, -1, sizeof(::NavMessage::RobotStatu)},
+  { 84, -1, sizeof(::NavMessage::NavStatu)},
+  { 95, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -391,16 +390,16 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "itylimit\030\t \001(\0132\026.NavMessage.SpeedLimit\0222"
   "\n\022adjustHorizonLimit\030\n \001(\0132\026.NavMessage."
   "SpeedLimit\022\021\n\twheelbase\030\013 \001(\002\022\023\n\013changed"
-  "Mode\030\014 \001(\005\"c\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003"
-  "key\030\002 \001(\t\022\021\n\twheelbase\030\003 \001(\002\022)\n\nnewActio"
-  "ns\030\005 \003(\0132\025.NavMessage.NewAction\"\250\001\n\010NavS"
-  "tatu\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\rselecte"
-  "d_traj\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014"
-  "predict_traj\030\007 \001(\0132\026.NavMessage.Trajecto"
-  "ry\"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.NavMes"
-  "sage.AgvStatub\006proto3"
+  "Mode\030\014 \001(\005\"P\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003"
+  "key\030\002 \001(\t\022)\n\nnewActions\030\005 \003(\0132\025.NavMessa"
+  "ge.NewAction\"\250\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\rselected_traj\030\006 \001(\0132\026.NavM"
+  "essage.Trajectory\022,\n\014predict_traj\030\007 \001(\0132"
+  "\026.NavMessage.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\010ag"
+  "vStatu\030\004 \001(\0132\024.NavMessage.AgvStatub\006prot"
+  "o3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -419,7 +418,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", 1341,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 1322,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 11, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 11, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -2919,18 +2918,14 @@ NavCmd::NavCmd(const NavCmd& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
-  ::memcpy(&action_, &from.action_,
-    static_cast<size_t>(reinterpret_cast<char*>(&wheelbase_) -
-    reinterpret_cast<char*>(&action_)) + sizeof(wheelbase_));
+  action_ = from.action_;
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavCmd)
 }
 
 void NavCmd::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavCmd_message_2eproto.base);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  ::memset(&action_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&wheelbase_) -
-      reinterpret_cast<char*>(&action_)) + sizeof(wheelbase_));
+  action_ = 0;
 }
 
 NavCmd::~NavCmd() {
@@ -2967,9 +2962,7 @@ void NavCmd::Clear() {
 
   newactions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  ::memset(&action_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&wheelbase_) -
-      reinterpret_cast<char*>(&action_)) + sizeof(wheelbase_));
+  action_ = 0;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -2997,13 +2990,6 @@ const char* NavCmd::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::int
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float wheelbase = 3;
-      case 3:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
-          wheelbase_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
-        } else goto handle_unusual;
-        continue;
       // repeated .NavMessage.NewAction newActions = 5;
       case 5:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
@@ -3060,12 +3046,6 @@ failure:
         2, this->_internal_key(), target);
   }
 
-  // float wheelbase = 3;
-  if (!(this->wheelbase() <= 0 && this->wheelbase() >= 0)) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_wheelbase(), target);
-  }
-
   // repeated .NavMessage.NewAction newActions = 5;
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->_internal_newactions_size()); i < n; i++) {
@@ -3111,11 +3091,6 @@ size_t NavCmd::ByteSizeLong() const {
         this->_internal_action());
   }
 
-  // float wheelbase = 3;
-  if (!(this->wheelbase() <= 0 && this->wheelbase() >= 0)) {
-    total_size += 1 + 4;
-  }
-
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -3154,9 +3129,6 @@ void NavCmd::MergeFrom(const NavCmd& from) {
   if (from.action() != 0) {
     _internal_set_action(from._internal_action());
   }
-  if (!(from.wheelbase() <= 0 && from.wheelbase() >= 0)) {
-    _internal_set_wheelbase(from._internal_wheelbase());
-  }
 }
 
 void NavCmd::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -3182,12 +3154,7 @@ void NavCmd::InternalSwap(NavCmd* other) {
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   newactions_.InternalSwap(&other->newactions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(NavCmd, wheelbase_)
-      + sizeof(NavCmd::wheelbase_)
-      - PROTOBUF_FIELD_OFFSET(NavCmd, action_)>(
-          reinterpret_cast<char*>(&action_),
-          reinterpret_cast<char*>(&other->action_));
+  swap(action_, other->action_);
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavCmd::GetMetadata() const {

MPC/monitor/emqx/message.pb.h

@@ -1734,7 +1734,6 @@ class NavCmd PROTOBUF_FINAL :
     kNewActionsFieldNumber = 5,
     kKeyFieldNumber = 2,
     kActionFieldNumber = 1,
-    kWheelbaseFieldNumber = 3,
   };
   // repeated .NavMessage.NewAction newActions = 5;
   int newactions_size() const;
@@ -1779,15 +1778,6 @@ class NavCmd PROTOBUF_FINAL :
   void _internal_set_action(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // float wheelbase = 3;
-  void clear_wheelbase();
-  float wheelbase() const;
-  void set_wheelbase(float value);
-  private:
-  float _internal_wheelbase() const;
-  void _internal_set_wheelbase(float value);
-  public:
-
   // @@protoc_insertion_point(class_scope:NavMessage.NavCmd)
  private:
   class _Internal;
@@ -1798,7 +1788,6 @@ class NavCmd PROTOBUF_FINAL :
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::NewAction > newactions_;
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
   ::PROTOBUF_NAMESPACE_ID::int32 action_;
-  float wheelbase_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -3711,26 +3700,6 @@ inline void NavCmd::set_allocated_key(std::string* key) {
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavCmd.key)
 }
 
-// float wheelbase = 3;
-inline void NavCmd::clear_wheelbase() {
-  wheelbase_ = 0;
-}
-inline float NavCmd::_internal_wheelbase() const {
-  return wheelbase_;
-}
-inline float NavCmd::wheelbase() const {
-  // @@protoc_insertion_point(field_get:NavMessage.NavCmd.wheelbase)
-  return _internal_wheelbase();
-}
-inline void NavCmd::_internal_set_wheelbase(float value) {
-  
-  wheelbase_ = value;
-}
-inline void NavCmd::set_wheelbase(float value) {
-  _internal_set_wheelbase(value);
-  // @@protoc_insertion_point(field_set:NavMessage.NavCmd.wheelbase)
-}
-
 // repeated .NavMessage.NewAction newActions = 5;
 inline int NavCmd::_internal_newactions_size() const {
   return newactions_.size();

MPC/monitor/monitor_emqx.cpp

@@ -77,7 +77,7 @@ void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a,d
 }
 void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a)
 {
-  double w=fabs(a)>0.001?a:0.0;
+  double w=fabs(a)>0.00001?a:0.0;
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;

MPC/navigation.cpp

@@ -67,10 +67,10 @@ double next_speed(double speed,double target_speed,double acc,double dt=0.1)
 
 
 
-bool Navigation::Init(const Navigation_parameter& parameter)
+bool Navigation::Init(const NavParameter::Navigation_parameter& parameter)
 {
   parameter_=parameter;
-  AgvEmqx_parameter agv_p=parameter.agv_emqx();
+  NavParameter::AgvEmqx_parameter agv_p=parameter.agv_emqx();
   if(monitor_== nullptr) {
     monitor_ = new Monitor_emqx(agv_p.nodeid());
     if(monitor_->Connect(agv_p.ip(),agv_p.port())==false)
@@ -83,7 +83,7 @@ bool Navigation::Init(const Navigation_parameter& parameter)
     monitor_->AddCallback(agv_p.subspeedtopic(),Navigation::RobotSpeedCallback,this);
   }
 
-  Emqx_parameter terminal_p=parameter.terminal_emqx();
+  NavParameter::Emqx_parameter terminal_p=parameter.terminal_emqx();
   if(terminator_== nullptr) {
     terminator_ = new Terminator_emqx(terminal_p.nodeid());
 
@@ -98,7 +98,7 @@ bool Navigation::Init(const Navigation_parameter& parameter)
   }
 
   if(parameter.has_brother_emqx()) {
-    BrotherEmqx brotherEmqx = parameter.brother_emqx();
+    NavParameter::BrotherEmqx brotherEmqx = parameter.brother_emqx();
     if (brotherEmqx_ == nullptr) {
       brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid());
 
@@ -557,71 +557,103 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
   return false;
 }
 
-bool Navigation::execute_InOut_space(NavMessage::NewAction action)
-{
-  if(action.type()!=1 && action.type()!=2){
+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) {
     printf(" Inout_space failed : msg action type must ==1\n ");
     return false;
   }
-  if(!action.has_inoutvlimit() || !action.has_spacenode()||
-     !action.has_streetnode()) {
+  if (!action.has_inoutvlimit() || !action.has_spacenode() ||
+      !action.has_streetnode()) {
     std::cout << "execute enter_space failed ,Action miss some infos:" << action.DebugString() << std::endl;
     return false;
   }
-  if(timedPose_.timeout()==true){
-    printf(" inout_space failed type:%d : current pose is timeout\n",action.type());
+  if (timedPose_.timeout() == true) {
+    printf(" inout_space failed type:%d : current pose is timeout\n", action.type());
     return false;
   }
-  Pose2d begin,target,begin_accuracy,target_accuracy;
-  if(action.type()==1)  //入库,起点为streetNode
-  {
-    begin = Pose2d(action.streetnode().pose().x(), action.streetnode().pose().y(), action.streetnode().pose().theta());
-    begin_accuracy=Pose2d(action.streetnode().accuracy().x(),
-                                 action.streetnode().accuracy().y(),
-                                 action.streetnode().accuracy().theta());
-    target = Pose2d(action.spacenode().pose().x(), action.spacenode().pose().y(), action.spacenode().pose().theta());
-    //目标点精度
-    target_accuracy=Pose2d(action.spacenode().accuracy().x(),
-                                  action.spacenode().accuracy().y(),
-                                  action.spacenode().accuracy().theta());
-  }else{  //出库起点终点对调
-    Pose2d mid=target;
-    Pose2d mid_accuracy=target_accuracy;
-    target=begin;
-    target_accuracy=begin_accuracy;
-    begin=mid;
-    begin_accuracy=mid_accuracy;
+  Pose2d begin, target, begin_accuracy, target_accuracy;
+  //入库,起点为streetNode
+
+  begin = Pose2d(action.streetnode().pose().x(), action.streetnode().pose().y(), action.streetnode().pose().theta());
+  begin_accuracy = Pose2d(action.streetnode().accuracy().x(),
+                          action.streetnode().accuracy().y(),
+                          action.streetnode().accuracy().theta());
+  target = Pose2d(action.spacenode().pose().x(), action.spacenode().pose().y(), action.spacenode().pose().theta());
+  //目标点精度
+  target_accuracy = Pose2d(action.spacenode().accuracy().x(),
+                           action.spacenode().accuracy().y(),
+                           action.spacenode().accuracy().theta());
+  double yaw = Pose2d::vector2yaw(target.x() - begin.x(), target.y() - begin.y());
+  if (action.type() == 2) {  //出库起点终点对调
+    Pose2d mid = target;
+    Pose2d mid_accuracy = target_accuracy;
+    target = begin;
+    target_accuracy = begin_accuracy;
+    begin = mid;
+    begin_accuracy = mid_accuracy;
+    yaw = Pose2d::vector2yaw(begin.x() - target.x(), begin.y() - target.y());
   }
   //给起点赋予朝向,保证agv以正对车位的方向行进
-  double yaw=Pose2d::vector2yaw(target.x()-begin.x(),target.y()-begin.y());
-  if(action.type()==2)  //出库
-    yaw=Pose2d::vector2yaw(begin.x()-target.x(),begin.y()-target.y());
-  begin.mutable_theta()=yaw;
+
+  begin.mutable_theta() = yaw;
 
   //严格调整到起点
-  stLimit adjust_x={ 0.05,0.2};
-  stLimit adjust_h={ 0.05,0.2};
-  stLimit adjust_angular={ 2*M_PI/180.0, 20*M_PI/180.0 };
-  stLimit mpc_velocity={0.05,0.3 };
+  stLimit adjust_x = {0.05, 0.2};
+  stLimit adjust_h = {0.05, 0.2};
+  stLimit adjust_angular = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
+  stLimit mpc_velocity = {0.05, 0.3};
 
   //判断当前点与起点的距离
-  Pose2d distance(0.5,0.5,M_PI*2);
-  if(action.type()==2) {  //出库要求起点距离当前点非常近
-    distance = Pose2d(0.05, 0.05, 1 * M_PI / 180.0);
-    adjust_x={ 0.03,0.1};
-    adjust_h={ 0.03,0.1};
-    adjust_angular={ 2*M_PI/180.0, 5*M_PI/180.0 };
-  }
-   if( !(Pose2d::abs(begin-timedPose_.Get())<distance) ){
-    printf(" Inout space failed type:%d: current pose too far from begin node\n",action.type());
+  Pose2d distance(0.5, 0.5, M_PI * 2);
+  if (action.type() == 2) {  //出库要求起点距离当前点非常近
+    distance = Pose2d(0.05, 0.05, 2 * M_PI);
+    adjust_x = {0.03, 0.1};
+    adjust_h = {0.03, 0.1};
+    adjust_angular = {2 * M_PI / 180.0, 5 * M_PI / 180.0};
+  }
+  if (!(Pose2d::abs(begin - timedPose_.Get()) < distance)) {
+    printf(" Inout space failed type:%d: current pose too far from begin node\n", action.type());
+    std::cout << " begin：" << begin << "   current:" << timedPose_.Get() << std::endl;
     return false;
   }
 
-  if(execute_adjust_action(begin,begin_accuracy,adjust_x,adjust_h,adjust_angular,false))
+  if (execute_adjust_action(begin, begin_accuracy, adjust_x, adjust_h, adjust_angular, false) == false)
     return false;
 
   if (execute_along_action(begin, target, target_accuracy, mpc_velocity, false) == false)
-      return false;
+    return false;
   return true;
 }
 
@@ -655,18 +687,22 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
 
   Trajectory optimize_trajectory;
   Trajectory selected_trajectory;
-
+  NavParameter::MPC_parameter mpc_parameter=parameter_.x_mpc_parameter();
   if (directY == true) {
     //将车身朝向旋转90°，车身朝向在(-pi,pi]
     statu[2] += M_PI / 2;
     if (statu[2] > M_PI)
       statu[2] -= 2 * M_PI;
+    mpc_parameter=parameter_.y_mpc_parameter();
   }
   bool ret = false;
   LoadedMPC MPC(relative, limit_v.min, limit_v.max);
-  float r=20.0*1.3; //最小转弯半径
-  if(directY) r=3*2.45;
-  ret = MPC.solve(traj, traj[traj.size() - 1], statu,r, out, selected_trajectory, optimize_trajectory);
+  LoadedMPC::MPC_parameter parameter={mpc_parameter.shortest_radius(),mpc_parameter.dt(),
+                                     mpc_parameter.acc_velocity(),mpc_parameter.acc_angular()};
+  //printf(" r:%f  dt:%f acc:%f  acc_a:%f\n",mpc_parameter.shortest_radius(),
+  //       mpc_parameter.dt(),mpc_parameter.acc_velocity(),mpc_parameter.acc_angular());
+  ret = MPC.solve(traj, traj[traj.size() - 1], statu,parameter,
+                  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);
@@ -681,14 +717,14 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
   Pose2d distance=Pose2d::relativePose(target,cur);
   Pose2d absDiff=Pose2d::abs(distance);
 
-  std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
+  //std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
   if(direct==1){ //纵向MPC
     if(absDiff.x()<diff.x() && absDiff.y()<diff.y())
     {
       float diffYaw=absDiff.theta();  //[0-pi)
       float diffYaw2=absDiff.theta()-M_PI;
       bool yawArrived=( fabs(diffYaw)<diff.theta() || fabs(diffYaw2)<diff.theta() );
-      if(yawArrived && fabs(velocity) < 0.03 && fabs(angular) < 5 * M_PI / 180.0)
+      if(yawArrived && fabs(velocity) < 0.05 && fabs(angular) < 5 * M_PI / 180.0)
         return true;
     }
   }else if(direct==2){    //横向MPC

MPC/navigation.h

@@ -38,12 +38,18 @@ public:
         eOpened=2
     };
 
+    enum AdjustYawMode{
+        eX=0,
+        eY,
+        eXY
+    };
+
  public:
     Navigation();
     virtual ~Navigation();
     //连接AGV emqx服务
 
-    bool Init(const Navigation_parameter& parameter);
+    bool Init(const NavParameter::Navigation_parameter& parameter);
 
     virtual void ResetPose(const Pose2d& pose);
     void ResetStatu(double v,double a);
@@ -94,6 +100,10 @@ public:
      * 按照路径点导航
      */
     bool execute_nodes(NavMessage::NewAction action);
+    /*
+     * 按照汽车模型横移导航
+     */
+    bool execute_vehicle_mode(NavMessage::NewAction action);
     /*
      * 进库
      */
@@ -144,7 +154,7 @@ public:
     TimedLockData<Trajectory> selected_traj_;
     TimedLockData<Trajectory> predict_traj_;
 
-    Navigation_parameter parameter_;
+    NavParameter::Navigation_parameter parameter_;
 };
 
 

MPC/navigation_main.cpp

@@ -51,12 +51,18 @@ void NavigationMain::HandleNavCmd(const NavMessage::NavCmd& cmd)
 {
   if(cmd.action()==4)
   {
-    if(cmd.wheelbase()<2.4 || cmd.wheelbase()>3.5)
+    if(cmd.newactions_size()==0)
     {
-      printf("Failed to Switch MoveMode --> main,wheelbase invalid :%f\n",cmd.wheelbase());
+      printf(" Error main \n");
       return;
     }
-    SwitchMode(Monitor_emqx::eMain,cmd.wheelbase());
+    NavMessage::NewAction action=cmd.newactions(0);
+    if(action.wheelbase()<2.4 || action.wheelbase()>3.5)
+    {
+      printf("Failed to Switch MoveMode --> main,wheelbase invalid :%f\n",action.wheelbase());
+      return;
+    }
+    SwitchMode(Monitor_emqx::eMain,action.wheelbase());
     return ;
   }
   if(cmd.action()==5)

MPC/trajectory.cpp

@@ -226,3 +226,57 @@ Trajectory Trajectory::create_line_trajectory(const Pose2d& start,const Pose2d&
   return trajectory;
 }
 
+
+Trajectory Trajectory::path_smooth_slid(const std::vector<Pose2d>& path, int size)
+{
+  //判断参数合理
+  if(path.size() <= 2*size+1)
+    return path;
+
+  Trajectory smoothed_path;
+  std::vector<float> smoothed_path_x;
+  std::vector<float> smoothed_path_y;
+  std::vector<float> smoothed_path_theta;
+  //处理xy坐标
+  for(auto i = 0; i < path.size(); ++i)
+  {
+    if(i >= size && i < path.size() - size)
+    {
+      float x_sum = 0.0, y_sum =0.0;
+      for(auto j = i - size; j < i + size + 1; ++j)
+      {
+        x_sum += path[j].x();
+        y_sum += path[j].y();
+      }
+      smoothed_path_x.push_back(x_sum/float(2.0*size+1));
+      smoothed_path_y.push_back(y_sum/float(2.0*size+1));
+    }
+    else
+    {
+      smoothed_path_x.push_back(path[i].x());
+      smoothed_path_y.push_back(path[i].y());
+    }
+  }
+  //处理theta角度
+  for(auto i = 0; i < path.size(); ++i)
+  {
+    if(i >= size - 1 && i < path.size() - size)
+    {
+      Pose2d point1 = Pose2d(smoothed_path_x[i],smoothed_path_y[i],0.0);
+      Pose2d point2 = Pose2d(smoothed_path_x[i+1],smoothed_path_y[i+1],0.0);
+      double angle = gen_axis_angle(point1,point2);
+      smoothed_path_theta.push_back((float)angle);
+    }
+    else
+    {
+      smoothed_path_theta.push_back(path[i].theta());
+    }
+  }
+  //构建平滑后的返回路径
+  for (int i = 0; i < path.size(); ++i)
+  {
+    smoothed_path.push_point(Pose2d(smoothed_path_x[i], smoothed_path_y[i], smoothed_path_theta[i]));
+  }
+
+  return smoothed_path;
+}

MPC/trajectory.h

@@ -24,7 +24,8 @@ class Trajectory
     void push_point(const Pose2d& point);
 
     static Trajectory create_trajectory(const Pose2d& start,const Pose2d& end,int point_count=100);
-    static Trajectory create_line_trajectory(const Pose2d& start,const Pose2d& end,float dt=0.05);
+    static Trajectory create_line_trajectory(const Pose2d& start,const Pose2d& end,float dt=0.2);
+    static Trajectory path_smooth_slid(const std::vector<Pose2d>& path, int size = 5);
 
  protected:
     std::vector<Pose2d>             m_pose_vector;

config/navigation.prototxt

@@ -29,3 +29,18 @@ brother_emqx
 	port:1883
 	subBrotherStatuTopic:"monitor_child/statu"
 }
+
+x_mpc_parameter
+{
+	shortest_radius:26
+	dt:0.1
+	acc_velocity:1
+	acc_angular:10
+}
+y_mpc_parameter
+{
+	shortest_radius:7.35
+	dt:0.1
+	acc_velocity:1
+	acc_angular:20
+}

config/navigation_main.prototxt

@@ -28,3 +28,18 @@ brother_emqx
 	port:1883
 	subBrotherStatuTopic:"monitor_child/statu"
 }
+
+x_mpc_parameter
+{
+	shortest_radius:26
+	dt:0.1
+	acc_velocity:1
+	acc_angular:10
+}
+y_mpc_parameter
+{
+	shortest_radius:24.5
+	dt:0.1
+	acc_velocity:1
+	acc_angular:10
+}

controller.cpp

@@ -17,7 +17,7 @@ int main(int argc,char* argv[])
   }
   std::string parameter_file=argv[1];
 
-  Navigation_parameter parameter;
+  NavParameter::Navigation_parameter parameter;
 
   if(proto_tool::get_instance_pointer()->read_proto_param(parameter_file,parameter)==false)
   {

parameter.proto

@@ -1,4 +1,5 @@
 syntax = "proto3";
+package NavParameter;
 message AgvEmqx_parameter
 {
   string NodeId=1;
@@ -25,11 +26,21 @@ message BrotherEmqx
   string subBrotherStatuTopic=4;
 }
 
+message MPC_parameter
+{
+  float shortest_radius=1;
+  float dt=2;
+  float acc_velocity=3;
+  float acc_angular=4;
+}
+
 message Navigation_parameter
 {
   bool main_agv=1;   //是否是两车整体
   AgvEmqx_parameter Agv_emqx=2;
   Emqx_parameter Terminal_emqx=3;
     BrotherEmqx brother_emqx=4;
+    MPC_parameter x_mpc_parameter=5;
+    MPC_parameter y_mpc_parameter=6;
 }