diff --git a/CMakeLists.txt b/CMakeLists.txt index e19eb1d..8325866 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ PROJECT(SJTU-RM-CV) SET(CMAKE_CXX_STANDARD 11) -SET(CMAKE_BUILD_TYPE RELEASE) +SET(CMAKE_BUILD_TYPE DEBUG) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}") diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 8387b76..c49cffe 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -6,6 +6,7 @@ #define _ARMOR_FINDER_H_ #include +#include #include #include #include @@ -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 id2name; //装甲板id到名称的map extern std::map name2id; //装甲板名称到id的map @@ -45,6 +61,10 @@ typedef std::vector 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 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); // 自瞄主函数 diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index ab5a621..8e593dc 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -4,49 +4,56 @@ #include -#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 - - -void ArmorFinder::anti_top() { -// switch (armor_box.id) { -// case R1: -// case R7: -// case B1: -// case B7: -// if (armor_box.rect != cv::Rect2d()) { -// sendBoxPosition(); -// } -// case R2: -// case R3: -// case R4: -// case R5: -// case B2: -// case B3: -// case B4: -// case B5: -// if (armor_box.rect != cv::Rect2d()) { -// sendBoxPosition(); -// } -// default: -// if (armor_box.rect != cv::Rect2d()) { -// sendBoxPosition(); -// } -// } - if (armor_box.rect != cv::Rect2d()) { - sendBoxPosition(); - } +static double getTimeIntervalms(const timeval& now, const timeval &last){ + return (now.tv_sec-last.tv_sec)*1000.0 + (now.tv_usec-last.tv_usec)/1000.0; +} + +void ArmorFinder::antiTop() { + if(anti_top_state == ANTI_TOP){ + cout << "anti top" << endl; + }else if(anti_top_state == NORMAL){ + cout << "Normal" << endl; + } + ArmorBox::BoxOrientation orientation = armor_box.getOrientation(); + if(orientation == ArmorBox::UNKNOWN){ + if(anti_top_state == NORMAL){ + sendBoxPosition(); + return; + }else{ + return; + } + } + if(orientation!=last_orient){ + timeval curr_time; + gettimeofday(&curr_time, nullptr); + auto interval = getTimeIntervalms(curr_time, last_switch_time); + cout << interval << endl; + if(50 < interval && interval < 700){ + anti_top_cnt++; + }else{ + anti_top_cnt = 0; + } + last_switch_time = curr_time; + }else{ + timeval curr_time; + gettimeofday(&curr_time, nullptr); + if(getTimeIntervalms(curr_time, last_switch_time) > 700) { + anti_top_state = NORMAL; + } + } + if(anti_top_cnt > 4){ + anti_top_state = ANTI_TOP; + } + + if(anti_top_state == ANTI_TOP){ + if(orientation == ArmorBox::FRONT){ + sendBoxPosition(); + } + }else if(anti_top_state == NORMAL){ + sendBoxPosition(); + } + + + last_orient = orientation; } diff --git a/armor/src/armor_finder/armor_box/armor_box.cpp b/armor/src/armor_finder/armor_box/armor_box.cpp index 65e35dd..521a26e 100644 --- a/armor/src/armor_finder/armor_box/armor_box.cpp +++ b/armor/src/armor_finder/armor_box/armor_box.cpp @@ -36,3 +36,44 @@ double ArmorBox::lengthDistanceRatio() const { return 100; } } + +double ArmorBox::getDistance() const{ + if(light_blobs.size() == 2 ){ + return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length); + } else { + return DISTANCE_HEIGHT / rect.height; + } +} + +ArmorBox::BoxOrientation ArmorBox::getOrientation() const{ +// cout << lengthDistanceRatio() << endl; + if(light_blobs.size() != 2){ + return UNKNOWN; + } + switch (id) { + case R1: + case R7: + case B1: + case B7: + if(lengthDistanceRatio() < 0.3){ + return FRONT; + }else{ + return SIDE; + } + case R2: + case R3: + case R4: + case R5: + case B2: + case B3: + case B4: + case B5: + if (lengthDistanceRatio() < 0.43) { + return FRONT; + }else{ + return SIDE; + } + default: + return UNKNOWN; + } +} diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index a724763..89a0679 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -78,7 +78,7 @@ void ArmorFinder::run(cv::Mat &src) { stateStandBy(); } - anti_top(); + antiTop(); if (show_armor_box) { // 根据条件显示当前目标装甲板 showArmorBox("box", src, armor_box); diff --git a/armor/src/armor_finder/find/find_armor_box.cpp b/armor/src/armor_finder/find/find_armor_box.cpp index 17776c3..690bff6 100644 --- a/armor/src/armor_finder/find/find_armor_box.cpp +++ b/armor/src/armor_finder/find/find_armor_box.cpp @@ -72,20 +72,11 @@ static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_ } static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) { -// cout << (light_blob_i.blob_color==BLOB_RED) << endl; -// cout << (light_blob_j.blob_color==BLOB_RED) << endl; -// cout << lengthRatioJudge(light_blob_i, light_blob_j) << endl; -// cout << lengthJudge(light_blob_i, light_blob_j) << endl; -//// cout << heightJudge(light_blob_i, light_blob_j) << endl; -// cout << angelJudge(light_blob_i, light_blob_j) << endl; -// cout << boxAngleJudge(light_blob_i, light_blob_j) << endl; -// cout << CuoWeiDuJudge(light_blob_i, light_blob_j) << endl; -// cout << "=============" << endl; return light_blob_i.blob_color == enemy_color && light_blob_j.blob_color == enemy_color && lengthRatioJudge(light_blob_i, light_blob_j) && lengthJudge(light_blob_i, light_blob_j) && - // heightJudge(light_blob_i, light_blob_j) && +// heightJudge(light_blob_i, light_blob_j) && angelJudge(light_blob_i, light_blob_j) && boxAngleJudge(light_blob_i, light_blob_j) && CuoWeiDuJudge(light_blob_i, light_blob_j); diff --git a/armor/src/armor_finder/find/find_light_blobs.cpp b/armor/src/armor_finder/find/find_light_blobs.cpp index 2a3f75a..af6ad38 100644 --- a/armor/src/armor_finder/find/find_light_blobs.cpp +++ b/armor/src/armor_finder/find/find_light_blobs.cpp @@ -121,7 +121,7 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) { color_channel = channels[0]; /* 根据目标颜色进行通道提取 */ else if (enemy_color == ENEMY_RED) /* */ color_channel = channels[2]; /************************/ - cv::threshold(color_channel, src_bin, 160, 255, CV_THRESH_BINARY); // 二值化对应通道 + cv::threshold(color_channel, src_bin, 170, 255, CV_THRESH_BINARY); // 二值化对应通道 imagePreProcess(src_bin); // 开闭运算 if(src_bin.size() == cv::Size(640, 480)) diff --git a/armor/src/show_images/show_images.cpp b/armor/src/show_images/show_images.cpp index 0459690..fe46a24 100644 --- a/armor/src/show_images/show_images.cpp +++ b/armor/src/show_images/show_images.cpp @@ -95,14 +95,41 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox & // if(box.light_blobs.size() == 2) // fprintf(fp, "%lf %lf %lf\n", box.light_blobs[0].length, box.light_blobs[1].length, box.blobsDistance()) // cout << box.lengthDistanceRatio() << endl; - if(box.lengthDistanceRatio() < 0.4 && box.lengthRatio() > 0.9){ - rectangle(image2show, box.rect, Scalar(0, 255, 0), 3); - }else{ - rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); - }; + switch (box.id) { + case R1: + case R7: + case B1: + case B7: + if(box.lengthDistanceRatio() < 0.3){ + rectangle(image2show, box.rect, Scalar(0, 255, 0), 3); + }else{ + rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); + } + case R2: + case R3: + case R4: + case R5: + case B2: + case B3: + case B4: + case B5: + if (box.lengthDistanceRatio() < 0.42) { + rectangle(image2show, box.rect, Scalar(0, 255, 0), 3); + }else{ + rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); + } + break; + default: + rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); + } +// if((id2name.at(box.id)=="B1"||id2name.at(box.id)=="B7") && box.lengthDistanceRatio() < 0.3/* && box.lengthRatio() > 0.9*/){ +// rectangle(image2show, box.rect, Scalar(0, 255, 0), 3); +// }else{ +// rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); +// }; char dist[5]; -// sprintf(dist, "%.1f", distance); + sprintf(dist, "%.1f", box.getDistance()); if (box.id == -1) putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0, 255, 0)); diff --git a/main.cpp b/main.cpp index ca68173..c5f4e0a 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 + ARMOR_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 diff --git a/others/include/camera/camera_wrapper.h b/others/include/camera/camera_wrapper.h index 37c6fa7..af8c2c5 100644 --- a/others/include/camera/camera_wrapper.h +++ b/others/include/camera/camera_wrapper.h @@ -48,6 +48,7 @@ public: bool readRaw(cv::Mat& src); bool readProcessed(cv::Mat& src); bool changeBrightness(int brightness); +// bool once }; diff --git a/others/src/camera/camera_wrapper.cpp b/others/src/camera/camera_wrapper.cpp index 901e6a6..c4917d7 100644 --- a/others/src/camera/camera_wrapper.cpp +++ b/others/src/camera/camera_wrapper.cpp @@ -78,10 +78,12 @@ bool CameraWrapper::init() { CameraSetAeState(h_camera, false); CameraSetExposureTime(h_camera, CAMERA_EXPOSURE*1000); CameraSetAnalogGain(h_camera, ARMOR_CAMERA_GAIN); + CameraSetWbMode(h_camera, false); if(mode == 0){ CameraSetGain(h_camera, CAMERA_BLUE_GAIN, CAMERA_GREEN_GAIN, CAMERA_RED_GAIN); CameraSetLutMode(h_camera, LUTMODE_PRESET); } + CameraSetOnceWB(h_camera); #endif double t; CameraGetExposureTime(h_camera, &t);