VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

反陀螺v2.0

Browse Source
This commit is contained in:
xinyang
2019-08-09 01:24:58 +08:00
parent faae07b5b3
commit 65a55353d6
8 changed files with 90 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
armor/include/armor_finder/armor_finder.h
View File

@@ -93,7 +93,7 @@ typedef std::vector<ArmorBox> ArmorBoxes;
/********************* 自瞄类定义 **********************/
class ArmorFinder{
public:
ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder);
ArmorFinder(uint8_t &color, Serial &u, const string &paras_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<cv::Tracker> tracker; // tracker对象实例
Classifier classifier; // CNN分类器对象实例，用于数字识别
int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
int tracking_cnt; // 记录追踪帧数，用于定时退出追踪
Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量
systime last_front_time; // 上次陀螺正对时间
RoundQueue<double, 4> top_periodms; // 陀螺周期循环队列
systime last_front_time; // 上一次发生装甲板方向切换的时间
int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺
AntiTopState anti_top_state; // 当前是否识别到陀螺
vector<systime> time_seq; // 一个周期内的时间采样点
vector<float> angle_seq; // 一个周期内的角度采样点
bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
bool findArmorBox(const cv::Mat &src, ArmorBox &box);

73
armor/src/armor_finder/anti_top/anti_top.cpp
View File

@@ -15,38 +15,47 @@ static double mean(RoundQueue<double, length> &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<systime> time_seq, const vector<float> 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;
}

18
armor/src/armor_finder/armor_finder.cpp
View File

@@ -46,13 +46,12 @@ std::map<string, int> prior_red = {
{"NO", 10},
};
ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder) :
ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_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);

8
main.cpp
View File

@@ -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.");

4
others/include/additions.h
View File

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

11
others/include/systime.h
View File

@@ -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 <sys/time.h>

4
others/src/additions.cpp
View File

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

22
others/src/systime.cpp
View File

@@ -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;
}