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

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

MPC/custom_type.h

@@ -99,10 +99,13 @@ enum ActionType {
     eVertical = 3,
     eClampClose = 5,
     eClampOpen = 6,
-    eLifterRise = 7,
+    eLifterRise = 7,    //提升机构上升
     eLifterDown =8
 };
+///////////////////////////////////////////////
 
+///////////////////////////////////////////////
+/// 提升机构运动状态
 enum LifterStatus {
     eInvalid = 0,
     eRose = 1,//上升到位

MPC/navigation_main.cpp

@@ -68,6 +68,7 @@ void NavigationMain::SendMoveCmd(int mode, ActionType type,
 bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
     if (Navigation::CreateRobotStatuMsg(robotStatu)) {
         robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
+        robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_lifter_.Get());
         return true;
     }
     //std::cout<<agvStatu.DebugString()<<std::endl;
@@ -78,12 +79,18 @@ void NavigationMain::ResetOtherClamp(ClampStatu statu) {
     timed_other_clamp_.reset(statu, 1);
 }
 
+void NavigationMain::ResetOtherLifter(LifterStatus status) {
+    timed_other_lifter_.reset(status, 1);
+}
+
 void NavigationMain::HandleAgvStatu(const MqttMsg &msg) {
     NavMessage::AgvStatu speed;
     if (msg.toProtoMessage(speed)) {
         ResetStatu(speed.v(), speed.w());
         ResetClamp((ClampStatu) speed.clamp());
         ResetOtherClamp((ClampStatu) speed.clamp_other());
+        ResetLifter((LifterStatus) speed.lifter());
+        ResetOtherLifter((LifterStatus) speed.lifter_other());
         //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
     }
 }
@@ -135,6 +142,54 @@ bool NavigationMain::clamp_open() {
     return false;
 }
 
+bool NavigationMain::lifter_rise() {
+    if (move_mode_ == eSingle) {
+        return Navigation::lifter_rise();
+    }
+    if (monitor_) {
+        printf("双车提升机构提升\n");
+        monitor_->lifter_rise(move_mode_);
+        while (exit_ == false) {
+            if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
+                printf("timed lifter is timeout\n");
+                return false;
+            }
+            if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get() == eRose) {
+                printf("双车提升机构提升completed!!!\n");
+                return true;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            monitor_->lifter_rise(move_mode_);
+        }
+        return false;
+    }
+    return false;
+}
+
+bool NavigationMain::lifter_down() {
+    if (move_mode_ == eSingle) {
+        return Navigation::lifter_down();
+    }
+    if (monitor_) {
+        printf("双车提升机构下降\n");
+        monitor_->lifter_down(move_mode_);
+        while (exit_ == false) {
+            if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
+                printf("timed lifter is timeout\n");
+                return false;
+            }
+            if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get() == eRose) {
+                printf("双车提升机构下降completed!!!\n");
+                return true;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            monitor_->lifter_down(move_mode_);
+        }
+        return false;
+    }
+    return false;
+}
+
 Navigation::MpcResult
 NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
 //    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {

MPC/navigation_main.h

@@ -23,6 +23,8 @@ public:
 
     void ResetOtherClamp(ClampStatu statu);
 
+    void ResetOtherLifter(LifterStatus status);
+
     virtual void HandleAgvStatu(const MqttMsg &msg);
 
     virtual void SwitchMode(int mode, float wheelBase) {
@@ -74,11 +76,16 @@ protected:
 
     virtual bool clamp_open();
 
+    virtual bool lifter_rise();
+
+    virtual bool lifter_down();
+
     virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu);
 
 protected:
     float wheelBase_;   //两节agv的位姿与单节的位姿变换
     TimedLockData<int> timed_other_clamp_;
+    TimedLockData<int> timed_other_lifter_;
 };
 
 #endif //NAVIGATION_NAVIGATION_MAIN_H