反陀螺v1.0完成。

armor/include/armor_finder/armor_finder.h

@@ -6,6 +6,7 @@
 #define _ARMOR_FINDER_H_
 
 #include <map>
+#include <sys/time.h>
 #include <constants.h>
 #include <opencv2/core.hpp>
 #include <opencv2/tracking.hpp>
@@ -22,6 +23,21 @@
 #define DISTANCE_HEIGHT_5MM (113.0)     // 单位: m*pixel
 #define DISTANCE_HEIGHT     DISTANCE_HEIGHT_5MM
 
+#define         B1 1
+#define         B2 2
+#define         B3 3
+#define         B4 4
+#define         B5 5
+#define         B7 6
+#define         B8 7
+#define         R1 8
+#define         R2 9
+#define         R3 10
+#define         R4 11
+#define         R5 12
+#define         R7 13
+#define         R8 14
+
 extern std::map<int, string> id2name;   //装甲板id到名称的map
 extern std::map<string, int> name2id;   //装甲板名称到id的map
 
@@ -45,6 +61,10 @@ typedef std::vector<LightBlob> LightBlobs;
 /******************* 装甲板类定义　**********************/
 class ArmorBox{
 public:
+    typedef enum{
+        FRONT, SIDE, UNKNOWN
+    } BoxOrientation;
+
     cv::Rect2d rect;
     LightBlobs light_blobs;
     uint8_t box_color;
@@ -55,6 +75,8 @@ public:
     double blobsDistance() const;
     double lengthRatio() const;
     double lengthDistanceRatio() const;
+    double getDistance() const;
+    BoxOrientation getOrientation() const;
 };
 
 typedef std::vector<ArmorBox> ArmorBoxes;
@@ -72,6 +94,10 @@ private:
         SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
     } State;                                            // 自瞄状态枚举定义
 
+    typedef enum{
+        NORMAL, ANTI_TOP
+    } AntiTopState;
+
     const uint8_t &enemy_color;                         // 敌方颜色，引用外部变量，自动变化
     State state;                                        // 自瞄状态对象实例
     ArmorBox armor_box;                                 // 当前目标装甲板
@@ -81,6 +107,10 @@ private:
     int tracking_cnt;                                   // 记录追踪帧数，用于定时退出追踪
     Serial &serial;                                     // 串口对象，引用外部变量，用于和能量机关共享同一个变量
     const uint8_t &use_classifier;                      // 标记是否启用CNN分类器，引用外部变量，自动变化
+    ArmorBox::BoxOrientation last_orient;               // 上一帧目标装甲板方向，用于反陀螺
+    timeval last_switch_time;                           // 上一次发生装甲板方向切换的时间
+    int anti_top_cnt;                                   // 满足条件的装甲板方向切换持续次数，用于反陀螺
+    AntiTopState anti_top_state;                        // 当前是否识别到陀螺
 
     bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
     bool findArmorBox(const cv::Mat &src, ArmorBox &box);
@@ -89,7 +119,7 @@ private:
     bool stateTrackingTarget(cv::Mat &src);             // tracking state主函数
     bool stateStandBy();                                // stand by state主函数（已弃用）
 
-    void anti_top();                                    // 反小陀螺
+    void antiTop();                                    // 反小陀螺
 
 public:
     void run(cv::Mat &src);                             // 自瞄主函数