diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 8b30574..b951735 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -37,8 +37,10 @@ public: private: EnergyPartParam energy_part_param_;//能量机关的参数设置 + bool isMark;//若操作手正在手动标定，则为true - bool centered=false;//云台是否对准中心 + bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程 + bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程 int fans_cnt;//图像中的扇叶个数 int armors_cnt;//图像中的装甲板个数 int centerRs_cnt;//图像中可能的风车中心字母R选区个数 @@ -48,7 +50,6 @@ private: int last_armors_cnt;//上一帧的装甲板个数 int last_flow_strip_fans_cnt;//上一帧的含流动条扇叶个数 int last_flow_strips_cnt;//上一帧的流动条个数 - int gimble_cnt; //经过的帧数 double radius;//大风车半径 float target_polar_angle;//待击打装甲板的极坐标角度 float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度 @@ -81,7 +82,6 @@ private: cv::Point target_point;//目标装甲板中心坐标 cv::Point last_target_point;//上一帧目标装甲板中心坐标 cv::Point predict_point;//预测的击打点坐标 - cv::Point former_point;//之前预测的圆心坐标 std::vectorfan_polar_angle;//当前帧所有扇叶的极坐标角度 std::vectorarmor_polar_angle;//当前帧所有装甲板的极坐标角度 std::vector all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算 @@ -91,6 +91,8 @@ private: void initEnergyPartParam();//能量机关参数初始化 void initRotation();//顺逆时针初始化 + bool isGimbleCentered();//判断云台摄像头对心是否完成 + int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶 int findArmor(const cv::Mat src, int &last_armors_cnt);//寻找图中所有装甲板 int findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R @@ -113,6 +115,7 @@ private: void getFanPolarAngle();//获取扇叶极坐标角度 void getArmorPolarAngle();//获取装甲板极坐标角度 void getAllArmorCenters();//记录所有装甲板中心坐标 + void getOrigin();//获得云台对心所需角度 void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径 diff --git a/energy/src/energy/change/mode_change.cpp b/energy/src/energy/change/mode_change.cpp new file mode 100644 index 0000000..c304c2b --- /dev/null +++ b/energy/src/energy/change/mode_change.cpp @@ -0,0 +1,22 @@ +// +// Created by sun on 19-7-10. +// +#include "energy/energy.h" + +using namespace std; +using namespace cv; + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于判断云台对心是否完成 +// --------------------------------------------------------------------------------------------------------------------- +bool Energy::isGimbleCentered(){ + if(centerRs.size()==1){ + circle_center_point = centerRs.at(0).rect.center; + isGimble = false; + isChassis = true; + return true; + } + return false; +} \ No newline at end of file diff --git a/energy/src/energy/change/target_change.cpp b/energy/src/energy/change/target_change.cpp new file mode 100644 index 0000000..7d83e65 --- /dev/null +++ b/energy/src/energy/change/target_change.cpp @@ -0,0 +1,24 @@ +// +// Created by sun on 19-7-10. +// +#include "energy/energy.h" + +using namespace std; +using namespace cv; + + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于判断目标是否切换 +// --------------------------------------------------------------------------------------------------------------------- +bool Energy::changeTarget(){ + if(pointDistance(target_point,last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX){ + last_target_point = target_point; + return false; + } + else{ + last_target_point= target_point; + return true; + } +} \ No newline at end of file diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index a311ec8..9e23efc 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -20,6 +20,9 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color), initEnergy(); initEnergyPartParam(); + energy_rotation_init = false; + isGimble = true; + isChassis = false; save_new_mark = false; if(ally_color == ALLY_RED){ @@ -50,7 +53,10 @@ Energy::~Energy() = default; void Energy::setEnergyRotationInit() { initEnergy(); initEnergyPartParam(); + energy_rotation_init = true; + isGimble = true; + isChassis = false; if(save_new_mark){ FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r"); diff --git a/energy/src/energy/energy_init.cpp b/energy/src/energy/energy_init.cpp index ea01939..29c616a 100644 --- a/energy/src/energy/energy_init.cpp +++ b/energy/src/energy/energy_init.cpp @@ -16,18 +16,15 @@ using std::vector; // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergy() { isMark = false; - centered=false; fans_cnt = 0; armors_cnt = 0; centerRs_cnt = 0; flow_strips_cnt = 0; flow_strip_fans_cnt = 0; - gimble_cnt = 0; circle_center_point = Point(0, 0); target_point = Point(0, 0); last_target_point = Point(0, 0); predict_point = Point(0, 0); - former_point = Point(0,0); target_polar_angle = -1000; last_target_polar_angle = -1000; radius = 0; @@ -42,14 +39,8 @@ void Energy::initEnergy() { pitch_rotation = 0; last_mark = 0; - red_origin_yaw = -0.35; - red_origin_pitch = 15.11719; - blue_origin_yaw = -0.439453; - blue_origin_pitch = 15.688477; - target_cnt = 0; small_energy_shoot = false; - energy_rotation_init = false; predict_rad = 20; fans.clear(); diff --git a/energy/src/energy/find/target_finder.cpp b/energy/src/energy/find/target_finder.cpp index 5081047..92a9c43 100644 --- a/energy/src/energy/find/target_finder.cpp +++ b/energy/src/energy/find/target_finder.cpp @@ -9,56 +9,50 @@ using std::endl; using std::vector; - //---------------------------------------------------------------------------------------------------------------------- // 此函数通过极坐标角度匹配扇叶和装甲板，找到目标装甲板,计算其极坐标角度和中心坐标 // --------------------------------------------------------------------------------------------------------------------- void Energy::findTargetByPolar() { - if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误 - if (armor_polar_angle.empty())return;//找不到扇叶，识别错误 - if (fan_polar_angle.empty()) { + if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误 + if (armor_polar_angle.empty())return;//找不到扇叶，识别错误 + if (fan_polar_angle.empty()) { target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板 - for (const auto &armor : armors) - { - target_point = armor.rect.center; - } - return; - } - sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序 - sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序 - int i = 0, j = 0; - for (i = 0; i < fan_polar_angle.size(); ++i) { - if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX - && armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) { - j++; - continue;//若第i个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功，i与j都加1 - } - else { + for (const auto &armor : armors) { + target_point = armor.rect.center; + } + return; + } + sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序 + sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序 + int i = 0, j = 0; + for (i = 0; i < fan_polar_angle.size(); ++i) { + if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX + && armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) { + j++; + continue;//若第i个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功，i与j都加1 + } else { target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板 - for (const auto &armor : armors) - { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), - (armor.rect.center.x - circle_center_point.x))); - if(target_polar_angle == angle){ - target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 - } - } - return; - } - } + for (const auto &armor : armors) { + float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), + (armor.rect.center.x - circle_center_point.x))); + if (target_polar_angle == angle) { + target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 + } + } + return; + } + } target_polar_angle = armor_polar_angle.at(armor_polar_angle.size() - 1);//前几个扇叶都匹配到装甲板，则最后剩下的装甲板为目标 - for (const auto &armor : armors) - { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), - (armor.rect.center.x - circle_center_point.x))); - if(target_polar_angle == angle){ - target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 - } - } + for (const auto &armor : armors) { + float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), + (armor.rect.center.x - circle_center_point.x))); + if (target_polar_angle == angle) { + target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 + } + } } - //---------------------------------------------------------------------------------------------------------------------- // 此函数根据矩形重合面积匹配扇叶与装甲板 // --------------------------------------------------------------------------------------------------------------------- @@ -73,19 +67,19 @@ void Energy::findTargetByIntersection() { while (i < armors.size()) { for (j = 0; j < fans.size(); ++j) { std::vector intersection; - if(rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积 + if (rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积 continue; else rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection); - double cur_contour_area = contourArea(intersection); - if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) { + double cur_contour_area = contourArea(intersection); + if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) { // cout << endl; // cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is" // << cur_contour_area << endl; - break; - } + break; + } } - if(j == fans.size()){ + if (j == fans.size()) { target_point = armors.at(i).rect.center; break; } @@ -94,20 +88,19 @@ void Energy::findTargetByIntersection() { } - //---------------------------------------------------------------------------------------------------------------------- // 此函数在流动条区域内寻找装甲板 // --------------------------------------------------------------------------------------------------------------------- -bool Energy::findTargetInFlowStripFan(){ +bool Energy::findTargetInFlowStripFan() { int i = 0; - for(i=0; i intersection; - if(rotatedRectangleIntersection(armors.at(i).rect, flow_strip_fans.at(0).rect, intersection)==0) + if (rotatedRectangleIntersection(armors.at(i).rect, flow_strip_fans.at(0).rect, intersection) == 0) continue;//返回0表示没有重合面积 double cur_contour_area = contourArea(intersection); - if(cur_contour_area < energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) + if (cur_contour_area < energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) continue; - else{ + else { target_armor.emplace_back(armors.at(i)); target_point = armors.at(i).rect.center; return true; diff --git a/energy/src/energy/get/origin_get.cpp b/energy/src/energy/get/origin_get.cpp new file mode 100644 index 0000000..bb715ef --- /dev/null +++ b/energy/src/energy/get/origin_get.cpp @@ -0,0 +1,20 @@ +// +// Created by sun on 19-7-10. +// +#include "energy/energy.h" + +using namespace std; +using namespace cv; + + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于获得云台对心得到的初始yaw和pitch（即以该yaw和pitch发射子弹，可以击中风车中心） +// --------------------------------------------------------------------------------------------------------------------- +void Energy::getOrigin(){ + double dx = circle_center_point.x - 320; + double dy = circle_center_point.y - 240; + origin_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; + origin_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; +} \ No newline at end of file diff --git a/energy/src/energy/get/predict_point_get.cpp b/energy/src/energy/get/predict_point_get.cpp index 956ad61..7608ac5 100644 --- a/energy/src/energy/get/predict_point_get.cpp +++ b/energy/src/energy/get/predict_point_get.cpp @@ -20,18 +20,3 @@ void Energy::getPredictPoint(){ if(energy_rotation_direction==-1) rotate(); } - - -//---------------------------------------------------------------------------------------------------------------------- -// 此函数用于操作手手动标定 -// --------------------------------------------------------------------------------------------------------------------- -bool Energy::changeTarget(){ - if(pointDistance(target_point,last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX){ - last_target_point = target_point; - return false; - } - else{ - last_target_point= target_point; - return true; - } -} \ No newline at end of file diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 004dddc..34293f6 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -10,70 +10,98 @@ using std::endl; using std::vector; - //---------------------------------------------------------------------------------------------------------------------- // 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头 // --------------------------------------------------------------------------------------------------------------------- -int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src){ - if(chassis_src.empty()) +int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) { + if (chassis_src.empty()) runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数 - else if(!centered) { + else if (isGimble) { + imshow("src", gimble_src); + fans.clear(); armors.clear(); - armor_polar_angle.clear(); - changeMark(); - if (isMark)return 0; + centerRs.clear(); + flow_strip_fans.clear(); + flow_strips.clear(); + center_ROI.clear(); + target_armor.clear(); threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); - imshow("yun",gimble_src); + 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); + } else { + fans_cnt = findFan(gimble_src, last_fans_cnt); + if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; + findTargetByIntersection(); + } + centerRs_cnt = findCenterR(gimble_src); + if (centerRs_cnt > 0)showCenterRContours("R", gimble_src); + if (isGimbleCentered()) { + getOrigin(); + initEnergy(); + destroyAllWindows(); + } + } else if (isChassis) { + imshow("src", chassis_src); + fans.clear(); + armors.clear(); + centerRs.clear(); + flow_strip_fans.clear(); + flow_strips.clear(); + center_ROI.clear(); + target_armor.clear(); - armors_cnt = findArmor(gimble_src, last_armors_cnt); - if(armors_cnt!=1) return 0;//滤去漏判的帧 +// imagePreprocess(chassis_src); +// imshow("img_preprocess", chassis_src); - getAllArmorCenters(); - circleLeastFit(); + changeMark(); + if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch -// attack_distance = 752;//单项赛 - attack_distance = 718; + threshold(chassis_src, chassis_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); + armors_cnt = findArmor(chassis_src, last_armors_cnt); + 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); + } else { + fans_cnt = findFan(chassis_src, last_fans_cnt); + if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; + findTargetByIntersection(); + } + + centerRs_cnt = findCenterR(chassis_src); + if (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), + (target_point.x - circle_center_point.x))); if (energy_rotation_init) { initRotation(); return 0; } - if(++gimble_cnt%8==0){ - former_point=circle_center_point; - //gimble_cnt=0; - } - - if(former_point==predict_point&&gimble_cnt%8==7&&predict_point!=Point(0,0)) { - centered=true; - cout<<"gimble focused!"<0||fans_cnt>0) showBothContours("Both", gimble_src); } - centerRs_cnt = findCenterR(gimble_src); - if(centerRs_cnt>0)showCenterRContours("R", gimble_src); - - writeDownMark(); - + if (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))); + if (energy_rotation_init) { + initRotation(); + getOrigin();//一旦确定风车旋向后，就开始移动云台，此时不再更新origin_yaw和origin_pitch + return 0; + } getPredictPoint(); gimbleRotation(); - if(changeTarget())target_cnt++; + if (changeTarget())target_cnt++; sendBigTarget(serial, yaw_rotation, pitch_rotation, target_cnt); -// cout<<"yaw: "< 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); + return 0; } -//---------------------------------------------------------------------------------------------------------------------- -// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头 -// --------------------------------------------------------------------------------------------------------------------- -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); - fans.clear(); - armors.clear();- - threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); - imshow("bin",gimble_src); - fans_cnt = findFan(gimble_src, last_fans_cnt); - armors_cnt = findArmor(gimble_src, last_armors_cnt); - if(fans_cnt==-1 || armors_cnt==-1 || armors_cnt != fans_cnt+1) return 0; - findTargetByIntersection(); - if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src); - getAimPoint(); - if(changeTarget())target_cnt++;//若云台移动过程中发现有新装甲板亮起，需改变target_cnt值，以及时告知主控板中断进程，防止重复打击 - sendSmallTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot); -} - - - - diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 8846ee0..2ab582e 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -27,6 +27,7 @@ void Energy::sendBigTarget(Serial& serial, float x, float y, float z) { buff[7] = 'e'; serial.WriteData(buff, sizeof(buff)); send_cnt+=1; +// cout<<"send cnt: "<read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头 if (save_video) saveVideos(gimble_src, chassis_src);//保存视频 if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像 -// if (from_camera == 0) { -// cv::resize(chassis_src, chassis_src, cv::Size(640, 480), 2); -// imshow("resize", chassis_src); -// energy.extract(chassis_src); -// } if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化 energy.setEnergyRotationInit(); cout << "set" << endl; } energy.runBig(gimble_src, chassis_src);//击打大符 +// energy.runBig(gimble_src); last_state = mcuData.state;//更新上一帧状态 } else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式 last_state = mcuData.state; @@ -118,10 +114,10 @@ int main(int argc, char *argv[]) { armorFinder.run(gimble_src); }); } else if (mcuData.state == SMALL_ENERGY_STATE) { -// energy.runSmall(gimble_src); - energy.runBig(gimble_src); + energy.runSmall(gimble_src); } } +// cv::waitKey(0); }); } while (ok); delete video_gimble;