merge

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<cv::Tracker> 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主函数（已弃用）

armor/src/armor_finder/armor_finder.cpp

@@ -8,17 +8,17 @@
 #include <opencv2/highgui.hpp>
 #include <armor_finder/armor_finder.h>
 
-std::map<int, string> id2name = {
+std::map<int, string> 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<string, int> name2id = {
+std::map<string, int> 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 &paras_folder, const uint8_t &use) :
             serial(u),
@@ -27,23 +27,33 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_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);

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));

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;

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
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
 // ---------------------------------------------------------------------------------------------------------------------

energy/src/energy/tool/tool.cpp

@@ -8,13 +8,13 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+
 using namespace cv;
 using std::cout;
 using std::endl;
 using std::vector;
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数选取图像中的一部分进行处理
 // ---------------------------------------------------------------------------------------------------------------------
@@ -26,7 +26,6 @@ void Energy::extract(cv::Mat &src){
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于计算预测的击打点坐标
 // ---------------------------------------------------------------------------------------------------------------------
@@ -38,12 +37,13 @@ void Energy::rotate() {
     y1 = circle_center_point.y * 100;
     y2 = target_point.y * 100;
 
-	predict_point.x = static_cast<int>((x1 + (x2 - x1)*cos(-predict_rad * d2r) - (y1 - y2)*sin(-predict_rad * d2r))/100);
+    predict_point.x = static_cast<int>(
-	predict_point.y = static_cast<int>((y1 - (x2 - x1)*sin(-predict_rad * d2r) - (y1 - y2)*cos(-predict_rad * d2r))/100);
+            (x1 + (x2 - x1) * cos(-predict_rad * PI / 180.0) - (y1 - y2) * sin(-predict_rad * PI / 180.0)) / 100);
+    predict_point.y = static_cast<int>(
+            (y1 - (x2 - x1) * sin(-predict_rad * PI / 180.0) - (y1 - y2) * cos(-predict_rad * PI / 180.0)) / 100);
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数将像素差转换到实际距离差
 // ---------------------------------------------------------------------------------------------------------------------

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 */