VSAG/vision_sjtu_19
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);