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

energy changed

Browse Source
This commit is contained in:
sun
2019-07-10 21:37:08 +08:00
parent 92295793d5
commit 2e1538902a
9 changed files with 156 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
energy/include/energy/energy.h
View File

@@ -31,8 +31,7 @@ public:
Serial &serial;//串口
void setEnergyRotationInit();//判断顺逆时针函数
void extract(cv::Mat &src);//框取图像中的一块区域进行处理
void sendBigTarget(Serial& serial, float x, float y, float z);
void sendSmallTarget(Serial& serial, float x, float y, char change, char shoot);
void sendTarget(Serial& serial, float x, float y, char change, char shoot);
private:
@@ -42,6 +41,8 @@ private:
bool isMark;//若操作手正在手动标定，则为true
bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
int fans_cnt;//图像中的扇叶个数
int armors_cnt;//图像中的装甲板个数
int centerRs_cnt;//图像中可能的风车中心字母R选区个数
@@ -70,7 +71,8 @@ private:
float origin_yaw, origin_pitch;//初始的云台角度设定值
float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板
bool save_new_mark;//若操作手进行过手动标定，则为true
bool small_energy_shoot;//若为true，则要求主控板发弹
bool big_energy_shoot;//大符模式中，若为true，则要求主控板发弹
bool small_energy_shoot;//小符模式中，若为true，则要求主控板发弹
std::vector<EnergyPart> fans;//图像中所有扇叶
std::vector<EnergyPart> armors;//图像中所有装甲板
std::vector<EnergyPart> centerRs;//风车中心字母R的可能候选区
@@ -92,6 +94,7 @@ private:
void initEnergyPartParam();//能量机关参数初始化
void initRotation();//顺逆时针初始化
void changeMode();//猜测模式与击打模式切换
bool isGimbleCentered();//判断云台摄像头对心是否完成
int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶

19
energy/src/energy/change/mode_change.cpp
View File

@@ -7,12 +7,27 @@ using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于切换猜测模式（发弹——新目标出现）和击打模式（新目标出现——发弹）
// ---------------------------------------------------------------------------------------------------------------------
void Energy::changeMode() {
if (changeTarget()) {
isGuessing = false;
isPredicting = true;
target_cnt++;//若云台移动过程中发现有新装甲板亮起，需改变target_cnt值，以及时告知主控板中断进程，防止重复打击
}
if (big_energy_shoot || small_energy_shoot){
isGuessing = true;
isPredicting = false;
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断云台对心是否完成
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::isGimbleCentered(){
if(centerRs.size()==1){
bool Energy::isGimbleCentered() {
if (centerRs.size() == 1) {
circle_center_point = centerRs.at(0).rect.center;
isGimble = false;
isChassis = true;

3
energy/src/energy/energy_init.cpp
View File

@@ -16,6 +16,8 @@ using std::vector;
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergy() {
isMark = false;
isPredicting = true;
isGuessing = false;
fans_cnt = 0;
armors_cnt = 0;
centerRs_cnt = 0;
@@ -40,6 +42,7 @@ void Energy::initEnergy() {
last_mark = 0;
target_cnt = 0;
big_energy_shoot = false;
small_energy_shoot = false;
predict_rad = 20;

42
energy/src/energy/get/gimble_rotation_get.cpp
View File

@@ -11,41 +11,41 @@ using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于操作手手动标定
// ---------------------------------------------------------------------------------------------------------------------
void Energy::changeMark() {
if (mcuData.mark == 0 && last_mark == 1) {
last_mark = mcuData.mark;
origin_yaw = mcuData.curr_yaw;
origin_pitch = mcuData.curr_pitch;
isMark = false;
if (mcuData.mark == 0 && last_mark == 1) {//完成标定
last_mark = mcuData.mark;
origin_yaw = mcuData.curr_yaw;
origin_pitch = mcuData.curr_pitch;
isMark = false;
// LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
}
else if (mcuData.mark == 1) {
last_mark = mcuData.mark;
isMark = true;
} else if (mcuData.mark == 1) {//正在标定
last_mark = mcuData.mark;
isMark = true;
// LOGM(STR_CTR(WORD_BLUE,"Marking..."));
}
else {
last_mark = mcuData.mark;
isMark = false;
}
//cout<<"mark: "<<int(mcuData.mark)<<endl;
} else {//未在标定
last_mark = mcuData.mark;
isMark = false;
}
//cout<<"mark: "<<int(mcuData.mark)<<endl;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于计算云台应当转到的角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::gimbleRotation(){
cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度
void Energy::gimbleRotation() {
cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度
stretch(predict_point, real_predict_point);
yaw_rotation = static_cast<float>(180 / PI *
atan2((attack_distance * tan(origin_yaw * PI / 180) - real_predict_point.x), attack_distance));
atan2((attack_distance * tan(origin_yaw * PI / 180) - real_predict_point.x),
attack_distance));
pitch_rotation = static_cast<float>(180 / PI *
atan2((attack_distance*tan(origin_pitch*PI/180)-real_predict_point.y), attack_distance));
atan2((attack_distance * tan(origin_pitch * PI / 180) - real_predict_point.y),
attack_distance));
if (fabs(yaw_rotation - mcuData.curr_yaw) < 0.5 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.5)
big_energy_shoot = true;
}

71
energy/src/energy/run.cpp
View File

@@ -3,11 +3,10 @@
//
#include "energy/energy.h"
#include "log.h"
#include "options/options.h"
using namespace std;
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
@@ -17,9 +16,8 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
if (chassis_src.empty())
runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
else if (isGimble) {
imshow("src", gimble_src);
if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
energy_part_param_ = gimble_energy_part_param_;
energy_part_param_ = chassis_energy_part_param_;
fans.clear();
armors.clear();
centerRs.clear();
@@ -29,26 +27,30 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
target_armor.clear();
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("bin", gimble_src);
if (show_bin)imshow("bin", gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt);
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(gimble_src);
showFlowStripFanContours("strip", gimble_src);
if (show_strip)showFlowStripFanContours("strip", gimble_src);
} else {
fans_cnt = findFan(gimble_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
if (show_single) {
if (fans_cnt > 0)showFanContours("fan", gimble_src);
if (armors_cnt > 0)showArmorContours("armor", gimble_src);
}
if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src);
}
centerRs_cnt = findCenterR(gimble_src);
if (centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (isGimbleCentered()) {
getOrigin();
initEnergy();
destroyAllWindows();
}
} else if (isChassis) {
imshow("src", chassis_src);
if (chassis_src.type() == CV_8UC3)cvtColor(chassis_src, chassis_src, COLOR_BGR2GRAY);
energy_part_param_ = chassis_energy_part_param_;
fans.clear();
@@ -60,7 +62,7 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
target_armor.clear();
// imagePreprocess(chassis_src);
// imshow("img_preprocess", chassis_src);
// if(show_split)imshow("img_preprocess", chassis_src);
changeMark();
if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
@@ -71,15 +73,21 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
flow_strip_fans_cnt = findFlowStripFan(chassis_src, last_flow_strip_fans_cnt);
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(chassis_src);
showFlowStripFanContours("strip", chassis_src);
if (show_strip)showFlowStripFanContours("strip", chassis_src);
} else {
fans_cnt = findFan(chassis_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
if (show_single) {
if (fans_cnt > 0)showFanContours("fan", gimble_src);
if (armors_cnt > 0)showArmorContours("armor", gimble_src);
}
if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src);
if (write_down)writeDownMark();
}
centerRs_cnt = findCenterR(chassis_src);
if (centerRs_cnt > 0)showCenterRContours("R", chassis_src);
if (show_center && centerRs_cnt > 0)showCenterRContours("R", chassis_src);
if (centerRs.size() != 1)return 0;
circle_center_point = centerRs.at(0).rect.center;
target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
@@ -89,11 +97,10 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
return 0;
}
getOrigin();
getPredictPoint();
gimbleRotation();
if (changeTarget())target_cnt++;
sendBigTarget(serial, yaw_rotation, pitch_rotation, target_cnt);
sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, big_energy_shoot);
return 0;
}
@@ -105,7 +112,6 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runBig(cv::Mat &gimble_src) {
imshow("src", gimble_src);
if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
energy_part_param_ = gimble_energy_part_param_;
fans.clear();
@@ -116,22 +122,31 @@ int Energy::runBig(cv::Mat &gimble_src) {
center_ROI.clear();
target_armor.clear();
// imagePreprocess(gimble_src);
// imshow("img_preprocess", gimble_src);
// if(show_split)imshow("img_preprocess", gimble_src);
changeMark();
if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
// imshow("bin",gimble_src);
if (show_bin)imshow("bin", gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt);
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(gimble_src);
showFlowStripFanContours("strip", gimble_src);
if (show_strip)showFlowStripFanContours("strip", gimble_src);
} else {
fans_cnt = findFan(gimble_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
if (show_single) {
if (fans_cnt > 0)showFanContours("fan", gimble_src);
if (armors_cnt > 0)showArmorContours("armor", gimble_src);
}
if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src);
if (write_down)writeDownMark();
}
centerRs_cnt = findCenterR(gimble_src);
if (centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (centerRs.size() != 1)return 0;
circle_center_point = centerRs.at(0).rect.center;
target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
@@ -144,7 +159,7 @@ int Energy::runBig(cv::Mat &gimble_src) {
getPredictPoint();
gimbleRotation();
if (changeTarget())target_cnt++;
sendBigTarget(serial, yaw_rotation, pitch_rotation, target_cnt);
sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, big_energy_shoot);
return 0;
}
@@ -154,7 +169,6 @@ int Energy::runBig(cv::Mat &gimble_src) {
// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runSmall(cv::Mat &gimble_src) {
imshow("gimble src", gimble_src);
if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
energy_part_param_ = gimble_energy_part_param_;
fans.clear();
@@ -165,26 +179,31 @@ int Energy::runSmall(cv::Mat &gimble_src) {
center_ROI.clear();
target_armor.clear();
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("bin", gimble_src);
if (show_bin)imshow("bin", gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt);
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
findCenterROI(gimble_src);
showFlowStripFanContours("strip", gimble_src);
if (show_strip)showFlowStripFanContours("strip", gimble_src);
} else {
fans_cnt = findFan(gimble_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection();
if (show_single) {
if (fans_cnt > 0)showFanContours("fan", gimble_src);
if (armors_cnt > 0)showArmorContours("armor", gimble_src);
}
if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src);
}
centerRs_cnt = findCenterR(gimble_src);
if (centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (centerRs.size() != 1)return 0;
circle_center_point = centerRs.at(0).rect.center;
target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
(target_point.x - circle_center_point.x)));
getAimPoint();
if (changeTarget())target_cnt++;//若云台移动过程中发现有新装甲板亮起，需改变target_cnt值，以及时告知主控板中断进程，防止重复打击
sendSmallTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot);
changeMode();
sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot);
return 0;
}

29
energy/src/energy/send/send.cpp
View File

@@ -8,34 +8,9 @@ using namespace std;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送小符数据
// 此函数用于发送数据给主控板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendBigTarget(Serial& serial, float x, float y, float z) {
static short x_tmp, y_tmp, z_tmp;
uint8_t buff[8];
x_tmp = static_cast<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 'e';
serial.WriteData(buff, sizeof(buff));
send_cnt+=1;
// cout<<"send cnt: "<<send_cnt<<endl;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送大符数据
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendSmallTarget(Serial& serial, float x, float y, char change, char shoot){
void Energy::sendTarget(Serial& serial, float x, float y, char change, char shoot){
static short x_tmp, y_tmp, z_tmp;
uint8_t buff[8];
x_tmp = static_cast<short>(x * (32768 - 1) / 100);

7
others/include/options/options.h
View File

@@ -15,6 +15,13 @@ extern bool run_with_camera;
extern bool save_video;
extern bool wait_uart;
extern bool save_labelled_boxes;
extern bool show_bin;
extern bool show_split;
extern bool show_single;
extern bool show_both;
extern bool show_center;
extern bool show_strip;
extern bool write_down;
void process_options(int argc, char *argv[]);

2
others/src/additions/additions.cpp
View File

@@ -116,7 +116,7 @@ void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src){
else if(!gimble_src.empty() && chassis_src.empty()){
gimble_video_writer.write(gimble_src);
}
else if(gimble_src.empty() && !chassis_src.empty()){
else if(gimble_src.empty() && !chassis_src.empty()){ if (show_origin)imshow("src", gimble_src);
Mat chassis_save = chassis_src.clone();
cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR);
chassis_video_writer.write(chassis_save);

68
others/src/options/options.cpp
View File

@@ -14,11 +14,18 @@ bool run_with_camera = false;
bool save_video = false;
bool wait_uart = false;
bool save_labelled_boxes = false;
bool show_bin = false;
bool show_split = false;
bool show_single = false;
bool show_both = false;
bool show_center = false;
bool show_strip = false;
bool write_down = false;
void process_options(int argc, char *argv[]){
if(argc >= 2){
for(int i=1; i<argc; i++){
if(strcmp(argv[i], "--help") == 0){
void process_options(int argc, char *argv[]) {
if (argc >= 2) {
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "--help") == 0) {
LOGM("--show-armor-box: show the aim box.");
LOGM("--show-armor-boxes: show the candidate aim boxes.");
LOGM("--show-light-blobs: show the candidate light blobs.");
@@ -26,19 +33,19 @@ void process_options(int argc, char *argv[]){
LOGM("--run-with-camera: start the program with camera directly without asking.");
LOGM("--save-video: save the video.");
LOGM("--save-labelled-boxes: save labelled armor boxes.");
}else if(strcmp(argv[i], "--show-armor-box") == 0){
} else if (strcmp(argv[i], "--show-armor-box") == 0) {
show_armor_box = true;
LOGM("Enable show armor box");
}else if(strcmp(argv[i], "--show-armor-boxes") == 0){
} else if (strcmp(argv[i], "--show-armor-boxes") == 0) {
show_armor_boxes = true;
LOGM("Enable show armor boxes");
}else if(strcmp(argv[i], "--show-light-blobs") == 0) {
} else if (strcmp(argv[i], "--show-light-blobs") == 0) {
show_light_blobs = true;
LOGM("Enable show light blobs");
}else if(strcmp(argv[i], "--show-origin") == 0) {
} else if (strcmp(argv[i], "--show-origin") == 0) {
show_origin = true;
LOGM("Enable show origin");
}else if(strcmp(argv[i], "--show-all") ==0 ) {
} else if (strcmp(argv[i], "--show-all") == 0) {
show_armor_box = true;
LOGM("Enable show armor box");
show_armor_boxes = true;
@@ -47,19 +54,52 @@ void process_options(int argc, char *argv[]){
LOGM("Enable show light blobs");
show_origin = true;
LOGM("Enable show origin");
}else if(strcmp(argv[i], "--run-with-camera") == 0){
show_bin = true;
LOGM("Enable show binary image");
show_split = true;
LOGM("Enable show split image");
show_single = true;
LOGM("Enable show single image");
show_both = true;
LOGM("Enable show both image");
show_center = true;
LOGM("Enable show circle center");
show_strip = true;
LOGM("Enable show strip");
} else if (strcmp(argv[i], "--run-with-camera") == 0) {
run_with_camera = true;
LOGM("Run with camera!");
}else if(strcmp(argv[i], "--save-video") == 0){
} else if (strcmp(argv[i], "--save-video") == 0) {
save_video = true;
LOGM("Save video!");
}else if(strcmp(argv[i], "--wait-uart") == 0){
} else if (strcmp(argv[i], "--wait-uart") == 0) {
wait_uart = true;
LOGM("Wait uart until available!");
}else if(strcmp(argv[i], "--save-labelled-boxes") == 0){
} else if (strcmp(argv[i], "--save-labelled-boxes") == 0) {
save_labelled_boxes = true;
LOGM("labelled armor boxes will be saved!");
}else{
} else if (strcmp(argv[i], "--show-bin") == 0) {
show_bin = true;
LOGM("Enable show binary image");
} else if (strcmp(argv[i], "--show-split") == 0) {
show_split = true;
LOGM("Enable show split image");
} else if (strcmp(argv[i], "--show-single") == 0) {
show_single = true;
LOGM("Enable show single image");
} else if (strcmp(argv[i], "--show-both") == 0) {
show_both = true;
LOGM("Enable show both image");
} else if (strcmp(argv[i], "--show-center") == 0) {
show_center = true;
LOGM("Enable show circle center");
} else if (strcmp(argv[i], "--show-strip") == 0) {
show_strip = true;
LOGM("Enable show strip");
} else if (strcmp(argv[i], "--save-mark") == 0) {
write_down = true;
LOGM("Write down mark");
} else {
LOGW("Unknown option: %s. Use --help to see options.", argv[i]);
}
}