diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 814abb2..bfbd5b7 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -30,7 +30,7 @@ private: typedef cv::TrackerKCF TrackerToUse; // Tracker类型定义 typedef enum{ - SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE + WEAKSEARCHING_STATE, SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE } State; // 自瞄状态枚举定义 const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化 @@ -40,9 +40,13 @@ private: cv::Ptr tracker; // tracker对象实例 Classifier classifier; // CNN分类器对象实例，用于数字识别 int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用） + int tracking_cnt; // 记录追踪帧数，用于定时退出追踪 + int miss_cnt; // + int weak_cnt; // Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量 const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化 + bool stateSearchingTarget(cv::Mat &src); // searching state主函数 bool stateTrackingTarget(cv::Mat &src); // tracking state主函数 bool stateStandBy(); // stand by state主函数（已弃用） diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 7ac97fe..a5ee01f 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -8,17 +8,17 @@ #include #include -std::map id2name = { +std::map id2name = { //装甲板id到名称的map {-1, "OO"},{ 0, "NO"}, { 1, "B1"},{ 2, "B2"},{ 3, "B3"},{ 4, "B4"},{ 5, "B5"},{ 6, "B7"},{ 7, "B8"}, { 8, "R1"},{ 9, "R2"},{10, "R3"},{11, "R4"},{12, "R5"},{13, "R7"},{14, "R8"}, -}; //装甲板id到名称的map +}; -std::map name2id = { +std::map name2id = { //装甲板名称到id的map {"OO", -1},{"NO", 0}, {"B1", 1},{"B2", 2},{"B3", 3},{"B4", 4},{"B5", 5},{"B7", 6},{"B8", 7}, {"R1", 8},{"R2", 9},{"R3", 10},{"R4", 11},{"R5", 12},{"R7", 13},{"R8", 14}, -}; //装甲板名称到id的map +}; ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, const uint8_t &use) : serial(u), @@ -27,23 +27,33 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, classifier(paras_folder), contour_area(0), use_classifier(use), - boxid(-1) + boxid(-1), + tracking_cnt(0), + miss_cnt(0), + weak_cnt(0) { } void ArmorFinder::run(cv::Mat &src) { - static int tracking_cnt = 0; // 记录追踪帧数，用于定时退出追踪 cv::Mat src_use = src.clone(); // 实际参与计算的图像对象 if(show_armor_box){ // 根据条件显示当前目标装甲板 showArmorBox("box", src, armor_box, boxid); cv::waitKey(1); } -// stateSearchingTarget(src_use); +// stateSearchingTarget(src_use); // for debug // return; switch (state){ + case WEAKSEARCHING_STATE: + if(stateSearchingTarget(src_use) && ++weak_cnt>5){ + miss_cnt = 0; + state = SEARCHING_STATE; + }else{ + weak_cnt = 0; + } case SEARCHING_STATE: if(stateSearchingTarget(src_use)){ + miss_cnt = 0; if((armor_box & cv::Rect2d(0, 0, 640, 480)) == armor_box) { // 判断装甲板区域是否脱离图像区域 if(!classifier || !use_classifier){ /* 如果分类器不可用或者不使用分类器 */ cv::Mat roi = src_use.clone()(armor_box), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */ @@ -57,10 +67,14 @@ void ArmorFinder::run(cv::Mat &src) { tracking_cnt = 0; LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track")); } + }else if(++miss_cnt>100){ + weak_cnt = 0; + state = WEAKSEARCHING_STATE; } break; case TRACKING_STATE: - if(++tracking_cnt>100 || !stateTrackingTarget(src_use)){ // 最多追踪100帧图像 + if(!stateTrackingTarget(src_use) || ++tracking_cnt>100){ // 最多追踪100帧图像 + miss_cnt = 0; state = SEARCHING_STATE; LOGM(STR_CTR(WORD_LIGHT_YELLOW ,"into search!")); } @@ -71,10 +85,6 @@ void ArmorFinder::run(cv::Mat &src) { } } -#define FOCUS_PIXAL_8MM (1488) -#define FOCUS_PIXAL_5MM (917) -#define FOCUS_PIXAL FOCUS_PIXAL_5MM - bool sendTarget(Serial& serial, double x, double y, double z) { static short x_tmp, y_tmp, z_tmp; static time_t last_time = time(nullptr); diff --git a/armor/src/armor_finder/searching_state/searching_state.cpp b/armor/src/armor_finder/searching_state/searching_state.cpp index 9c10a5a..6b692d0 100644 --- a/armor/src/armor_finder/searching_state/searching_state.cpp +++ b/armor/src/armor_finder/searching_state/searching_state.cpp @@ -291,7 +291,7 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { showArmorBoxVector("boxes", src, armor_boxes); cv::waitKey(1); } - if (classifier && use_classifier) { + if (classifier && use_classifier && state!=WEAKSEARCHING_STATE) { for (auto box : armor_boxes) { cv::Mat roi = src(box).clone(); cv::resize(roi, roi, cv::Size(48, 36)); diff --git a/energy/include/energy/constant.h b/energy/include/energy/constant.h index 2dd2cc7..b8a6660 100644 --- a/energy/include/energy/constant.h +++ b/energy/include/energy/constant.h @@ -6,10 +6,8 @@ #include "additions/additions.h" -#define d2r (CV_PI / 180.0) +//#define d2r (CV_PI / 180.0) -const int ALLY_BLUE = ENEMY_RED; -const int ALLY_RED = ENEMY_BLUE; const int SRC_WIDTH_CAMERA = 640; const int SRC_HEIGHT_CAMERA = 480; const int SRC_WIDTH = 320; diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index aeb2af7..82d501a 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -8,10 +8,6 @@ using namespace std; using namespace cv; -#define FOCUS_PIXAL_5MM (917) -#define FOCUS_PIXAL FOCUS_PIXAL_5MM - - //---------------------------------------------------------------------------------------------------------------------- // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 // --------------------------------------------------------------------------------------------------------------------- diff --git a/energy/src/energy/tool/tool.cpp b/energy/src/energy/tool/tool.cpp index ffd3bbb..601562c 100644 --- a/energy/src/energy/tool/tool.cpp +++ b/energy/src/energy/tool/tool.cpp @@ -8,56 +8,56 @@ #include #include #include + using namespace cv; using std::cout; using std::endl; using std::vector; - //---------------------------------------------------------------------------------------------------------------------- // 此函数选取图像中的一部分进行处理 // --------------------------------------------------------------------------------------------------------------------- -void Energy::extract(cv::Mat &src){ - cv::Rect rect(EXTRACT_POINT_X, EXTRACT_POINT_Y, EXTRACT_WIDTH, EXTRACT_HEIGHT); - src = src(rect).clone(); - cv::resize(src, src, cv::Size(640, 480), 2); - imshow("extract", src); +void Energy::extract(cv::Mat &src) { + cv::Rect rect(EXTRACT_POINT_X, EXTRACT_POINT_Y, EXTRACT_WIDTH, EXTRACT_HEIGHT); + src = src(rect).clone(); + cv::resize(src, src, cv::Size(640, 480), 2); + imshow("extract", src); } - //---------------------------------------------------------------------------------------------------------------------- // 此函数用于计算预测的击打点坐标 // --------------------------------------------------------------------------------------------------------------------- void Energy::rotate() { - int x1, x2, y1, y2; - // 为了减小强制转换的误差 - x1 = circle_center_point.x * 100; - x2 = target_point.x * 100; - y1 = circle_center_point.y * 100; - y2 = target_point.y * 100; + int x1, x2, y1, y2; + // 为了减小强制转换的误差 + x1 = circle_center_point.x * 100; + x2 = target_point.x * 100; + y1 = circle_center_point.y * 100; + y2 = target_point.y * 100; - predict_point.x = static_cast((x1 + (x2 - x1)*cos(-predict_rad * d2r) - (y1 - y2)*sin(-predict_rad * d2r))/100); - predict_point.y = static_cast((y1 - (x2 - x1)*sin(-predict_rad * d2r) - (y1 - y2)*cos(-predict_rad * d2r))/100); + predict_point.x = static_cast( + (x1 + (x2 - x1) * cos(-predict_rad * PI / 180.0) - (y1 - y2) * sin(-predict_rad * PI / 180.0)) / 100); + predict_point.y = static_cast( + (y1 - (x2 - x1) * sin(-predict_rad * PI / 180.0) - (y1 - y2) * cos(-predict_rad * PI / 180.0)) / 100); } - //---------------------------------------------------------------------------------------------------------------------- // 此函数将像素差转换到实际距离差 // --------------------------------------------------------------------------------------------------------------------- -void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){ - if(point_1==circle_center_point){ +void Energy::stretch(cv::Point point_1, cv::Point2f &point_2) { + if (point_1 == circle_center_point) { // cout<<"stretch wrong!"<( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0); - point_2.y = static_cast( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0); + point_2.x = static_cast( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0); + point_2.y = static_cast( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0); } @@ -65,10 +65,10 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){ // 此函数用于计算两点距离 // --------------------------------------------------------------------------------------------------------------------- -double Energy::pointDistance(cv::Point point_1, cv::Point point_2){ +double Energy::pointDistance(cv::Point point_1, cv::Point point_2) { double distance = 0; - distance = sqrt(pow(static_cast(point_1.x - point_2.x),2) - + pow(static_cast(point_1.y - point_2.y),2)); + distance = sqrt(pow(static_cast(point_1.x - point_2.x), 2) + + pow(static_cast(point_1.y - point_2.y), 2)); return distance; } diff --git a/others/include/constants.h b/others/include/constants.h index 3514a83..34290c9 100644 --- a/others/include/constants.h +++ b/others/include/constants.h @@ -10,8 +10,15 @@ #define ENEMY_BLUE 0 #define ENEMY_RED 1 +#define ALLY_BLUE ENEMY_RED +#define ALLY_RED ENEMY_BLUE + #define BIG_ENERGY_STATE 'b' #define SMALL_ENERGY_STATE 's' #define ARMOR_STATE 'a' +#define FOCUS_PIXAL_8MM (1488) +#define FOCUS_PIXAL_5MM (917) +#define FOCUS_PIXAL FOCUS_PIXAL_5MM + #endif /* _CONSTANTS_H */