From 86846625369501ef47f0925e9790a496b8e6a601 Mon Sep 17 00:00:00 2001 From: xinyang Date: Sun, 21 Jul 2019 17:25:09 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=8D=E9=99=80=E8=9E=BAv1.2,=E4=BB=A5?= =?UTF-8?q?=E5=8F=8A=E8=AF=86=E5=88=AB=E4=BC=98=E5=8C=96=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- armor/include/armor_finder/armor_finder.h | 4 +- armor/src/armor_finder/anti_top/anti_top.cpp | 63 +++++++------------ .../src/armor_finder/armor_box/armor_box.cpp | 4 +- armor/src/armor_finder/armor_finder.cpp | 6 +- .../src/armor_finder/find/find_armor_box.cpp | 10 +-- .../armor_finder/find/find_light_blobs.cpp | 41 ++++++++---- .../armor_finder/send_target/send_target.cpp | 29 ++++----- armor/src/show_images/show_images.cpp | 38 ++--------- 8 files changed, 83 insertions(+), 112 deletions(-) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index b17e19a..4d1b6ae 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -20,7 +20,7 @@ #define BOX_RED ENEMY_RED #define BOX_BLUE ENEMY_BLUE -#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel +#define DISTANCE_HEIGHT_5MM (107.0) // 单位: m*pixel #define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM #define B1 1 @@ -123,7 +123,7 @@ private: public: void run(cv::Mat &src); // 自瞄主函数 - bool sendBoxPosition(uint8_t shoot, bool isTrack); // 和主控板通讯 + bool sendBoxPosition(uint16_t shoot); // 和主控板通讯 }; #endif /* _ARMOR_FINDER_H_ */ diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index dafd697..feb044a 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -12,19 +12,16 @@ static double getTimeIntervalms(const timeval& now, const timeval &last){ void ArmorFinder::antiTop() { static double top_periodms = 0; static double last_top_periodms = 0; + uint16_t shoot_delay = 0; timeval curr_time; - uint8_t shoot = 0; - /*if(anti_top_state == ANTI_TOP){ - cout << "anti top" << endl; - }else if(anti_top_state == NORMAL){ - cout << "Normal" << endl; - }*/ +// if(anti_top_state == ANTI_TOP){ +// cout << "anti top" << endl; +// }else if(anti_top_state == NORMAL){ +// cout << "Normal" << endl; +// } gettimeofday(&curr_time, nullptr); auto interval = getTimeIntervalms(curr_time, last_front_time); - LOGM("interval:%.1lf", interval); - if(anti_top_state == ANTI_TOP && (top_periodms+last_top_periodms)/2.0-120 700){ + if(interval > 700){ anti_top_state = NORMAL; anti_top_cnt = 0; } @@ -32,50 +29,38 @@ void ArmorFinder::antiTop() { ArmorBox::BoxOrientation orientation = armor_box.getOrientation(); if(orientation == ArmorBox::UNKNOWN){ if(anti_top_state == NORMAL){ - sendBoxPosition(shoot, true); - return; - }else{ - sendBoxPosition(shoot, false); - return; + sendBoxPosition(shoot_delay); } + return; } - if(orientation != last_orient){ - if(interval > 700){ - anti_top_cnt = 0; - anti_top_state = NORMAL; - if(orientation == ArmorBox::FRONT){ - last_front_time = curr_time; - } - }else if(interval > 150){ - if(orientation == ArmorBox::FRONT){ - anti_top_cnt++; - if(anti_top_state == ANTI_TOP){ - last_top_periodms = top_periodms; - top_periodms = interval; - LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms); + if(orientation!=last_orient && orientation==ArmorBox::FRONT){ + last_front_time = curr_time; + if(150 4){ + anti_top_state = ANTI_TOP; } - last_front_time = curr_time; } } - last_orient = orientation; - } - if(anti_top_cnt > 4){ - anti_top_state = ANTI_TOP; } if(anti_top_state == ANTI_TOP){ if(orientation == ArmorBox::FRONT){ - sendBoxPosition(shoot, true); - }else if(shoot){ - sendBoxPosition(shoot, false); + sendBoxPosition(shoot_delay); } }else if(anti_top_state == NORMAL){ - sendBoxPosition(shoot, true); + sendBoxPosition(shoot_delay); } - 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 892fd43..d669d61 100644 --- a/armor/src/armor_finder/armor_box/armor_box.cpp +++ b/armor/src/armor_finder/armor_box/armor_box.cpp @@ -57,7 +57,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{ case B1: case B7: case B8: - if(lengthDistanceRatio() < 0.30){ + if(lengthDistanceRatio() < 0.24){ return FRONT; }else{ return SIDE; @@ -70,7 +70,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{ case B3: case B4: case B5: - if (lengthDistanceRatio() < 0.46) { + if (lengthDistanceRatio() < 0.48) { return FRONT; }else{ return SIDE; diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 89a0679..b25efeb 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -46,8 +46,8 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, } void ArmorFinder::run(cv::Mat &src) { -// stateSearchingTarget(src_use); // for debug -// return; +// stateSearchingTarget(src); // for debug +// goto end; switch (state) { case SEARCHING_STATE: if (stateSearchingTarget(src)) { @@ -77,7 +77,7 @@ void ArmorFinder::run(cv::Mat &src) { default: stateStandBy(); } - +end: antiTop(); if (show_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 690bff6..cb40a83 100644 --- a/armor/src/armor_finder/find/find_armor_box.cpp +++ b/armor/src/armor_finder/find/find_armor_box.cpp @@ -23,7 +23,7 @@ static bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blo light_blob_i.rect.angle - 90; float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : light_blob_j.rect.angle - 90; - return abs(angle_i - angle_j) < 10; + return abs(angle_i - angle_j) < 20; } static bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { @@ -35,12 +35,12 @@ static bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_bl double side_length; cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; side_length = sqrt(centers.ddot(centers)); - return (side_length / light_blob_i.length < 6 && side_length / light_blob_i.length > 0.5); + return (side_length / light_blob_i.length < 8 && side_length / light_blob_i.length > 0.5); } static bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { - return (light_blob_i.length / light_blob_j.length < 2 - && light_blob_i.length / light_blob_j.length > 0.5); + return (light_blob_i.length / light_blob_j.length < 2.5 + && light_blob_i.length / light_blob_j.length > 0.4); } /* 判断两个灯条的错位度，不知道英文是什么！！！ */ @@ -56,7 +56,7 @@ static bool CuoWeiDuJudge(const LightBlob &light_blob_i, const LightBlob &light_ Vector2f orientation(cos(angle), sin(angle)); Vector2f p2p(light_blob_j.rect.center.x - light_blob_i.rect.center.x, light_blob_j.rect.center.y - light_blob_i.rect.center.y); - return abs(orientation.dot(p2p)) < 20; + return abs(orientation.dot(p2p)) < 25; } static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &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 bebd371..460f1cf 100644 --- a/armor/src/armor_finder/find/find_light_blobs.cpp +++ b/armor/src/armor_finder/find/find_light_blobs.cpp @@ -20,8 +20,8 @@ static double areaRatio(const std::vector &contour, const cv::Rotated static bool isValidLightBlob(const std::vector &contour, const cv::RotatedRect &rect) { return (1.5 < lw_rate(rect) && lw_rate(rect) < 10) && // (rect.size.area() < 3000) && - ((rect.size.area() < 50 && areaRatio(contour, rect) > 0.5) || - (rect.size.area() >= 50 && areaRatio(contour, rect) > 0.7)); + ((rect.size.area() < 50 && areaRatio(contour, rect) > 0.4) || + (rect.size.area() >= 50 && areaRatio(contour, rect) > 0.6)); } // 此函数可以有性能优化. @@ -114,23 +114,40 @@ static void imagePreProcess(cv::Mat &src) { } bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) { - cv::Mat src_bin, color_channel; + cv::Mat color_channel; + cv::Mat src_bin_light, src_bin_dim; std::vector channels; // 通道拆分 cv::split(src, channels); /************************/ - if (enemy_color == ENEMY_BLUE) /* */ + if (enemy_color == ENEMY_BLUE){ /* */ color_channel = channels[0]; /* 根据目标颜色进行通道提取 */ - else if (enemy_color == ENEMY_RED) /* */ + }else if (enemy_color == ENEMY_RED){ /* */ color_channel = channels[2]; /************************/ - cv::threshold(color_channel, src_bin, 140, 255, CV_THRESH_BINARY); // 二值化对应通道 - imagePreProcess(src_bin); // 开闭运算 + } - if(src_bin.size() == cv::Size(640, 480) && show_light_blobs) - imshow("bin", src_bin); - std::vector > light_contours; - cv::findContours(src_bin, light_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); - for (auto &light_contour : light_contours) { + + cv::threshold(color_channel, src_bin_light, 200, 255, CV_THRESH_BINARY); // 二值化对应通道 + imagePreProcess(src_bin_light); // 开闭运算 + + cv::threshold(color_channel, src_bin_dim, 160, 255, CV_THRESH_BINARY); // 二值化对应通道 + imagePreProcess(src_bin_dim); // 开闭运算 + + if(src_bin_light.size() == cv::Size(640, 480) && show_light_blobs) { + imshow("bin_light", src_bin_light); + imshow("bin_dim", src_bin_dim); + } + + std::vector > light_contours_light, light_contours_dim; + cv::findContours(src_bin_light, light_contours_light, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); + cv::findContours(src_bin_dim, light_contours_dim, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); + for (auto &light_contour : light_contours_light) { + cv::RotatedRect rect = cv::minAreaRect(light_contour); + if (isValidLightBlob(light_contour, rect)) { + light_blobs.emplace_back(rect, get_blob_color(src, rect)); + } + } + for (auto &light_contour : light_contours_dim) { cv::RotatedRect rect = cv::minAreaRect(light_contour); if (isValidLightBlob(light_contour, rect)) { light_blobs.emplace_back(rect, get_blob_color(src, rect)); diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index b2b21c1..b2a2fa8 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -5,11 +5,11 @@ #include #include -static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t shoot) { +static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) { static short x_tmp, y_tmp, z_tmp; static time_t last_time = time(nullptr); static int fps; - uint8_t buff[9]; + uint8_t buff[10]; time_t t = time(nullptr); if (last_time != t) { @@ -30,14 +30,19 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t sho buff[4] = static_cast((y_tmp >> 0) & 0xFF); buff[5] = static_cast((z_tmp >> 8) & 0xFF); buff[6] = static_cast((z_tmp >> 0) & 0xFF); - buff[7] = shoot; - buff[8] = 'e'; - + buff[7] = static_cast((shoot_delay >> 8) & 0xFF); + buff[8] = static_cast((shoot_delay >> 0) & 0xFF); + buff[9] = 'e'; +// if(buff[7]<<8 | buff[8]) +// cout << (buff[7]<<8 | buff[8]) << endl; return serial.WriteData(buff, sizeof(buff)); } -bool ArmorFinder::sendBoxPosition(uint8_t shoot, bool isTrack) { +bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { if(armor_box.rect == cv::Rect2d()) return false; + if(shoot_delay){ + LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay); + } auto rect = armor_box.rect; double dx = rect.x + rect.width / 2 - 320; double dy = rect.y + rect.height / 2 - 240 - 20; @@ -45,15 +50,5 @@ bool ArmorFinder::sendBoxPosition(uint8_t shoot, bool isTrack) { double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; double dist = DISTANCE_HEIGHT / rect.height; // cout << yaw << endl; - if(isTrack){ - if(shoot){ - LOGM(STR_CTR(WORD_RED,"Shoot!!!")); - } - return sendTarget(serial, yaw, -pitch, dist, shoot); - LOGM(STR_CTR(WORD_RED,"Shoot!!!")); - return sendTarget(serial, 0, -0, dist, 1); - }else if(shoot){ - LOGM(STR_CTR(WORD_RED,"Shoot!!!")); - return sendTarget(serial, 0, 0, 4, 1); - } + return sendTarget(serial, yaw, -pitch, dist, shoot_delay); } diff --git a/armor/src/show_images/show_images.cpp b/armor/src/show_images/show_images.cpp index fe46a24..b48e870 100644 --- a/armor/src/show_images/show_images.cpp +++ b/armor/src/show_images/show_images.cpp @@ -95,38 +95,12 @@ 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; - 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); -// }; + + if(box.getOrientation() == ArmorBox::FRONT){ + 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", box.getDistance());