diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index d3c9557..0090c2b 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -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 fans;//图像中所有扇叶 std::vector armors;//图像中所有装甲板 std::vector 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);//寻找图中所有扇叶 diff --git a/energy/src/energy/change/mode_change.cpp b/energy/src/energy/change/mode_change.cpp index c304c2b..b331f5f 100644 --- a/energy/src/energy/change/mode_change.cpp +++ b/energy/src/energy/change/mode_change.cpp @@ -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; diff --git a/energy/src/energy/energy_init.cpp b/energy/src/energy/energy_init.cpp index 2d322aa..358a89d 100644 --- a/energy/src/energy/energy_init.cpp +++ b/energy/src/energy/energy_init.cpp @@ -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; diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index d300c30..f1ae97a 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -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: "<(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(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; } \ No newline at end of file diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index aff41de..2e46392 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -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(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(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(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; } diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 2ab582e..c6ad954 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -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(x * (32768 - 1) / 100); - y_tmp = static_cast(y * (32768 - 1) / 100); - z_tmp = static_cast(z * (32768 - 1) / 1000); - buff[0] = 's'; - buff[1] = static_cast((x_tmp >> 8) & 0xFF); - buff[2] = static_cast((x_tmp >> 0) & 0xFF); - buff[3] = static_cast((y_tmp >> 8) & 0xFF); - buff[4] = static_cast((y_tmp >> 0) & 0xFF); - buff[5] = static_cast((z_tmp >> 8) & 0xFF); - buff[6] = static_cast((z_tmp >> 0) & 0xFF); - buff[7] = 'e'; - serial.WriteData(buff, sizeof(buff)); - send_cnt+=1; -// cout<<"send cnt: "<(x * (32768 - 1) / 100); diff --git a/others/include/options/options.h b/others/include/options/options.h index b7c07c3..a6e1c43 100644 --- a/others/include/options/options.h +++ b/others/include/options/options.h @@ -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[]); diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index 9b8ff23..b2f6a6c 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -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); diff --git a/others/src/options/options.cpp b/others/src/options/options.cpp index 0448aec..64cc585 100644 --- a/others/src/options/options.cpp +++ b/others/src/options/options.cpp @@ -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= 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]); } }