diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 2a4ad28..eb49254 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -124,9 +124,7 @@ private: Serial &serial; // 串口对象,引用外部变量,用于和能量机关共享同一个变量 const uint8_t &use_classifier; // 标记是否启用CNN分类器,引用外部变量,自动变化 RoundQueue top_periodms; // 陀螺周期循环队列 - RoundQueue box_ratioes; // systime last_front_time; // 上一次发生装甲板方向切换的时间 - BoxRatioChangeType last_ratio_type; // int anti_top_cnt; // 满足条件的装甲板方向切换持续次数,用于反陀螺 AntiTopState anti_top_state; // 当前是否识别到陀螺 diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 8bc90b9..e0bdd88 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/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; } diff --git a/armor/src/armor_finder/find/find_armor_box.cpp b/armor/src/armor_finder/find/find_armor_box.cpp index 3a4b053..c4a5194 100644 --- a/armor/src/armor_finder/find/find_armor_box.cpp +++ b/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){ - box = armor_boxes[0]; + for(auto &one_box : armor_boxes){ + if(one_box.id != 0){ + box = one_box; + } } if (save_labelled_boxes) { for (const auto &one_box : armor_boxes) { diff --git a/armor/src/armor_finder/searching_state/searching_state.cpp b/armor/src/armor_finder/searching_state/searching_state.cpp index be99312..e7b6195 100644 --- a/armor/src/armor_finder/searching_state/searching_state.cpp +++ b/armor/src/armor_finder/searching_state/searching_state.cpp @@ -8,17 +8,18 @@ #include bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { - if(anti_switch_cnt >= 3){ - last_box = ArmorBox(); - } +// if(anti_switch_cnt >= 3){ +// 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; + } +} +*/ \ No newline at end of file diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 06f53f2..8e45ad0 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/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; diff --git a/armor/src/armor_finder/tracking_state/tracking_state.cpp b/armor/src/armor_finder/tracking_state/tracking_state.cpp index cd9a9b7..53689dc 100644 --- a/armor/src/armor_finder/tracking_state/tracking_state.cpp +++ b/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(); - if(classifier){ - cv::resize(roi, roi, cv::Size(48, 36)); - 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(); +// roi = src(pos).clone(); +// if(classifier){ +// cv::resize(roi, roi, cv::Size(48, 36)); +// 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; } return true; } diff --git a/main.cpp b/main.cpp index 7d6a681..bac5761 100644 --- a/main.cpp +++ b/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_chassis = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi"); + video_gimbal = new VideoWrapper(PROJECT_DIR"/gimbal_video/0.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;