fix bug.

armor/include/armor_finder/armor_finder.h

@@ -124,9 +124,7 @@ private:
     Serial &serial;                                     // 串口对象，引用外部变量，用于和能量机关共享同一个变量
     const uint8_t &use_classifier;                      // 标记是否启用CNN分类器，引用外部变量，自动变化
     RoundQueue<double, 4> top_periodms;                 // 陀螺周期循环队列
-    RoundQueue<double, 5> box_ratioes;                  //
     systime last_front_time;                            // 上一次发生装甲板方向切换的时间
-    BoxRatioChangeType last_ratio_type;                 //
     int anti_top_cnt;                                   // 满足条件的装甲板方向切换持续次数，用于反陀螺
     AntiTopState anti_top_state;                        // 当前是否识别到陀螺
 

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -81,15 +81,17 @@ void ArmorFinder::antiTop() {
 
 void ArmorFinder::antiTop() {
     if (target_box.rect == cv::Rect2d()) return;
+    static int fps = 0;
     uint16_t shoot_delay = 0;
     auto interval = getTimeIntervalms(frame_time, last_front_time);
     if (anti_top_state == ANTI_TOP && interval > 700) {
         anti_top_state = NORMAL;
         LOGM(STR_CTR(WORD_YELLOW, "switch to normal"));
     }
-//    cout << getPointLength(last_box.getCenter() - target_box.getCenter()) << endl;
+    fps++;
-    if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 2.0) {
+    if (last_box.rect != cv::Rect2d() &&
-        LOGM("switch!");
+        getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.0) {
+        LOGM("switch %d! %lf", fps, getPointLength(last_box.getCenter() - target_box.getCenter()) / last_box.rect.height);
         if (150 < interval && interval < 700) {
             if (anti_top_state == ANTI_TOP) {
                 top_periodms.push(interval);
@@ -106,11 +108,12 @@ void ArmorFinder::antiTop() {
                 }
             }
         }
+        fps = 0;
         last_front_time = frame_time;
     }
     if (anti_top_state == NORMAL) {
         sendBoxPosition(0);
-    } else if (interval < top_periodms[-1] * 0.2){
+    } else if (interval < top_periodms[-1] * 0.2) {
         sendBoxPosition(0);
     }
     last_box = target_box;

armor/src/armor_finder/find/find_armor_box.cpp

@@ -155,8 +155,10 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) {
                 return a < b;
             }
         });
-        if(armor_boxes[0].id != 0){
+        for(auto &one_box : armor_boxes){
-            box = armor_boxes[0];
+            if(one_box.id != 0){
+                box = one_box;
+            }
         }
         if (save_labelled_boxes) {
             for (const auto &one_box : armor_boxes) {

armor/src/armor_finder/searching_state/searching_state.cpp

@@ -8,14 +8,16 @@
 #include <log.h>
 
 bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
-    if(anti_switch_cnt >= 3){
+//    if(anti_switch_cnt >= 3){
-        last_box = ArmorBox();
+//        last_box = ArmorBox();
-    }
+//        anti_switch_cnt = 0;
+//    }
     if (findArmorBox(src, target_box)) {
         if (last_box.rect != cv::Rect2d() &&
-            (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 3.0) &&
+            (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 2.0) &&
             anti_switch_cnt++ < 3) {
             target_box = ArmorBox();
+            LOGM("anti-switch!");
             return false;
         } else {
             anti_switch_cnt = 0;
@@ -27,3 +29,14 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
         return false;
     }
 }
+
+/*
+bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
+    if (findArmorBox(src, target_box)) {
+        return true;
+    } else {
+        target_box = ArmorBox();
+        return false;
+    }
+}
+*/

armor/src/armor_finder/tracking_state/tracking_state.cpp

@@ -42,25 +42,27 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
         tracker = TrackerToUse::create();
         tracker->init(src, target_box.rect);
     }else{
-        roi = src(pos).clone();
+//        roi = src(pos).clone();
-        if(classifier){
+//        if(classifier){
-            cv::resize(roi, roi, cv::Size(48, 36));
+//            cv::resize(roi, roi, cv::Size(48, 36));
-            if(classifier(roi) == 0){
+//            if(classifier(roi) == 0){
+//                target_box = ArmorBox();
+//                return false;
+//            }
+//        }else{
+//            cv::Mat roi_gray;
+//            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
+//            cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
+//            contour_area = cv::countNonZero(roi_gray);
+//            if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
+//                target_box = ArmorBox();
+//                return false;
+//            }
+//        }
+//        target_box.rect = pos;
+//        target_box.light_blobs.clear();
         target_box = ArmorBox();
         return false;
     }
-        }else{
-            cv::Mat roi_gray;
-            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
-            cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
-            contour_area = cv::countNonZero(roi_gray);
-            if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
-                target_box = ArmorBox();
-                return false;
-            }
-        }
-        target_box.rect = pos;
-        target_box.light_blobs.clear();
-    }
     return true;
 }

main.cpp

@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
             video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi");
+            video_gimbal = new VideoWrapper(PROJECT_DIR"/gimbal_video/0.avi");
-            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi");
+            video_chassis = new VideoWrapper(PROJECT_DIR"/gimbal_video/0.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");