From 65a55353d62eef959fd5d439fddd2907e286c963 Mon Sep 17 00:00:00 2001 From: xinyang <895639507@qq.com> Date: Fri, 9 Aug 2019 01:24:58 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=8D=E9=99=80=E8=9E=BAv2.0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- armor/include/armor_finder/armor_finder.h | 17 ++--- armor/src/armor_finder/anti_top/anti_top.cpp | 73 +++++++++++--------- armor/src/armor_finder/armor_finder.cpp | 18 +++-- main.cpp | 8 +-- others/include/additions.h | 4 +- others/include/systime.h | 11 +-- others/src/additions.cpp | 4 -- others/src/systime.cpp | 22 ++++-- 8 files changed, 90 insertions(+), 67 deletions(-) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 789a578..600d7cc 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -93,7 +93,7 @@ typedef std::vector ArmorBoxes; /********************* 自瞄类定义 **********************/ class ArmorFinder{ public: - ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder); + ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, const uint8_t &anti_top); ~ArmorFinder() = default; private: @@ -103,24 +103,21 @@ private: SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE } State; // 自瞄状态枚举定义 - typedef enum{ - NORMAL, ANTI_TOP - } AntiTopState; - - systime frame_time; // 当前帧对应时间; + systime frame_time; // 当前帧对应时间 const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化 + const uint8_t &is_anti_top; // 进入反陀螺，引用外部变量，自动变化 State state; // 自瞄状态对象实例 ArmorBox target_box, last_box; // 目标装甲板 int anti_switch_cnt; // 防止乱切目标计数器 cv::Ptr tracker; // tracker对象实例 Classifier classifier; // CNN分类器对象实例，用于数字识别 - int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用） + int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用） int tracking_cnt; // 记录追踪帧数，用于定时退出追踪 Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量 + systime last_front_time; // 上次陀螺正对时间 RoundQueue top_periodms; // 陀螺周期循环队列 - systime last_front_time; // 上一次发生装甲板方向切换的时间 - int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺 - AntiTopState anti_top_state; // 当前是否识别到陀螺 + vector time_seq; // 一个周期内的时间采样点 + vector angle_seq; // 一个周期内的角度采样点 bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs); bool findArmorBox(const cv::Mat &src, ArmorBox &box); diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 0e730d6..f51a2ce 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -15,38 +15,47 @@ static double mean(RoundQueue &vec) { return sum / length; } -void ArmorFinder::antiTop() { - if (target_box.rect == cv::Rect2d()) return; - uint16_t shoot_delay = 0; - auto interval = getTimeIntervalms(frame_time, last_front_time); - if (anti_top_state == ANTI_TOP && interval > 700) { - anti_top_state = NORMAL; - LOGM(STR_CTR(WORD_YELLOW, "switch to normal")); +static systime getFrontTime(const vector time_seq, const vector angle_seq) { + double A = 0, B = 0, C = 0, D = 0; + int len = time_seq.size(); + for (int i = 0; i < len; i++) { + A += angle_seq[i] * angle_seq[i]; + B += angle_seq[i]; + C += angle_seq[i] * time_seq[i]; + D += time_seq[i]; + cout << "(" << angle_seq[i] << ", " << time_seq[i] << ") "; + } + double b = (A * D - B * C) / (len * A - B * B); + cout << b << endl; + return b; +} + +void ArmorFinder::antiTop() { + if (target_box.rect == cv::Rect2d()) return; + + if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) { + auto front_time = getFrontTime(time_seq, angle_seq); + auto once_periodms = getTimeIntervalms(front_time, last_front_time); +// if (abs(once_periodms - top_periodms[-1]) > 50) { +// sendBoxPosition(0); +// return; +// } + LOGM(STR_CTR(WORD_GREEN, "Top period: %.1lf"), once_periodms); + top_periodms.push(once_periodms); + auto periodms = mean(top_periodms); + systime curr_time; + getsystime(curr_time); + uint16_t shoot_delay = front_time + periodms * 2 - curr_time; + sendBoxPosition(shoot_delay); + time_seq.clear(); + angle_seq.clear(); + last_front_time = front_time; + } else { + time_seq.emplace_back(frame_time); + double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X; + double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; + angle_seq.emplace_back(yaw); + sendBoxPosition(0); } - if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) { - if (150 < interval && interval < 700) { - if (anti_top_state == ANTI_TOP) { - top_periodms.push(interval); - LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), interval); - systime curr_time; - getsystime(curr_time); - auto calculate_time = getTimeIntervalms(curr_time, frame_time); - shoot_delay = mean(top_periodms) - calculate_time; - sendBoxPosition(shoot_delay); - } else { - if (++anti_top_cnt > 4) { - anti_top_state = ANTI_TOP; - LOGM(STR_CTR(WORD_CYAN, "switch to anti-top")); - } - } - } - last_front_time = frame_time; - } - if (anti_top_state == NORMAL) { - sendBoxPosition(0); - } else if (interval < top_periodms[-1] * 0.1){ - sendBoxPosition(shoot_delay); - } - last_box = target_box; } diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 844d73e..43c48c7 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -46,13 +46,12 @@ std::map prior_red = { {"NO", 10}, }; -ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder) : +ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, const uint8_t &anti_top) : serial(u), enemy_color(color), + is_anti_top(anti_top), state(STANDBY_STATE), - anti_top_cnt(0), anti_switch_cnt(0), - anti_top_state(NORMAL), classifier(paras_folder), contour_area(0), tracking_cnt(0) { @@ -91,9 +90,18 @@ void ArmorFinder::run(cv::Mat &src) { stateStandBy(); } end: -// antiTop(); - if(target_box.rect != cv::Rect2d()) + if(is_anti_top) { + antiTop(); + }else if(target_box.rect != cv::Rect2d()) { + time_seq.clear(); + angle_seq.clear(); sendBoxPosition(0); + } + + if(target_box.rect != cv::Rect2d()){ + last_box = target_box; + } + if (show_armor_box) { // 根据条件显示当前目标装甲板 showArmorBox("box", src, target_box); cv::waitKey(1); diff --git a/main.cpp b/main.cpp index 42ca501..2abb49a 100644 --- a/main.cpp +++ b/main.cpp @@ -32,7 +32,7 @@ McuData mcu_data = { // 单片机端回传结构体 0, // 当前云台pitch角 ARMOR_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 - 1, // 是否启用数字识别 + 0, // 是否为反陀螺模式 ENEMY_RED, // 敌方颜色 0, // 能量机关x轴补偿量 0, // 能量机关y轴补偿量 @@ -44,7 +44,7 @@ WrapperHead *video_chassis = nullptr; // 底盘摄像头视频源 Serial serial(115200); // 串口对象 uint8_t last_state = INIT_STATE; // 上次状态，用于初始化 // 自瞄主程序对象 -ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/"); +ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top); // 能量机关主程序对象 Energy energy(serial, mcu_data.enemy_color); @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi"); - video_chassis = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi"); + video_gimbal = new VideoWrapper(PROJECT_DIR"/gimbal_video/1.avi"); + video_chassis = new VideoWrapper(PROJECT_DIR"/gimbal_video/1.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); diff --git a/others/include/additions.h b/others/include/additions.h index a6edadf..bd64658 100644 --- a/others/include/additions.h +++ b/others/include/additions.h @@ -15,7 +15,7 @@ struct McuData { float curr_pitch; uint8_t state; uint8_t mark; - uint8_t use_classifier; + uint8_t anti_top; uint8_t enemy_color; int delta_x; int delta_y; @@ -41,7 +41,7 @@ void extract(cv::Mat &gimbal_src, cv::Mat &chassis_src); void extract(cv::Mat &gimbal_src); -float getTimeIntervalms(const systime &now, const systime &last); +double getTimeIntervalms(const systime &now, const systime &last); double getPointLength(const cv::Point2f &p); diff --git a/others/include/systime.h b/others/include/systime.h index 6ed04a8..32dde36 100644 --- a/others/include/systime.h +++ b/others/include/systime.h @@ -5,12 +5,15 @@ #ifndef _PLATFORM_H_ #define _PLATFORM_H_ -typedef struct{ - int second; - int millisecond; -} systime; +//typedef struct{ +// float second; +// float millisecond; +//} systime; + +typedef double systime; void getsystime(systime &t); +double getTimeIntervalms(const systime &now, const systime &last); #if defined(Linux) || defined(Darwin) #include diff --git a/others/src/additions.cpp b/others/src/additions.cpp index cb543c3..da13f49 100644 --- a/others/src/additions.cpp +++ b/others/src/additions.cpp @@ -173,10 +173,6 @@ void extract(cv::Mat &gimbal_src) {//图像预处理，将视频切成640×480 } } -float getTimeIntervalms(const systime &now, const systime &last){ - return (now.second-last.second)*1000.0 + (now.millisecond-last.millisecond); -} - double getPointLength(const cv::Point2f &p) { return sqrt(p.x * p.x + p.y * p.y); } diff --git a/others/src/systime.cpp b/others/src/systime.cpp index 7a5e4ab..aa36b78 100644 --- a/others/src/systime.cpp +++ b/others/src/systime.cpp @@ -5,11 +5,17 @@ #if defined(Linux) || defined(Darwin) -void getsystime(systime &t){ +static systime getsystime(){ timeval tv; gettimeofday(&tv, nullptr); - t.second = tv.tv_sec; - t.millisecond = tv.tv_usec/1000; + return tv.tv_usec / 1000.0 + tv.tv_sec * 1000.0; +} + +void getsystime(systime &t) { + static systime time_base = getsystime(); + timeval tv; + gettimeofday(&tv, nullptr); + t = tv.tv_usec / 1000.0 + tv.tv_sec * 1000.0 - time_base; } #elif defined(Windows) @@ -17,10 +23,14 @@ void getsystime(systime &t){ void getsystime(systime &t){ SYSTEMTIME tv; GetLocalTime(&tv); - t.second = tv.wSecond; - t.millisecond = tv.wMilliseconds; + t = tv.wMilliseconds + tv.wSecond * 1000.0; } #else - #error "nonsupport platform." +#error "nonsupport platform." #endif + + +double getTimeIntervalms(const systime &now, const systime &last) { + return now - last; +}