1.出库改为整车

2.出入口出库速度改为巡线速度
3.限制进载车板角速度
4.优化整车模式等待载车板和门控信号
5.优化读取车位与载车板对应表

MPC/custom_type.h

@@ -159,7 +159,7 @@ enum eCarrierIdInRegionId{
 };
 
 typedef std::pair<short, short > Region_Carry;
-typedef std::map<short ,Region_Carry> SpaceNo2Region_Carry;
+typedef std::map<int ,Region_Carry> SpaceNo2Region_Carry;
 
 //////////////////////////////////////////////////
 

MPC/loaded_mpc.cpp

@@ -172,7 +172,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     double wmg = statu[4];
 
 
-    size_t N=20;//int(10./(1+exp(-16.*(fabs(line_velocity)-0.5)))+10);
+    size_t N=18;//int(10./(1+exp(-16.*(fabs(line_velocity)-0.5)))+10);
     size_t nx = 0;
     size_t ny = nx + N;
     size_t nth = ny + N;

MPC/navigation.cpp

@@ -485,7 +485,7 @@ bool Navigation::AdjustReferToTarget(const Pose2d &target, stLimit limit_rotate,
     node.set_l(0.03);
     node.set_w(0.2);
 
-    stLimit limitV = {0.03, 0.5};
+    stLimit limitV = {parameter_.inoutvlimit().min(), parameter_.inoutvlimit().max()};
     stLimit limitW = {0.03, 0.1};
     std::cout << " Adjust MoveTarget:" << Pose2d(node.x(), node.y(), node.theta()) << std::endl;
     return MoveToTarget(node, limitV, limitW, true, true);
@@ -621,22 +621,6 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
             bool adjustRet = AdjustReferToTarget(target, limit_w,directions);
             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;
-            }
-
-            int directions = Direction::eForward;
-            if (anyDirect)
-                directions = Direction::eForward | Direction::eBackward |
-                             Direction::eLeft | Direction::eRight;
-
-            if (RotateReferToTarget(node, limit_w, directions) != eMpcSuccess) {
-                std::cout << " Rotate refer to target failed,target: " << target <<
-                          " directions: " << std::hex << directions << " line_: " << __LINE__ << std::endl;
-                return false;
-            }
-            continue;*/
         }
 
         already_in_mpc = true;
@@ -681,7 +665,7 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
         ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
             NavMessage::PathNode node = action.pathnodes(i);
-            if (i + 1 < action.pathnodes_size()) {
+            if (i  < action.pathnodes_size()-1) {
                 NavMessage::PathNode next = action.pathnodes(i + 1);
                 Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
                 node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
@@ -698,12 +682,33 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
             if (anyDirect)
                 directions = Direction::eForward | Direction::eBackward |
                              Direction::eLeft | Direction::eRight;
-            /*if (i > 0) {
-                if (RotateReferToTarget(node, wmg_limit, directions) != eMpcSuccess) {
-                    printf(" before move node  RotateReferToNone Failed!!\n");
-                    return false;
+            /*if(i>=1 && move_mode_==eSingle){
+                //马路Y巡航，需要判断等待
+                Pose2d targetInAGV = Pose2d::relativePose(Pose2d(node.x(), node.y(), 0), timedPose_.Get());
+                double distance=Pose2d::distance(Pose2d(0,0,0),targetInAGV);
+                double thresh=0.5*obs_w_*distance;
+                while(cancel_==false) {
+                    usleep(1000*500);
+                    if(timedPose_.timeout() || timedBrotherPose_.timeout()){
+                        return false;
+                    }
+                    Pose2d otherInAGV = Pose2d::relativePose(timedBrotherPose_.Get(), timedPose_.Get());
+                    double dot = targetInAGV.x() * otherInAGV.x() + targetInAGV.y() * otherInAGV.y();
+                    if (dot > thresh) {  //目标点与other在当前车的同一朝向上，需要等待other点到位
+                        printf(" need to wait other,dot:%f,thresh:%f\n",dot,thresh);
+                        if(fabs(otherInAGV.x())>obs_w_*3 && fabs(otherInAGV.y())<0.2){
+                            std::cout<<"Wait other agv completed otherInCurrent:"<<otherInAGV<<std::endl;
+                            break;
+                        }else{
+                            std::cout<<"Wait other agv ,otherInCurrent:"<<otherInAGV<<std::endl;
+                        }
+                    }else{
+                        printf(" not need wait other,dot:%f,thresh:%f\n",dot,thresh);
+                        break;
+                    }
                 }
             }*/
+
             if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true, clampLifterAction) == false)
                 return false;
             else
@@ -858,7 +863,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
     target.set_y(y);
     target.set_theta(current.theta());
     target.set_l(0.02);
-    target.set_w(0.05);
+    target.set_w(0.5);
     target.set_id(space.id());
     printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n", x, y, target.theta());
 
@@ -926,7 +931,7 @@ int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) {
     }
     std::string space_flag = "S_";
     if (spaceNode.id().find(space_flag) >= 0) {
-        std::string id = spaceNode.id().substr(2, spaceNode.id().length() - 1);
+        std::string id = spaceNode.id().substr(space_flag.size(), spaceNode.id().length() - 1);
         int space_id = stoi(id);
         return space_id;
     }
@@ -935,12 +940,12 @@ int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) {
 
 bool Navigation::TargetIsEntrance(NavMessage::PathNode node) {
     int space_id = GetSpaceId(node);
-    return space_id==1101;
+    return space_id==1101 || space_id==1102;
 }
 
 bool Navigation::TargetIsExport(NavMessage::PathNode node) {
     int space_id = GetSpaceId(node);
-    return space_id==1101;
+    return space_id==1101 || space_id==1102;
 }
 
 
@@ -983,6 +988,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         }
         if (move_mode_ == eSingle)
             clampLifterAction |= eByteClampHalfOpen;
+        
         if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true, clampLifterAction) == false) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
@@ -1103,6 +1109,15 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             clampLifterAction |= eByteCLampClose;
 
         printf(" clampLifter_statu2 :%d\n", clampLifterAction);
+        if(!TargetIsExport(action.spacenode())&& !TargetIsEntrance(action.spacenode()))
+            limit_w={0.01,0.05}; //进库纠偏角速度限制
+        else{
+            // 整车进 出入口速度调整为与巡线一致
+            if (move_mode_==eDouble){
+                limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
+                limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+            }
+        }
         bool retMove = MoveToTarget(new_target, limit_v, limit_w, true, false, clampLifterAction);
 
         if (retMove == false) {
@@ -1111,7 +1126,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         }
 
     } else if (action.type() == 2) {
-//        LogWriter("Out space beg-");
+        //出库,起点为spaceNode
         // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿
         Pose2d vec(action.streetnode().x() - action.spacenode().x(),
                    action.streetnode().y() - action.spacenode().y(), 0);
@@ -1119,6 +1134,11 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         Pose2d space(action.spacenode().x(), action.spacenode().y(), theta);
         Pose2d current = timedPose_.Get();
         Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(), current.y(), 0), space));
+        // 整车从出入口出库速度调整为与巡线一致
+        if (move_mode_==eDouble && (TargetIsExport(action.spacenode()) || TargetIsExport(action.spacenode()))){
+            limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
+            limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+        }
         if (diff.x() < 0.05 || diff.y() < 0.10) {
             //出库，移动到马路点，允许自由方向，不允许中途旋转
             action.mutable_streetnode()->set_theta(theta);
@@ -1312,7 +1332,7 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
     double b = (x - tx) * sin(theta) + (y - ty) * cos(theta);
 
     if (pow(a / l, 8) + pow(b / w, 8) <= 1) {
-        if (fabs(timedV_.Get()) < 0.05 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(timedV_.Get()) < 0.06 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" Arrived target: %f  %f l:%f w:%f theta:%f\n", tx, ty, l, w, theta);
             return true;
         }
@@ -1583,34 +1603,70 @@ bool Navigation::WaitCarrierCompleted(int spaceID, CarrierStatu statu) {
 
 bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
     int currentStatu = 0;
-    while (cancel_ == false) {
-        if (timed_clamp_.timeout() || timed_lifter_.timeout()) {
-            return false;
-        }
-        if (timed_clamp_.Get() == eClosed) {
-            currentStatu |= eByteCLampClose;
-        }
-        if (timed_clamp_.Get() == eHalfOpened) {
-            currentStatu |= eByteClampHalfOpen;
-        }
-        if (timed_clamp_.Get() == eFullyOpened) {
-            currentStatu |= eByteClampFullyOpen;
-        }
-        if (timed_lifter_.Get() == eRose) {
-            currentStatu |= eByteLifterUp;
+    if (move_mode_ == eSingle){
+        while (cancel_ == false) {
+            if (timed_clamp_.timeout() || timed_lifter_.timeout()) {
+                return false;
+            }
+            if (timed_clamp_.Get() == eClosed) {
+                currentStatu |= eByteCLampClose;
+            }
+            if (timed_clamp_.Get() == eHalfOpened) {
+                currentStatu |= eByteClampHalfOpen;
+            }
+            if (timed_clamp_.Get() == eFullyOpened) {
+                currentStatu |= eByteClampFullyOpen;
+            }
+            if (timed_lifter_.Get() == eRose) {
+                currentStatu |= eByteLifterUp;
+            }
+            if (timed_lifter_.Get() == eDowned) {
+                currentStatu |= eByteLifterDown;
+            }
+            printf(" current statu:%d   action status:%d  last:%d\n",
+                   currentStatu,clampLifterAction,currentStatu&clampLifterAction);
+            if((currentStatu&clampLifterAction) == clampLifterAction)
+                return true;
+
+            double down_v[3] = {0, 0, 0};//下发线速度0
+            double down_w[3] = {0, 0, 0};//下发角速度0
+            SendMoveCmd(clampLifterAction, move_mode_, actionType_, down_v, down_w);
+            usleep(1000 * 100);
         }
-        if (timed_lifter_.Get() == eDowned) {
-            currentStatu |= eByteLifterDown;
+
+    } else if (move_mode_ == eDouble){
+
+        while (cancel_ == false) {
+            if (timed_clamp_.timeout() || timed_lifter_.timeout() ||
+                    timed_other_clamp_.timeout() || timed_other_lifter_.timeout()) {
+                return false;
+            }
+            if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get() == eClosed) {
+                currentStatu |= eByteCLampClose;
+            }
+            if (timed_clamp_.Get() == eHalfOpened && timed_other_clamp_.Get() == eHalfOpened) {
+                currentStatu |= eByteClampHalfOpen;
+            }
+            if (timed_clamp_.Get() == eFullyOpened && timed_other_clamp_.Get() == eFullyOpened) {
+                currentStatu |= eByteClampFullyOpen;
+            }
+            if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get() == eRose) {
+                currentStatu |= eByteLifterUp;
+            }
+            if (timed_lifter_.Get() == eDowned && timed_other_lifter_.Get() == eDowned) {
+                currentStatu |= eByteLifterDown;
+            }
+            printf(" current statu:%d   action status:%d  last:%d\n",
+                   currentStatu,clampLifterAction,currentStatu&clampLifterAction);
+            if((currentStatu&clampLifterAction) == clampLifterAction)
+                return true;
+
+            double down_v[3] = {0, 0, 0};//下发线速度0
+            double down_w[3] = {0, 0, 0};//下发角速度0
+            SendMoveCmd(clampLifterAction, move_mode_, actionType_, down_v, down_w);
+            usleep(1000 * 100);
         }
-        printf(" current statu:%d   action status:%d  last:%d\n",
-               currentStatu,clampLifterAction,currentStatu&clampLifterAction);
-        if((currentStatu&clampLifterAction) == clampLifterAction)
-            return true;
 
-        double down_v[3] = {0, 0, 0};//下发线速度0
-        double down_w[3] = {0, 0, 0};//下发角速度0
-        SendMoveCmd(clampLifterAction, move_mode_, actionType_, down_v, down_w);
-        usleep(1000 * 100);
     }
     if (cancel_ == true) {
         return false;
@@ -1714,14 +1770,13 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         if (directY) {
             target_in_agv = target_in_agv.rotate(M_PI / 2.0);
         }
-        if (Pose2d::abs(target_in_agv) < Pose2d(0.2, 2, M_PI * 2)) {
+
             for (int i = 0; i < down_count; ++i) {
                 if (down_v[i] >= 0 && down_v[i] < limit_v.min)
                     down_v[i] = limit_v.min;
                 if (down_v[i] < 0 && down_v[i] > -limit_v.min)
                     down_v[i] = -limit_v.min;
             }
-        }
 
         //放大角速度
 //        out[1] *= 10;
@@ -2172,20 +2227,26 @@ void Navigation::ResetCarrier(std::vector<int32_t> status) {
 
 }
 
-void Navigation::Stringsplit(const std::string& str, const std::string splits, std::vector<std::string>& res){
+bool Navigation::Stringsplit(const std::string& str, const std::string splits, std::vector<std::string>& res){
     res.clear();
-    if(str.empty())
-        return;
+    if(str.empty()){
+        return false;
+    }
     std::string strs = str+splits;
     size_t pos = strs.find(splits);
     int step = splits.size();
 
     while (pos!= std::string::npos){
         std::string temp = strs.substr(0, pos);
-        res.push_back(temp);
+        if (temp.size() > 0)
+            res.push_back(temp);
         strs = strs.substr(pos+step,strs.size());
         pos = strs.find(splits);
     }
+//    for (int i = 0; i < res.size(); ++i) {
+//        printf("%d: %s[%zu]\n",i, res[i].c_str(),res[i].size());
+//    }
+    return true;
 }
 
 bool Navigation::LoadSpaceNo2CarrierNo(){
@@ -2194,13 +2255,20 @@ bool Navigation::LoadSpaceNo2CarrierNo(){
     std::string line;
     std::vector<std::string> data_in_one_line;
     if (in){
-        short space_id, region_id, carrier_no;
+        int space_id, region_id, carrier_no;
         while(std::getline(in,line)){
-            Stringsplit(line, " ",data_in_one_line);
-            space_id = std::stoi(data_in_one_line[0]);
-            region_id = std::stoi(data_in_one_line[1]);
-            carrier_no = std::stoi(data_in_one_line[2]);
-            spaceNo_2_region_carrier_excel_[space_id] = Region_Carry(region_id,carrier_no);
+            if (!Stringsplit(line, " ",data_in_one_line) || line.find("//") != std::string::npos)
+                continue;
+            if (data_in_one_line.size()==3) {
+                space_id = std::stoi(data_in_one_line[0]);
+                region_id = std::stoi(data_in_one_line[1]);
+                carrier_no = std::stoi(data_in_one_line[2]);
+                spaceNo_2_region_carrier_excel_[space_id] = Region_Carry(region_id, carrier_no);
+            }
+        }
+        //打印
+        for (auto & i : spaceNo_2_region_carrier_excel_) {
+            printf("space_id: %d, region_id: %d, carrier_no: %d\n",i.first,i.second.first,i.second.second);
         }
         return true;
     } else{

MPC/navigation.h

@@ -73,7 +73,7 @@ public:
 
     CarrierStatu GetCarrierStatusBySpaceID(int space_id);
 
-    void Stringsplit(const std::string &str, const std::string splits, std::vector<std::string> &res);
+    bool Stringsplit(const std::string &str, const std::string splits, std::vector<std::string> &res);
 
     static void anasisDoorStatu(int32_t i_door_status, DoorStatusMap& m_door_status);
 

MPC/navigation_main.cpp

@@ -306,7 +306,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     target.set_y(y);
     target.set_theta(current.theta());
     target.set_l(0.02);
-    target.set_w(0.1);
+    target.set_w(0.5);
     target.set_id(space.id());
     printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
 

MPC/navigation_main.h

@@ -42,7 +42,7 @@ public:
             Pose2d pose = timedPose_.Get();
             Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));
 
-            if (diff.x() > 3.4 || diff.x() < 2.3 || diff.y() > 0.2 || diff.theta() > 5 * M_PI / 180.0) {
+            if (diff.x() > 3.4 || diff.x() < 2.3 || diff.y() > 0.5 || diff.theta() > 5 * M_PI / 180.0) {
                 std::cout << " distance with two agv is too far diff: " << diff << std::endl;
                 return false;
             }

config/SpaceNO2CarrarierNO.txt

@@ -1,4 +1,86 @@
-65 -1 2
+// 车位id + “ ” + 区域id + “ ” + 区域内载车板编号
+// 区域id < 0 :与载车板无关。绝对值为所在区域
+01      2   1
+03 2 2
+05 2 3
+07 2 4
+09 2 5
+11 2 6
+
+02 5 6
+04 5 5
+06 5 4
+08 5 3
+10 5 2
+12 5 1
+
+13 7 1
+15 7 2
+17 7 3
+19 7 4
+21 7 5
+23 7 6
+
+14 9 9
+16 9 8
+18 9 7
+20 9 6
+22 9 5
+24 9 4
+
+25 6 9
+27 6 8
+29 6 7
+31 6 6
+33 6 5
+35 6 4
+37 6 3
+39 6 2
+41 6 1
+
+26 8 1
+28 8 2
+30 8 3
+32 8 4
+34 8 5
+36 8 6
+38 8 7
+40 8 8
+42 8 9
+
+43 4 1
+45 4 2
+47 4 3
+49 4 4
+51 4 5
+53 4 6
+
+55 1 1
+57 1 2
+59 1 3
+
+44 3 9
+46 3 8
+48 3 7
+50 3 6
+52 3 5
+54 3 4
+56 3 3
+58 3 2
+60 3 1
+
+
+61 -3 9
+62 -3 8
+63 -3 7
+64 -3 6
+65 -3 5
+66 -3 4
+67 -3 3
+68 -3 2
+69 -3 1
+
+
 101 2 1
 103 2 2
 105 2 3
@@ -17,8 +99,6 @@
 115 7 2
 117 7 3
 119 7 4
-
-
 121 7 5
 123 7 6