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

陀螺识别方式改变,参数更新。

Browse Source
This commit is contained in:
xinyang
2019-07-29 16:26:29 +08:00
parent 550d51049d
commit 2d399cc567
18 changed files with 17906 additions and 17919 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -107,6 +107,11 @@ private:
NORMAL, ANTI_TOP
} AntiTopState;
typedef enum{
INCREASE, DECREASE, NOCHANGE
} BoxRatioChangeType;
timeval frame_time; // 当前帧对应时间;
const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化
State state; // 自瞄状态对象实例
ArmorBox armor_box; // 当前目标装甲板
@@ -116,8 +121,10 @@ private:
int tracking_cnt; // 记录追踪帧数，用于定时退出追踪
Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量
const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化
ArmorBox::BoxOrientation last_orient; // 上一帧目标装甲板方向，用于反陀螺
timeval last_front_time; // 上一次发生装甲板方向切换的时间
RoundQueue<double, 4> top_periodms;
RoundQueue<double, 5> box_ratioes;
timeval last_front_time; // 上一次发生装甲板方向切换的时间
BoxRatioChangeType last_ratio_type;
int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺
AntiTopState anti_top_state; // 当前是否识别到陀螺
@@ -129,6 +136,7 @@ private:
bool stateStandBy(); // stand by state主函数（已弃用）
void antiTop(); // 反小陀螺
BoxRatioChangeType getRatioChangeType(RoundQueue<double, 5> &vec);
public:
void run(cv::Mat &src); // 自瞄主函数

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

@@ -6,66 +6,74 @@
#include <additions/additions.h>
#include <log.h>
static double boxDistance(const cv::Rect2d &a, const cv::Rect2d &b){
cv::Point2d centerA(a.x+a.width/2, a.y+a.height/2);
cv::Point2d centerB(b.x+b.width/2, b.y+b.height/2);
auto dist = centerA-centerB;
return sqrt(dist.x*dist.x + dist.y*dist.y);
static double boxDistance(const cv::Rect2d &a, const cv::Rect2d &b) {
cv::Point2d centerA(a.x + a.width / 2, a.y + a.height / 2);
cv::Point2d centerB(b.x + b.width / 2, b.y + b.height / 2);
auto dist = centerA - centerB;
return sqrt(dist.x * dist.x + dist.y * dist.y);
}
template <int length>
static double mean(RoundQueue<double, length> &vec) {
double sum = 0;
for (int i = 0; i < vec.size(); i++) {
sum += vec[i];
}
return sum / length;
}
ArmorFinder::BoxRatioChangeType ArmorFinder::getRatioChangeType(RoundQueue<double, 5> &vec) {
auto d = (vec[0] - vec[1] + vec[3] + vec[4]) / 3.0;
if (d > 0.1) {
return INCREASE;
} else if (d < -0.1) {
return DECREASE;
} else {
return NOCHANGE;
}
}
void ArmorFinder::antiTop() {
static double top_periodms = 0;
static double last_top_periodms = 0;
static cv::Rect2d last_pos;
if (armor_box.rect == cv::Rect2d()) return;
uint16_t shoot_delay = 0;
timeval curr_time;
// 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);
if(interval > 700){
anti_top_state = NORMAL;
auto interval = getTimeIntervalms(frame_time, last_front_time);
box_ratioes.push(armor_box.rect.width / armor_box.rect.height);
auto change_type = getRatioChangeType(box_ratioes);
auto orientation = armor_box.getOrientation();
if (interval > 700) {
anti_top_cnt = 0;
}
ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
if(orientation == ArmorBox::UNKNOWN){
if(anti_top_state == NORMAL){
sendBoxPosition(shoot_delay);
if (anti_top_state == ANTI_TOP) {
anti_top_state = NORMAL;
LOGM(STR_CTR(WORD_YELLOW, "switch to normal"));
}
return;
}
auto dist = boxDistance(last_pos, armor_box.rect);
if(orientation!=last_orient && orientation==ArmorBox::FRONT && dist>=6){
last_front_time = curr_time;
if(150<interval && interval<700){
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);
shoot_delay = (last_top_periodms+top_periodms)/2.0-110;
last_orient = orientation;
}else if(anti_top_state == NORMAL){
// LOGM("interval:%.1lf", interval);
if(++anti_top_cnt > 4){
if (change_type == INCREASE && last_ratio_type != change_type) {
last_front_time = frame_time;
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);
timeval curr_time;
gettimeofday(&curr_time, nullptr);
auto calculate_time = getTimeIntervalms(curr_time, frame_time);
shoot_delay = mean(top_periodms) - calculate_time;
} else if (anti_top_state == NORMAL) {
if (++anti_top_cnt > 4) {
anti_top_state = ANTI_TOP;
LOGM(STR_CTR(WORD_CYAN, "switch to anti-top"));
}
}
}
}
if(anti_top_state == ANTI_TOP){
if(orientation == ArmorBox::FRONT){
if (change_type != NOCHANGE) {
last_ratio_type = change_type;
}
if (anti_top_state == ANTI_TOP) {
if (orientation == ArmorBox::FRONT) {
sendBoxPosition(shoot_delay);
}
}else if(anti_top_state == NORMAL){
} else if (anti_top_state == NORMAL) {
sendBoxPosition(shoot_delay);
}
last_orient = orientation;
}

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

@@ -58,12 +58,12 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
}
void ArmorFinder::run(cv::Mat &src) {
gettimeofday(&frame_time, nullptr);
// stateSearchingTarget(src); // for debug
// goto end;
switch (state) {
case SEARCHING_STATE:
if (stateSearchingTarget(src)) {
// cout << armor_box.rect << endl;
if ((armor_box.rect & cv::Rect2d(0, 0, 640, 480)) == armor_box.rect) { // 判断装甲板区域是否脱离图像区域
if (!classifier || !use_classifier) { /* 如果分类器不可用或者不使用分类器 */
cv::Mat roi = src(armor_box.rect).clone(), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */

1
armor/src/armor_finder/send_target/send_target.cpp
View File

@@ -52,6 +52,5 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
double dist = DISTANCE_HEIGHT / rect.height;
// cout << yaw << endl;
return sendTarget(serial, yaw, -pitch, dist, shoot_delay);
}

15
armor/src/armor_finder/tracking_state/tracking_state.cpp
View File

@@ -39,6 +39,8 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
blob.rect.center.x += bigger_rect.x;
blob.rect.center.y += bigger_rect.y;
}
tracker = TrackerToUse::create();
tracker->init(src, armor_box.rect);
}else{
roi = src(pos).clone();
if(classifier){
@@ -60,18 +62,5 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
armor_box.rect = pos;
armor_box.light_blobs.clear();
}
// armor_box.x -= armor_box.width / 4.0;
// armor_box.y -= armor_box.height / 4.0;
// armor_box.height *= 1.5;
// armor_box.width *= 1.5;
// roi = src(armor_box);
// if(findSearchingTarget(roi)){
//
// }
// sendBoxPosition();
return true;
}