Merge remote-tracking branch 'origin/anti-top-test' into anti-top-test

# Conflicts:
#	armor/src/armor_finder/anti_top/anti_top.cpp

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

@@ -96,19 +96,21 @@ void ArmorFinder::antiTop() {
                 getsystime(curr_time);
                 auto calculate_time = getTimeIntervalms(curr_time, frame_time);
                 shoot_delay = mean(top_periodms) - calculate_time;
-
+                sendBoxPosition(shoot_delay);
             } else {
                 if (++anti_top_cnt > 4) {
-                    anti_top_state == ANTI_TOP;
+                    anti_top_state = ANTI_TOP;
                     LOGM(STR_CTR(WORD_CYAN, "switch to anti-top"));
                 }
             }
         }
+        last_front_time = frame_time;
     }
     if (anti_top_state == NORMAL) {
         sendBoxPosition(0);
     } else if (interval < top_periodms[-1] * 0.1){
         sendBoxPosition(shoot_delay);
     }
+    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,17 +8,18 @@
 #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 {
-            last_box = target_box;
             anti_switch_cnt = 0;
             return true;
         }
@@ -28,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/send_target/send_target.cpp

@@ -44,7 +44,7 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     if (target_box.rect == cv::Rect2d()) return false;
     if (shoot_delay) {
-        LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay);
+        LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
     }
     auto rect = target_box.rect;
     double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;

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();
+//                target_box = ArmorBox();
-                return false;
+//                return false;
-            }
+//            }
-        }else{
+//        }else{
-            cv::Mat roi_gray;
+//            cv::Mat roi_gray;
-            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
+//            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
-            cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
+//            cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
-            contour_area = cv::countNonZero(roi_gray);
+//            contour_area = cv::countNonZero(roi_gray);
-            if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
+//            if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
-                target_box = ArmorBox();
+//                target_box = ArmorBox();
-                return false;
+//                return false;
-            }
+//            }
-        }
+//        }
-        target_box.rect = pos;
+//        target_box.rect = pos;
-        target_box.light_blobs.clear();
+//        target_box.light_blobs.clear();
+        target_box = ArmorBox();
+        return false;
     }
     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.");
@@ -178,7 +178,7 @@ int main(int argc, char *argv[]) {
                     armorFinder.run(gimbal_src);
                 });
             }
-//                cv::waitKey(0);
+//            cv::waitKey(0);
             });
         } while (ok);
         delete video_gimbal;