1.修复MPC旋转计算时未加入当前角速度与下一时刻角速度约束问题

2.修复下发最小角速度只有正方向问题
3.通信协议新增提升机构控制字段
4.添加提升机构控制功能

MPC/custom_type.h

@@ -98,7 +98,16 @@ enum ActionType {
     eHorizontal = 2,
     eVertical = 3,
     eClampClose = 5,
-    eClampOpen = 6
+    eClampOpen = 6,
+    eLifterRise = 7,
+    eLifterDown =8
+};
+
+enum LifterStatus {
+    eInvalid = 0,
+    eRose = 1,//上升到位
+    eDowned = 2//下降到位
 };
 //////////////////////////////////////////////////
+
 #endif //NAVIGATION_CUSTOM_TYPE_H

MPC/loaded_mpc.cpp

@@ -197,6 +197,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     //根据当前点和目标点距离,计算目标速度
 
     double ref_velocity = Pose2d::distance(pose_agv, target) / 2.5;
+//    std::cout<<"ref_velocity----"<<pose_agv<<" "<<target<<std::endl;
     if (ref_velocity < 0.1)
         ref_velocity = 0.1;
     //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0

MPC/monitor/emqx/message.pb.cc

@@ -54,6 +54,10 @@ PROTOBUF_CONSTEXPR AgvStatu::AgvStatu(
 
   , /*decltype(_impl_.clamp_other_)*/ 0
 
+  , /*decltype(_impl_.lifter_)*/ 0
+
+  , /*decltype(_impl_.lifter_other_)*/ 0
+
   , /*decltype(_impl_._cached_size_)*/{}} {}
 struct AgvStatuDefaultTypeInternal {
   PROTOBUF_CONSTEXPR AgvStatuDefaultTypeInternal() : _instance(::_pbi::ConstantInitialized{}) {}
@@ -333,6 +337,8 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.w_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.clamp_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.clamp_other_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.lifter_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.lifter_other_),
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _internal_metadata_),
     ~0u,  // no _extensions_
@@ -484,16 +490,16 @@ static const ::_pbi::MigrationSchema
     schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
         { 0, -1, -1, sizeof(::NavMessage::LidarOdomStatu)},
         { 13, -1, -1, sizeof(::NavMessage::AgvStatu)},
-        { 25, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
-        { 40, -1, -1, sizeof(::NavMessage::Pose2d)},
-        { 51, -1, -1, sizeof(::NavMessage::PathNode)},
-        { 65, -1, -1, sizeof(::NavMessage::Trajectory)},
-        { 74, 89, -1, sizeof(::NavMessage::NewAction)},
-        { 96, -1, -1, sizeof(::NavMessage::NavCmd)},
-        { 107, -1, -1, sizeof(::NavMessage::NavResponse)},
-        { 117, -1, -1, sizeof(::NavMessage::ManualCmd)},
-        { 128, 145, -1, sizeof(::NavMessage::NavStatu)},
-        { 154, 166, -1, sizeof(::NavMessage::RobotStatu)},
+        { 27, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
+        { 42, -1, -1, sizeof(::NavMessage::Pose2d)},
+        { 53, -1, -1, sizeof(::NavMessage::PathNode)},
+        { 67, -1, -1, sizeof(::NavMessage::Trajectory)},
+        { 76, 91, -1, sizeof(::NavMessage::NewAction)},
+        { 98, -1, -1, sizeof(::NavMessage::NavCmd)},
+        { 109, -1, -1, sizeof(::NavMessage::NavResponse)},
+        { 119, -1, -1, sizeof(::NavMessage::ManualCmd)},
+        { 130, 147, -1, sizeof(::NavMessage::NavStatu)},
+        { 156, 168, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -513,46 +519,47 @@ static const ::_pb::Message* const file_default_instances[] = {
 const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
     "\n\rmessage.proto\022\nNavMessage\"M\n\016LidarOdom"
     "Statu\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\t\n\001v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"D\n\010AgvStatu\022\t"
+    "(\002\022\t\n\001v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"j\n\010AgvStatu\022\t"
     "\n\001v\030\001 \001(\002\022\t\n\001w\030\002 \001(\002\022\r\n\005clamp\030\003 \001(\005\022\023\n\013c"
-    "lamp_other\030\004 \001(\005\"Y\n\010ToAgvCmd\022\t\n\001H\030\001 \001(\005\022"
-    "\t\n\001M\030\002 \001(\005\022\t\n\001T\030\003 \001(\005\022\t\n\001V\030\004 \001(\002\022\t\n\001W\030\005 "
-    "\001(\002\022\t\n\001L\030\006 \001(\002\022\013\n\003end\030\007 \001(\005\"-\n\006Pose2d\022\t\n"
-    "\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\"Q\n\010Pa"
-    "thNode\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022"
-    "\t\n\001w\030\004 \001(\002\022\r\n\005theta\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n"
-    "\nTrajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage."
-    "Pose2d\"\345\001\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tsp"
-    "aceNode\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010p"
-    "assNode\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\ns"
-    "treetNode\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n"
-    "\tpathNodes\030\005 \003(\0132\024.NavMessage.PathNode\022\021"
-    "\n\twheelbase\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P"
-    "\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n"
-    "\nnewActions\030\005 \003(\0132\025.NavMessage.NewAction"
-    "\"(\n\013NavResponse\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001"
-    "(\t\"B\n\tManualCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operatio"
-    "n_type\030\002 \001(\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavSt"
-    "atu\022\r\n\005statu\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\t"
-    "move_mode\030\003 \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessa"
-    "ge.LidarOdomStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselect"
-    "ed_traj\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n"
-    "\014predict_traj\030\007 \001(\0132\026.NavMessage.Traject"
-    "ory\022\020\n\010in_space\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\""
-    "Y\n\nRobotStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005t"
-    "heta\030\003 \001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavMessag"
-    "e.AgvStatu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.Nav"
-    "Message.NavCmd\032\027.NavMessage.NavResponse\""
-    "\000\0227\n\006Cancel\022\022.NavMessage.NavCmd\032\027.NavMes"
-    "sage.NavResponse\"\000\022C\n\017ManualOperation\022\025."
-    "NavMessage.ManualCmd\032\027.NavMessage.NavRes"
-    "ponse\"\000b\006proto3"
+    "lamp_other\030\004 \001(\005\022\016\n\006lifter\030\005 \001(\005\022\024\n\014lift"
+    "er_other\030\006 \001(\005\"Y\n\010ToAgvCmd\022\t\n\001H\030\001 \001(\005\022\t\n"
+    "\001M\030\002 \001(\005\022\t\n\001T\030\003 \001(\005\022\t\n\001V\030\004 \001(\002\022\t\n\001W\030\005 \001("
+    "\002\022\t\n\001L\030\006 \001(\002\022\013\n\003end\030\007 \001(\005\"-\n\006Pose2d\022\t\n\001x"
+    "\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\"Q\n\010Path"
+    "Node\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022\t\n"
+    "\001w\030\004 \001(\002\022\r\n\005theta\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n\nT"
+    "rajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Po"
+    "se2d\"\345\001\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tspac"
+    "eNode\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010pas"
+    "sNode\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\nstr"
+    "eetNode\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tp"
+    "athNodes\030\005 \003(\0132\024.NavMessage.PathNode\022\021\n\t"
+    "wheelbase\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P\n\006"
+    "NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nn"
+    "ewActions\030\005 \003(\0132\025.NavMessage.NewAction\"("
+    "\n\013NavResponse\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t"
+    "\"B\n\tManualCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operation_"
+    "type\030\002 \001(\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavStat"
+    "u\022\r\n\005statu\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmo"
+    "ve_mode\030\003 \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessage"
+    ".LidarOdomStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselected"
+    "_traj\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014p"
+    "redict_traj\030\007 \001(\0132\026.NavMessage.Trajector"
+    "y\022\020\n\010in_space\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\"Y\n"
+    "\nRobotStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005the"
+    "ta\030\003 \001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavMessage."
+    "AgvStatu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMe"
+    "ssage.NavCmd\032\027.NavMessage.NavResponse\"\000\022"
+    "7\n\006Cancel\022\022.NavMessage.NavCmd\032\027.NavMessa"
+    "ge.NavResponse\"\000\022C\n\017ManualOperation\022\025.Na"
+    "vMessage.ManualCmd\032\027.NavMessage.NavRespo"
+    "nse\"\000b\006proto3"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1415,
+    1453,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -957,6 +964,10 @@ inline void AgvStatu::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.clamp_other_) { 0 }
 
+    , decltype(_impl_.lifter_) { 0 }
+
+    , decltype(_impl_.lifter_other_) { 0 }
+
     , /*decltype(_impl_._cached_size_)*/{}
   };
 }
@@ -985,8 +996,8 @@ void AgvStatu::Clear() {
   (void) cached_has_bits;
 
   ::memset(&_impl_.v_, 0, static_cast<::size_t>(
-      reinterpret_cast<char*>(&_impl_.clamp_other_) -
-      reinterpret_cast<char*>(&_impl_.v_)) + sizeof(_impl_.clamp_other_));
+      reinterpret_cast<char*>(&_impl_.lifter_other_) -
+      reinterpret_cast<char*>(&_impl_.v_)) + sizeof(_impl_.lifter_other_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1032,6 +1043,24 @@ const char* AgvStatu::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
           goto handle_unusual;
         }
         continue;
+      // int32 lifter = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 40)) {
+          _impl_.lifter_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // int32 lifter_other = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 48)) {
+          _impl_.lifter_other_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
       default:
         goto handle_unusual;
     }  // switch
@@ -1097,6 +1126,20 @@ failure:
         4, this->_internal_clamp_other(), target);
   }
 
+  // int32 lifter = 5;
+  if (this->_internal_lifter() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteInt32ToArray(
+        5, this->_internal_lifter(), target);
+  }
+
+  // int32 lifter_other = 6;
+  if (this->_internal_lifter_other() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteInt32ToArray(
+        6, this->_internal_lifter_other(), target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::_pbi::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -1143,6 +1186,18 @@ failure:
         this->_internal_clamp_other());
   }
 
+  // int32 lifter = 5;
+  if (this->_internal_lifter() != 0) {
+    total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
+        this->_internal_lifter());
+  }
+
+  // int32 lifter_other = 6;
+  if (this->_internal_lifter_other() != 0) {
+    total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
+        this->_internal_lifter_other());
+  }
+
   return MaybeComputeUnknownFieldsSize(total_size, &_impl_._cached_size_);
 }
 
@@ -1181,6 +1236,12 @@ void AgvStatu::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   if (from._internal_clamp_other() != 0) {
     _this->_internal_set_clamp_other(from._internal_clamp_other());
   }
+  if (from._internal_lifter() != 0) {
+    _this->_internal_set_lifter(from._internal_lifter());
+  }
+  if (from._internal_lifter_other() != 0) {
+    _this->_internal_set_lifter_other(from._internal_lifter_other());
+  }
   _this->_internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
 }
 
@@ -1199,8 +1260,8 @@ void AgvStatu::InternalSwap(AgvStatu* other) {
   using std::swap;
   _internal_metadata_.InternalSwap(&other->_internal_metadata_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(AgvStatu, _impl_.clamp_other_)
-      + sizeof(AgvStatu::_impl_.clamp_other_)
+      PROTOBUF_FIELD_OFFSET(AgvStatu, _impl_.lifter_other_)
+      + sizeof(AgvStatu::_impl_.lifter_other_)
       - PROTOBUF_FIELD_OFFSET(AgvStatu, _impl_.v_)>(
           reinterpret_cast<char*>(&_impl_.v_),
           reinterpret_cast<char*>(&other->_impl_.v_));

MPC/monitor/emqx/message.pb.h

@@ -457,6 +457,8 @@ class AgvStatu final :
     kWFieldNumber = 2,
     kClampFieldNumber = 3,
     kClampOtherFieldNumber = 4,
+    kLifterFieldNumber = 5,
+    kLifterOtherFieldNumber = 6,
   };
   // float v = 1;
   void clear_v() ;
@@ -497,6 +499,26 @@ class AgvStatu final :
   ::int32_t _internal_clamp_other() const;
   void _internal_set_clamp_other(::int32_t value);
 
+  public:
+  // int32 lifter = 5;
+  void clear_lifter() ;
+  ::int32_t lifter() const;
+  void set_lifter(::int32_t value);
+
+  private:
+  ::int32_t _internal_lifter() const;
+  void _internal_set_lifter(::int32_t value);
+
+  public:
+  // int32 lifter_other = 6;
+  void clear_lifter_other() ;
+  ::int32_t lifter_other() const;
+  void set_lifter_other(::int32_t value);
+
+  private:
+  ::int32_t _internal_lifter_other() const;
+  void _internal_set_lifter_other(::int32_t value);
+
   public:
   // @@protoc_insertion_point(class_scope:NavMessage.AgvStatu)
  private:
@@ -510,6 +532,8 @@ class AgvStatu final :
     float w_;
     ::int32_t clamp_;
     ::int32_t clamp_other_;
+    ::int32_t lifter_;
+    ::int32_t lifter_other_;
     mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
   union { Impl_ _impl_; };
@@ -2807,6 +2831,46 @@ inline void AgvStatu::_internal_set_clamp_other(::int32_t value) {
   _impl_.clamp_other_ = value;
 }
 
+// int32 lifter = 5;
+inline void AgvStatu::clear_lifter() {
+  _impl_.lifter_ = 0;
+}
+inline ::int32_t AgvStatu::lifter() const {
+  // @@protoc_insertion_point(field_get:NavMessage.AgvStatu.lifter)
+  return _internal_lifter();
+}
+inline void AgvStatu::set_lifter(::int32_t value) {
+  _internal_set_lifter(value);
+  // @@protoc_insertion_point(field_set:NavMessage.AgvStatu.lifter)
+}
+inline ::int32_t AgvStatu::_internal_lifter() const {
+  return _impl_.lifter_;
+}
+inline void AgvStatu::_internal_set_lifter(::int32_t value) {
+  ;
+  _impl_.lifter_ = value;
+}
+
+// int32 lifter_other = 6;
+inline void AgvStatu::clear_lifter_other() {
+  _impl_.lifter_other_ = 0;
+}
+inline ::int32_t AgvStatu::lifter_other() const {
+  // @@protoc_insertion_point(field_get:NavMessage.AgvStatu.lifter_other)
+  return _internal_lifter_other();
+}
+inline void AgvStatu::set_lifter_other(::int32_t value) {
+  _internal_set_lifter_other(value);
+  // @@protoc_insertion_point(field_set:NavMessage.AgvStatu.lifter_other)
+}
+inline ::int32_t AgvStatu::_internal_lifter_other() const {
+  return _impl_.lifter_other_;
+}
+inline void AgvStatu::_internal_set_lifter_other(::int32_t value) {
+  ;
+  _impl_.lifter_other_ = value;
+}
+
 // -------------------------------------------------------------------
 
 // ToAgvCmd

MPC/monitor/monitor_emqx.cpp

@@ -52,6 +52,34 @@ void Monitor_emqx::clamp_open(int mode)
   Publish(speedcmd_topic_,msg);
 }
 
+void Monitor_emqx::lifter_rise(int mode)
+{
+    MqttMsg msg;
+    NavMessage::ToAgvCmd speed;
+    heat_=(heat_+1)%255;
+    speed.set_h(heat_);
+    speed.set_t(eLifterRise);
+
+    speed.set_m(mode);
+    speed.set_end(1);
+    msg.fromProtoMessage(speed);
+    Publish(speedcmd_topic_,msg);
+}
+
+void Monitor_emqx::lifter_down(int mode)
+{
+    MqttMsg msg;
+    NavMessage::ToAgvCmd speed;
+    heat_=(heat_+1)%255;
+    speed.set_h(heat_);
+    speed.set_t(eLifterDown);
+
+    speed.set_m(mode);
+    speed.set_end(1);
+    msg.fromProtoMessage(speed);
+    Publish(speedcmd_topic_,msg);
+}
+
 void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a,double L)
 {
   if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)

MPC/monitor/monitor_emqx.h

@@ -19,6 +19,8 @@ class Monitor_emqx : public Terminator_emqx
     void set_speed(int mode,ActionType type,double v,double a,double L);
     void clamp_close(int mode);
     void clamp_open(int mode);
+    void lifter_rise(int mode);
+    void lifter_down(int mode);
     void stop();
 
  protected:

MPC/navigation.cpp

@@ -56,15 +56,16 @@ double next_speed(double speed, double target_speed, double acc, double dt) {
 }
 
 bool Navigation::PoseTimeout() {
-    if (timedPose_.timeout() == false && timedBrotherPose_.timeout() == false) {
+//    if (timedPose_.timeout() == false && timedBrotherPose_.timeout() == false) {
+    if (timedPose_.timeout() == false) {
         return false;
     } else {
         if (timedPose_.timeout()) {
             printf(" current pose is timeout \n");
         }
-        if (timedBrotherPose_.timeout()) {
-            printf(" brother pose is timeout \n");
-        }
+//        if (timedBrotherPose_.timeout()) {
+//            printf(" brother pose is timeout \n");
+//        }
         return true;
     }
 }
@@ -137,6 +138,7 @@ void Navigation::HandleAgvStatu(const MqttMsg &msg) {
     if (msg.toProtoMessage(speed)) {
         ResetStatu(speed.v(), speed.w());
         ResetClamp((ClampStatu) speed.clamp());
+        ResetLifter((LifterStatus) speed.lifter());
     }
 }
 
@@ -231,6 +233,7 @@ bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
             plc.set_v(timedV_.Get());
             plc.set_w(timedA_.Get());
             plc.set_clamp(timed_clamp_.Get());
+            plc.set_lifter(timed_lifter_.Get());
             //printf(" timed_clamp_:%d\n ",timed_clamp_.Get());
             robotStatu.mutable_agvstatu()->CopyFrom(plc);
         }
@@ -248,6 +251,10 @@ void Navigation::ResetClamp(ClampStatu statu) {
     timed_clamp_.reset(statu, 1);
 }
 
+void Navigation::ResetLifter(LifterStatus status) {
+    timed_lifter_.reset(status, 1);
+}
+
 void Navigation::ResetPose(const Pose2d &pose) {
     timedPose_.reset(pose, 1.0);
 }
@@ -591,10 +598,11 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
 
 Navigation::MpcResult
 Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
-    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
-        printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
-        return eMpcFailed;
-    }
+//    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
+//    if (timedPose_.timeout()) {
+//        printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
+//        return eMpcFailed;
+//    }
     NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();
 
     stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
@@ -783,7 +791,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
     Pose2d brother_in_self = Pose2d::relativePose(brother, pose);
 //    std::cout<<" brother_in_self: "<<brother_in_self<<std::endl;
     if (move_mode_ == eDouble) {
-        brother_in_self = Pose2d(pose.x()+100,pose.y()+100,0);
+        brother_in_self = Pose2d(pose.x() + 100, pose.y() + 100, 0);
     }
     RotateMPC MPC(brother_in_self, obs_w, obs_h, limit_wmg.min, limit_wmg.max);
     RotateMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
@@ -793,10 +801,15 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
         printf(" Rotation_mpc solve no solution set v/w = 0\n");
         out.clear();
         out.push_back(0);
-    } else
-    {
-        double min_angular_velocity = 3.0/180*M_PI;
-        if (out[0] < min_angular_velocity) out[0] = min_angular_velocity;
+    } else {
+        double min_angular_velocity = 3.0 / 180 * M_PI;
+        if (fabs(out[0]) < min_angular_velocity) {
+            if (out[0] > 0)
+                out[0] = min_angular_velocity;
+            else if (out[0] < 0)
+                out[0] = -min_angular_velocity;
+            else {}
+        }
     }
     return ret == success || ret == no_solution;
 }
@@ -960,6 +973,48 @@ bool Navigation::clamp_open() {
     return false;
 }
 
+bool Navigation::lifter_rise() {
+    if (monitor_) {
+        printf("Lifter upping...\n");
+        monitor_->lifter_rise(move_mode_);
+        actionType_ = eLifterRise;
+        while (cancel_ == false) {
+            if (timed_lifter_.timeout()) {
+                printf("timed lifter is timeout\n");
+                return false;
+            }
+            if (timed_lifter_.Get() == eRose)
+                return true;
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            monitor_->lifter_rise(move_mode_);
+            actionType_ = eLifterRise;
+        }
+        return false;
+    }
+    return false;
+}
+
+bool Navigation::lifter_down() {
+    if (monitor_) {
+        printf("Lifter downing...\n");
+        monitor_->lifter_down(move_mode_);
+        actionType_ = eLifterDown;
+        while (cancel_ == false) {
+            if (timed_lifter_.timeout()) {
+                printf("timed lifter is timeout\n");
+                return false;
+            }
+            if (timed_lifter_.Get() == eDowned)
+                return true;
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            monitor_->lifter_down(move_mode_);
+            actionType_ = eLifterDown;
+        }
+        return false;
+    }
+    return false;
+}
+
 Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect) {
     if (IsArrived(node)) {
         printf(" current already in target completed !!!\n");
@@ -1179,9 +1234,18 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             }
         } else if (act.type() == 8) { //切换模式
             SwitchMode(act.changedmode(), act.wheelbase());
-        } else {
-            printf(" action type invalid not handled !!\n");
-            break;
+        } else if (act.type() == 9) { //提升机构提升
+            if (this->lifter_rise() == false) {
+                printf("提升failed ...\n");
+                break;
+            }
+        } else if (act.type() == 10) { //提升机构下降
+            if (this->lifter_down() == false) {
+                printf("下降failed ...\n");
+                break;
+            }
+        }else {
+            printf(" action type invalid not handled !!\n");break;
         }
         newUnfinished_cations_.pop();
     }

MPC/navigation.h

@@ -55,6 +55,8 @@ public:
 
     virtual void ResetClamp(ClampStatu statu);
 
+    virtual void ResetLifter(LifterStatus status);
+
     TimedLockData<Pose2d> &RealTimePose() {
         return timedPose_;
     }
@@ -138,6 +140,10 @@ protected:
 
     virtual bool clamp_open();
 
+    virtual bool lifter_rise();
+
+    virtual bool lifter_down();
+
     bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);
 
     /* 原地旋转mpc
@@ -173,7 +179,8 @@ protected:
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;
-    TimedLockData<ClampStatu> timed_clamp_;
+    TimedLockData<ClampStatu> timed_clamp_;     //加持杆
+    TimedLockData<LifterStatus> timed_lifter_;    //提升机构
 
     TimedLockData<Pose2d> timedBrotherPose_;           //当前位姿
     TimedLockData<double> timedBrotherV_;              //底盘数据

MPC/navigation_main.cpp

@@ -137,7 +137,8 @@ bool NavigationMain::clamp_open() {
 
 Navigation::MpcResult
 NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
-    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
+//    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
+    if (timedPose_.timeout()) {
         printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
         return eMpcFailed;
     }

MPC/rotate_mpc.cpp

@@ -44,7 +44,7 @@ public:
         for (int t = 0; t < N - 1; ++t) {
             /// 角速度，角加速度weight loss
             //fg[0] += angular_velocity_cost_weight * CppAD::pow(vars[N + t], 2);
-            fg[0] += angular_acceleration_cost_weight * CppAD::pow(vars[N + t + 1] - vars[N + t], 2);
+            //fg[0] += angular_acceleration_cost_weight * CppAD::pow(vars[N + t + 1] - vars[N + t], 2);
             /// 目标角度 loss
             fg[0] += angular_cost_weight * CppAD::pow(target_theta - vars[t], 2);
         }
@@ -55,7 +55,7 @@ public:
         int n_obs = n_angular_acceleration + N;
 
         /// 角度等式约束 1~N
-        fg[n_angular] = vars[0] - vars[N] * dt;
+        fg[n_angular] = vars[0] - angular_velocity * dt;
         for (int t = 1; t < N; ++t) {
             // State at time t + 1
             CppAD::AD<double> theta1 = vars[t];
@@ -190,10 +190,21 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     Pose2d targetAngularInAGV = current_to_target;
     /// 调整到适合的目标角度
-    double acc_time = (max_angular_velocity - angular_velocity) / max_angular_acceleration_velocity;
-    double max_angular_once = max_angular_velocity * N * dt - acc_time * (max_angular_velocity - angular_velocity);
-    if (targetAngularInAGV.theta() > max_angular_once ) {
-        targetAngularInAGV.set_theta(max_angular_once);
+    if (targetAngularInAGV.theta() > 0) {
+        double acc_time = (max_angular_velocity - angular_velocity) / max_angular_acceleration_velocity;
+        double max_angular_once = max_angular_velocity * N * dt - acc_time * (max_angular_velocity - angular_velocity);
+        if (targetAngularInAGV.theta() > max_angular_once ) {
+            targetAngularInAGV.set_theta(max_angular_once);
+        }
+    }
+    else if (targetAngularInAGV.theta() < 0) {
+        double acc_time = fabs((-max_angular_velocity - angular_velocity) / -max_angular_acceleration_velocity);
+        double max_angular_once = -max_angular_velocity * N * dt + acc_time * (-max_angular_velocity - angular_velocity);
+        if (targetAngularInAGV.theta() < max_angular_once ) {
+            targetAngularInAGV.set_theta(max_angular_once);
+        }
+    } else{
+
     }
 
 

config/navigation_main.prototxt

@@ -1,4 +1,4 @@
-#127.0.0.1
+
 main_agv:true
 rpc_ipport:"192.168.0.70:9090"
 Agv_emqx
@@ -6,9 +6,9 @@ Agv_emqx
 	NodeId:"agv-main"
 	ip:"192.168.0.70"
 	port:1883
-	pubSpeedTopic:"monitor_main/speedcmd"
-	subPoseTopic:"monitor_main/statu"
-	subSpeedTopic:"monitor_main/speed"
+	pubSpeedTopic:"monitor_child/speedcmd"
+	subPoseTopic:"monitor_child/statu"
+	subSpeedTopic:"monitor_child/speed"
 }
 
 Terminal_emqx

message.proto

@@ -15,13 +15,14 @@ message AgvStatu {
     float w = 2;
     int32 clamp = 3;    //夹持状态  0，中间状态，1夹紧到位，2，打开到位，
     int32 clamp_other = 4; //另外一节
-
+    int32 lifter = 5;   //提升机构
+    int32 lifter_other = 6;     //另外一节
 }
 
 message ToAgvCmd {
     int32 H = 1;  //心跳
     int32 M = 2;    //模式：2整车模式，1：单车
-    int32 T = 3; // 1:原地旋转，2:横移，3:MPC巡线/前进, 5,夹持，6，松夹持，其他/未接收到：停止
+    int32 T = 3; // 1:原地旋转, 2:横移, 3:MPC巡线/前进, 5:夹持, 6:松夹持, 7:提升机构抬升, 8:抬升机构下降, 其他/未接收到：停止
     float V = 4;  //角速度
     float W = 5;  //线速度
     float L = 6;  //轴距
@@ -53,7 +54,7 @@ message Trajectory
 //----------------------------
 message NewAction //进库，出库，轨迹导航，夹持，松夹持
 {
-    int32 type = 1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，6:夹持，7:松夹持，8:切换模式
+    int32 type = 1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，6:夹持，7:松夹持，8:切换模式， 9:提升机构提升， 10: 提升机构下降
     PathNode spaceNode = 2; //进出库起点
     PathNode passNode = 3; //进出库途径点
     PathNode streetNode = 4; //进出库终点