1.mqtt publish 加锁

2.马路点导航取消提升机构动作
3.马路点导航精度方向调整。最后一点精度不旋转
4.马路点导航，单车夹持杆并行打开到半开位
5.从入口出库旋转180度
6.修正入库精度方向（+90度）
7.修正入库旋转方法返回值判断错误
8.双单车入库，先进车越过后进车目标点后，后进车再开始动作
9.添加判断载车板车位函数
10.添加入库时全开位夹持与行走并行函数
11.修改整车模式下，整车角度计算方法。改为两车角度取平均
12.到达不了目标点时，由旋转对正改为平移对正
13.取消入库时全开位夹持与行走并行函数
14.修改整车模式下，整车角度计算方法。改为两车位置计算

MPC/monitor/monitor_emqx.cpp

@@ -164,7 +164,8 @@ void Monitor_emqx::stop()
   Publish(speedcmd_topic_,msg);
 }
 
-void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[], double L, int P, double D) {
+void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[],
+                                double L, int P, double D,double Y1,double Y2) {
 //    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
 //    {
 //        printf(" Main agv mpc must set wheelBase\n ");
@@ -189,6 +190,8 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
     agvCmd.set_w2(w_correct[1]);
     agvCmd.set_v3(velocity[2]);
     agvCmd.set_w3(w_correct[2]);
+    agvCmd.set_y1(Y1);
+    agvCmd.set_y2(Y2);
     agvCmd.set_l1(L);
     agvCmd.set_p1(P);
     agvCmd.set_d1(D);

MPC/monitor/monitor_emqx.h

@@ -18,7 +18,8 @@ class Monitor_emqx : public Terminator_emqx
     void set_clampLifterCmd_topic(std::string topic);
     void set_speed(int mode,ActionType type,double v,double a);
     void set_speed(int mode,ActionType type,double v,double a,double L);
-    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double L=0,int P=0,double D=0);
+    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],
+                      double L=0,int P=0,double D=0,double Y1=0.,double Y2=0.);
 //    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double yaw_diff[], double L=0,int P=0,double D=0);
     void clamp_close(int mode);
     void clamp_half_open(int mode);

MPC/navigation.cpp

@@ -426,6 +426,47 @@ bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate,
     return false;
 }
 
+
+bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate) {
+    if (inited_ == false) {
+        printf(" MPC_rotate has not inited\n");
+        return false;
+    }
+    if (PoseTimeout()) {
+        printf(" MPC_rotate Error:Pose is timeout \n");
+        return false;
+    }
+
+    Pose2d current = timedPose_.Get();
+    NavMessage::PathNode yawTarget;
+    yawTarget.set_x(current.x() + 100.);
+    yawTarget.set_y(current.y());
+    int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+    RotateReferToTarget(yawTarget, limit_rotate, directions);
+    printf(" exec MPC_rotate autoDirect:%d\n", directions);
+
+    current = timedPose_.Get();
+    Pose2d diff=target-current;
+    Pose2d newTarget;
+    NavMessage::PathNode node;
+    if(fabs(diff.x())>fabs(diff.y())){
+        newTarget=Pose2d(current.x(),target.y(),current.theta());
+        node.set_theta(M_PI/2.0);
+    }else{
+        newTarget=Pose2d(target.x(),current.y(),current.theta());
+        node.set_theta(0);
+    }
+    node.set_x(newTarget.x());
+    node.set_y(newTarget.y());
+    node.set_l(0.03);
+    node.set_w(0.2);
+
+    stLimit limitV={0.03,0.5};
+    stLimit limitW={0.03,0.1};
+    return MoveToTarget(node,limitV,limitW,true,true);
+
+}
+
 Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node, stLimit limit_rotate, int directions) {
     if (inited_ == false) {
         printf(" MPC_rotate has not inited\n");
@@ -557,8 +598,11 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         Pose2d target(node.x(), node.y(), 0);
         Pose2d targetInCurrent = Pose2d::relativePose(target, current);
 
-        if (enableTotarget == false && Pose2d::distance(Pose2d(0, 0, 0), targetInCurrent) > 0.3) {
-            if (already_in_mpc && enable_rotate == false) {
+        if (enableTotarget == false ) {
+            bool adjustRet=AdjustReferToTarget(target,limit_w);
+            if(adjustRet==false)
+                return false;
+            /*if (already_in_mpc && enable_rotate == false) {
                 printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
                 return false;
             }
@@ -573,8 +617,9 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
                           " directions: " << std::hex << directions << " line_: " << __LINE__ << std::endl;
                 return false;
             }
-            continue;
+            continue;*/
         }
+
         already_in_mpc = true;
         MpcResult ret = MpcToTarget(node, limit_v, anyDirect);
         if (ret == eMpcSuccess) {
@@ -609,10 +654,8 @@ bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
 
         /////////////////////////////////////////
 
-
-        std::thread *thread_clampOpenHalf = nullptr;
         if(move_mode_==eSingle) {
-            thread_clampOpenHalf = new std::thread(&Navigation::clamp_half_open, this);
+            clamp_half_open();
         }
         ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
@@ -634,23 +677,17 @@ bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
             if (anyDirect)
                 directions = Direction::eForward | Direction::eBackward |
                              Direction::eLeft | Direction::eRight;
-            if (i > 0) {
+            /*if (i > 0) {
                 if (RotateReferToTarget(node, wmg_limit, directions) != eMpcSuccess) {
                     printf(" before move node  RotateReferToNone Failed!!\n");
                     return false;
                 }
-            }
+            }*/
             if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true) == false)
                 return false;
             else
                 isInSpace_ = false;
         }
-        if(move_mode_==eSingle) {
-            printf(" wait clamp open half...\n");
-            thread_clampOpenHalf->join();
-            printf("clamp open half completed!!!\n");
-            delete thread_clampOpenHalf;
-        }
         if (cancel_ == true) {
             return false;
         }
@@ -843,23 +880,13 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         //计算车位朝向
         action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
         //计算当前车是进入车位1点还是2点,    提升下降动作与旋转调整并行
-        LifterStatus lifter_actType;
-        std::thread *thread_lifter = nullptr;
-        if (IsUperSpace(action.spacenode())) {
-            //进载车板库前提升机构上升
-            lifter_actType = eRose;
-            thread_lifter = new std::thread(&Navigation::lifter_rise, this);
-        } else {
-            //否则下降
-            lifter_actType = eDowned;
-            thread_lifter = new std::thread(&Navigation::lifter_down, this);
+        if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
+            lifter_rise();
+        } else {    //否则下降
+            lifter_down();
         }
         NavMessage::PathNode new_target;
         bool retRotate=RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcSuccess;
-        printf(" wait lifter action:%d\n", lifter_actType);
-        thread_lifter->join();
-        delete thread_lifter;
-        printf("lifter action:%d  completed!!!\n", lifter_actType);
 
         if(retRotate==false){
             return false;
@@ -897,13 +924,9 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         }
         isInSpace_ = true;
         printf("Enter space beg...\n");
-
-        std::thread *thread_clamp = nullptr;
-        thread_clamp = new std::thread(&Navigation::singleClamp_fullyOpen_until_inspaceBegin, this);
-
+        if(move_mode_==eSingle)
+            clamp_half_open();  //////// 后续再改为全打开位
         bool retMove=MoveToTarget(new_target, limit_v, limit_w, true, false);
-        thread_clamp->join();
-        delete thread_clamp;
 
         if(retMove==false){
             printf(" Enter space:%s failed. type:%d\n", action.spacenode().id().data(), action.type());
@@ -934,7 +957,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             return false;
         }
         isInSpace_ = false;
-
+        lifter_down();
         //从入口出库旋转180
         if (move_mode_ == eDouble && action.spacenode().id().find("S1101") != -1) {
             Pose2d current_pose = timedPose_.Get();
@@ -1154,9 +1177,10 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
     }
 }
 
-void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance) {
+void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1,double Y2) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance);
+        monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance,Y1,Y2);
         if (type == eRotation)
             RWheel_position_ = eR;
         if (type == eVertical)
@@ -1240,6 +1264,7 @@ bool Navigation::singleClamp_fullyOpen_until_inspaceBegin(){
             return false;
         }
         if(fabs(timedV_.Get())<0.02){
+
             usleep(1000*300);
             continue;
         }
@@ -1373,7 +1398,11 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         } else {
             std::cout << " MPC X axis  target_relative:" << target_relative << std::endl;
         }
+        if(!PossibleToTarget(node, directY)){
+            directY=!directY;
+        }
     }
+
     printf(" exec along autoDirect:%d\n", autoDirect);
 
     //文件log
@@ -1476,11 +1505,15 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             std::string id = node.id().substr(1, node.id().length() - 1);
             space_id = stoi(id);
         }
-//        double diff_yaw_[2] = {diff_yaw[0], diff_yaw[1]};
+        double Y1=0.0,Y2=0.0;
+        if(move_mode_==eDouble){
+            Y1=timeYawDiff1_.Get();
+            Y2=timeYawDiff2_.Get();
+        }
         if (directY == false)
-            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance);
+            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance,Y1,Y2);
         else
-            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance);
+            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance,Y1,Y2);
         actionType_ = directY ? eHorizontal : eVertical;
 
         //日志打印
@@ -1674,7 +1707,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 break;
             }
         } else if (act.type() == 7) {
-            if (this->clamp_fully_open() == false) {
+            if (this->clamp_half_open() == false) {
                 printf("打开夹持 failed...\n");
                 break;
             }

MPC/navigation.h

@@ -113,7 +113,8 @@ protected:
 
     virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
 
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance);
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1=0.0,double Y2=0.0);
 //    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], double diff_yaw[], int space_id, double distance);
 
     static void RobotPoseCallback(const MqttMsg &msg, void *context);
@@ -145,6 +146,8 @@ protected:
 
     virtual MpcResult RotateReferToTarget(NavMessage::PathNode target, stLimit limit_rotate, int directions);
 
+    virtual bool AdjustReferToTarget(const Pose2d& target, stLimit limit_v);
+
     /*
      * 按照路径点导航
      */
@@ -213,6 +216,8 @@ protected:
     Terminator_emqx *terminator_ = nullptr;  //本机emqx连接
     Terminator_emqx *brotherEmqx_ = nullptr;  //本机emqx连接
 
+    TimedLockData<double> timeYawDiff1_;
+    TimedLockData<double> timeYawDiff2_;
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;

MPC/navigation_main.cpp

@@ -32,9 +32,12 @@ void NavigationMain::ResetPose(const Pose2d &pose) {
         }
         Pose2d abs_diff = pose - brother;
         //计算两车朝向的方向
-        float theta = (pose.theta()+brother.theta())/2.0;//Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
+        float theta = Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
         Pose2d agv = Pose2d((pose.x() + brother.x()) / 2.0, (pose.y() + brother.y()) / 2.0, theta);
-
+        Pose2d abs_diff1 = pose - agv;
+        Pose2d abs_diff2 = brother - agv;
+        timeYawDiff1_.reset(abs_diff1.theta());
+        timeYawDiff2_.reset(abs_diff2.theta());
         Navigation::ResetPose(agv);
 //        timedPose_().m_diffYaw1=pose.theta()-theta;
 //        timedPose_().m_diffYaw2=brother.theta()-theta;
@@ -68,9 +71,11 @@ void NavigationMain::SendMoveCmd(int mode, ActionType type,
     }
 }
 
-void NavigationMain::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance) {
+void NavigationMain::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                                 int space_id, double distance,double Y1,double Y2) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_, space_id, distance);
+        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_,
+                               space_id, distance,Y1,Y2);
         if (type == eRotation)
             RWheel_position_ = eR;
         if (type == eVertical)
@@ -314,7 +319,23 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 3 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
-            return eMpcSuccess;
+
+            usleep(1000*1000);
+            Pose2d current_2 = timedPose_.Get();
+            if(spaceInCurrent.x()<0.)
+                current_2 = current_2.rotate(current_2.x(), current_2.y(), M_PI);
+            yawDiff = vecTheta - current_2.theta();
+            if (move_mode_ == eDouble) {
+                double yawDiff2 = vecTheta - (Pose2d(0, 0, M_PI) + current_now).theta();
+                if (fabs(yawDiff) > fabs(yawDiff2)) {
+                    yawDiff=yawDiff2;
+                }
+            }
+            if(fabs(yawDiff) < 0.3){
+                return eMpcSuccess;
+            }else{
+                continue;
+            }
         } else{
             const int down_count = 3;
             double v[down_count] = {0,0,0};

MPC/navigation_main.h

@@ -71,7 +71,8 @@ protected:
     RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
 
     virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance);
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1=0.,double Y2=0.);
 
     virtual bool clamp_close();