diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 7551947..9d615ca 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -25,7 +25,10 @@ class Energy { public: Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色 ~Energy();//默认析构函数 - int run(cv::Mat &gimble_src, cv::Mat &base_src); + int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src); + int runBig(cv::Mat &gimble_src); + int runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src); + int runSmall(cv::Mat &gimble_src); Serial &serial;//串口 void setEnergyRotationInit();//判断顺逆时针函数 void extract(cv::Mat &src);//框取图像中的一块区域进行处理 @@ -99,6 +102,7 @@ private: void rotate();//获取预测点位 void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差 + double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离 void writeDownMark();//记录操作手标定的云台初始角度 diff --git a/energy/src/energy/param_init.cpp b/energy/src/energy/param_init.cpp index 44f2b8b..8779cf6 100644 --- a/energy/src/energy/param_init.cpp +++ b/energy/src/energy/param_init.cpp @@ -26,7 +26,7 @@ void Energy::initEnergy() { target_polar_angle = -1000; last_target_polar_angle = -1000; radius = 0; - energy_rotation_direction = CLOCKWISE; + energy_rotation_direction = ANTICLOCKWISE; attack_distance = ATTACK_DISTANCE; last_fans_cnt = 0; last_armors_cnt = 0; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index fd0682b..84c20c6 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -12,12 +12,23 @@ using std::vector; //---------------------------------------------------------------------------------------------------------------------- -// 此函数为能量机关模式主控制流函数 +// 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头 // --------------------------------------------------------------------------------------------------------------------- -int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){ +int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src){ + if(chassis_src.empty())runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数 + else { + runBig(chassis_src); + return 0; + } +} - cv::Mat src = gimble_src; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头 +// --------------------------------------------------------------------------------------------------------------------- +int Energy::runBig(cv::Mat &gimble_src){ // imshow("src",src); fans.clear(); armors.clear(); @@ -25,35 +36,29 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){ fan_polar_angle.clear(); armor_polar_angle.clear(); - changeMark(); - if (isMark)return 0; + changeMark(); + if (isMark)return 0; // imagePreprocess(src); // imshow("img_preprocess",src); - threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); + threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); // imshow("bin",src); - fans_cnt = findFan(src, last_fans_cnt); + fans_cnt = findFan(gimble_src, last_fans_cnt); // cout<<"fans_cnt: "<0)showFanContours("fan",src); // fans_cnt=0; - armors_cnt = findArmor(src, last_armors_cnt); + armors_cnt = findArmor(gimble_src, last_armors_cnt); // cout<<"armors_cnt: "<0) showArmorContours("armor",src); - if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src); + if(armors_cnt != fans_cnt+1) return 0; - centerRs_cnt = findCenterR(src); - if(centerRs_cnt>0)showCenterRContours("R",src); - - - if(armors_cnt != fans_cnt+1) - { - return 0; - } + centerRs_cnt = findCenterR(gimble_src); +// if(centerRs_cnt>0)showCenterRContours("R", gimble_src); getAllArmorCenters(); circleLeastFit(); @@ -63,10 +68,12 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){ getArmorPolarAngle(); findTarget(); - if (energy_rotation_init) { - initRotation(); - return 0; - } + if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src); + + if (energy_rotation_init) { + initRotation(); + return 0; + } getPredictPoint(); gimbleRotation(); sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt); @@ -79,3 +86,26 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){ + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为小能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头 +// --------------------------------------------------------------------------------------------------------------------- +int Energy::runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src){ + if(chassis_src.empty())runSmall(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数 + else return 0; +} + + + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为小能量机关模式主控制流函数，且步兵仅拥有云台摄像头 +// --------------------------------------------------------------------------------------------------------------------- +int Energy::runSmall(cv::Mat &gimble_src){ + +} + + + + diff --git a/energy/src/energy/show/show.cpp b/energy/src/energy/show/show.cpp index ce5e198..a226857 100644 --- a/energy/src/energy/show/show.cpp +++ b/energy/src/energy/show/show.cpp @@ -8,6 +8,8 @@ using std::cout; using std::endl; using std::vector; + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于显示图像中所有扇叶 // --------------------------------------------------------------------------------------------------------------------- @@ -23,19 +25,18 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) { { image2show = src.clone(); } - //cvtColor(image2show, image2show, COLOR_GRAY2RGB); for (const auto &fan : fans) { Point2f vertices[4]; //定义矩形的4个顶点 fan.rect.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 2); - //cout << fan.rect.center << '\t' << fan.rect.angle << '\t'; - //cout << endl; } imshow(windows_name, image2show); } + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于显示图像中所有装甲板 // --------------------------------------------------------------------------------------------------------------------- @@ -51,19 +52,21 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) { { image2show = src.clone(); } - //cvtColor(image2show, image2show, COLOR_GRAY2RGB); for (const auto &armor : armors) { Point2f vertices[4]; //定义矩形的4个顶点 armor.rect.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); - //cout << armor.rect.center << '\t' << armor.rect.angle << '\t'; - //cout << endl; } imshow(windows_name, image2show); } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于显示图像中所有扇叶和装甲板,并框出待击打装甲板 +// --------------------------------------------------------------------------------------------------------------------- void Energy::showBothContours(std::string windows_name, const cv::Mat src) { if (src.empty())return; static Mat image2show; @@ -75,25 +78,23 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) { { image2show = src.clone(); } - //cvtColor(image2show, image2show, COLOR_GRAY2RGB); for (const auto &fan : fans) { Point2f vertices[4]; //定义矩形的4个顶点 fan.rect.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4); -// cout << "fan" << fan.rect.size.height <<'\t'<< fan.rect.size.width << '\t' << '\t'; -// cout << endl; } for (const auto &armor : armors) { Point2f vertices[4]; //定义矩形的4个顶点 armor.rect.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4); -// cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle; -// cout << endl; - + for (int i = 0; i < 4; i++){ + if(pointDistance(static_cast(armor.rect.center),target_point) < 5) + line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4); + else + line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4); + } cv::Point2f point = armor.rect.center; cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径 @@ -102,6 +103,8 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) { imshow(windows_name, image2show); } + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于显示图像中所有可能的风车中心候选区R // --------------------------------------------------------------------------------------------------------------------- diff --git a/energy/src/energy/tool/tool.cpp b/energy/src/energy/tool/tool.cpp index e4a9c7d..6818636 100644 --- a/energy/src/energy/tool/tool.cpp +++ b/energy/src/energy/tool/tool.cpp @@ -58,4 +58,16 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){ point_2.x = static_cast( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0); point_2.y = static_cast( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0); +} + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于计算两点距离 +// --------------------------------------------------------------------------------------------------------------------- + +double Energy::pointDistance(cv::Point point_1, cv::Point point_2){ + double distance = 0; + distance = sqrt(pow(static_cast(point_1.x - point_2.x),2) + + pow(static_cast(point_1.y - point_2.y),2)); + return distance; } \ No newline at end of file diff --git a/main.cpp b/main.cpp index eec46a4..624fb61 100644 --- a/main.cpp +++ b/main.cpp @@ -26,16 +26,23 @@ using namespace std; mcu_data mcuData = { 0, 0, - BIG_ENERGY_STATE, + ARMOR_STATE, 0, 1, ENEMY_RED, }; +WrapperHead *video_gimble = nullptr; +WrapperHead *video_chassis = nullptr; + +Serial serial(115200); +uint8_t last_state = mcuData.state; + +ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier); +Energy energy(serial, mcuData.enemy_color); + int main(int argc, char *argv[]) { process_options(argc, argv); - Serial serial(115200); - uint8_t last_state = mcuData.state; thread receive(uartReceive, &serial); int from_camera = 1; @@ -45,170 +52,81 @@ int main(int argc, char *argv[]) { } while (true) { - VideoWriter armor_video_writer, energy_video_writer; - if (save_video) { - initVideoWriter(armor_video_writer, PROJECT_DIR"/armor_video/"); - initVideoWriter(energy_video_writer, PROJECT_DIR"/energy_video/"); - } - - WrapperHead *video_armor=nullptr; - WrapperHead *video_energy=nullptr; if (from_camera) { - video_armor = new CameraWrapper(0, "armor"); - video_energy = new CameraWrapper(1, "energy"); + video_gimble = new CameraWrapper(0, "armor"); + video_chassis = new CameraWrapper(1, "energy"); } else { -// string armor_video, energy_video; -// lastVideo(armor_video, PROJECT_DIR"/armor_video/"); -// video_armor = new VideoWrapper(armor_video); -// lastVideo(energy_video, PROJECT_DIR"/energy_video/"); -// video_energy = new VideoWrapper(energy_video); - video_armor = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); - video_energy = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); + video_gimble = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); } - if (video_armor->init()) { + if (video_gimble->init()) { LOGM("video_armor source initialization successfully."); } else { LOGW("video_armor source unavailable!"); - delete video_armor; - video_armor = nullptr; + delete video_gimble; + video_gimble = nullptr; } - if (video_energy->init()) { + if (video_chassis->init()) { LOGM("video_energy source initialization successfully."); } else { LOGW("video_energy source unavailable!"); - delete video_energy; - video_energy = nullptr; + delete video_chassis; + video_chassis = nullptr; } - Mat energy_src, armor_src; + Mat gimble_src, chassis_src; for (int i = 0; i < 10; i++) { - if (video_armor) { - video_armor->read(armor_src); + if (video_gimble) { + video_gimble->read(gimble_src); } - if (video_energy) { - video_energy->read(energy_src); + if (video_chassis) { + video_chassis->read(chassis_src); } } - - ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier); - - Energy energy(serial, mcuData.enemy_color); - bool ok = true; cout<<"start running"<read(armor_src) && video_energy->read(energy_src); - if (!ok) { - delete video_armor; - delete video_energy; - video_armor = nullptr; - video_energy = nullptr; - } - if(save_video){ - Mat energy_save = energy_src.clone(); - cvtColor(energy_save,energy_save,COLOR_GRAY2BGR); - armor_video_writer.write(armor_src); - energy_video_writer.write(energy_save); - } - if (show_origin) { - imshow("armor src", armor_src); - imshow("energy src", energy_src); - } + if (mcuData.state != ARMOR_STATE) {//能量机关模式 + ok = checkReconnect(video_gimble->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 (mcuData.state == BIG_ENERGY_STATE) {//大符模式 // if (from_camera == 0) { -// cv::resize(energy_src, energy_src, cv::Size(640, 480), 2); -// imshow("resize", energy_src); +// cv::resize(energy_src, energy_src, cv::Size(640, 480), 2); +// imshow("resize", energy_src); // energy.extract(energy_src); // } - energy.run(armor_src, energy_src); - waitKey(1); - } - else { - video_energy = new CameraWrapper(1, "energy"); - if(!video_energy->init()){ - delete video_energy; - video_energy = nullptr; + if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化 + energy.setEnergyRotationInit(); + cout << "set" << endl; } + energy.runBig(gimble_src, chassis_src);//击打大符 } + else if (mcuData.state == SMALL_ENERGY_STATE) { + energy.runSmall(gimble_src, chassis_src);//击打小符 + } + last_state = mcuData.state;//更新上一帧状态 } - - else if (mcuData.state == SMALL_ENERGY_STATE) { + else if (mcuData.state == ARMOR_STATE) {//自瞄模式 last_state = mcuData.state; - if (video_armor && video_energy) { - ok = video_armor->read(armor_src) && video_energy->read(energy_src); - if (!ok) { - delete video_armor; - delete video_energy; - video_armor = nullptr; - video_energy = nullptr; - } - if(save_video){ - Mat energy_save = energy_src.clone(); - cvtColor(energy_save,energy_save,COLOR_GRAY2BGR); - armor_video_writer.write(armor_src); - energy_video_writer.write(energy_save); - } - if (show_origin) { - imshow("armor src", armor_src); - imshow("energy src", energy_src); - } -// if (from_camera == 0) { -// cv::resize(energy_src, energy_src, cv::Size(640, 480), 2); -// imshow("resize", energy_src); -// energy.extract(energy_src); -// } - energy.run(armor_src, energy_src); - waitKey(1); - } - else { - video_energy = new CameraWrapper(1, "energy"); - if(!video_energy->init()){ - delete video_energy; - video_energy = nullptr; - } - } - } + ok = checkReconnect(video_gimble->read(gimble_src)); + if (save_video) saveVideos(gimble_src); + if (show_origin) showOrigin(gimble_src); + CNT_TIME("Armor Time", { + armorFinder.run(gimble_src); + }); - else if (mcuData.state == ARMOR_STATE) { - last_state = mcuData.state; - if (video_armor) { - ok = video_armor->read(armor_src); - if (!ok) { - delete video_armor; - video_armor = nullptr; - } - if(save_video){ - armor_video_writer.write(armor_src); - } -// flip(armor_src, armor_src, 0); - if (show_origin) { - imshow("armor src", armor_src); - } - CNT_TIME("Armor Time", { - armorFinder.run(armor_src); - }); - } - else { - video_armor = new CameraWrapper(0, "armor"); - if(!video_armor->init()){ - delete video_armor; - video_armor = nullptr; - } - } } - + cv::waitKey(1); }); } while (ok); - delete video_armor; - delete video_energy; + delete video_gimble; + video_gimble = nullptr; + delete video_chassis; + video_chassis = nullptr; cout << "Program fails. Restarting" << endl; } return 0; diff --git a/others/include/additions/additions.h b/others/include/additions/additions.h index 13fbaa1..2cfe668 100644 --- a/others/include/additions/additions.h +++ b/others/include/additions/additions.h @@ -29,7 +29,11 @@ struct mcu_data{ extern mcu_data mcuData; void uartReceive(Serial *pSerial); -void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix); -void lastVideo(std::string &video_name, const std::string &filename_prefix); +bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect); +bool checkReconnect(bool is_gimble_connect); +void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src); +void saveVideos(const cv::Mat &gimble_src); +void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src); +void showOrigin(const cv::Mat &gimble_src); #endif /* _ADDITIONS_H_ */ diff --git a/others/include/camera/wrapper_head.h b/others/include/camera/wrapper_head.h index bb9dfea..d92906c 100644 --- a/others/include/camera/wrapper_head.h +++ b/others/include/camera/wrapper_head.h @@ -15,7 +15,6 @@ public: virtual ~WrapperHead() = default;; virtual bool init() = 0; virtual bool read(cv::Mat &src) = 0; - }; diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index 851d253..339748c 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -4,14 +4,30 @@ #include #include +#include +#include +#include #include +#include #include #include #include +#include +#include #define RECEIVE_LOG_LEVEL LOG_MSG using namespace std; +using namespace cv; + +extern WrapperHead *video_gimble; +extern WrapperHead *video_chassis; + +extern Serial serial; +extern uint8_t last_state; + +extern ArmorFinder armorFinder; +extern Energy energy; void uartReceive(Serial* pSerial) { char buffer[20]; @@ -38,7 +54,8 @@ void uartReceive(Serial* pSerial) { } } -void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix){ +cv::VideoWriter initVideoWriter(const std::string &filename_prefix){ + cv::VideoWriter video; std::ifstream in(filename_prefix + "cnt.txt"); int cnt = 0; if (in.is_open()) @@ -54,15 +71,85 @@ void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix) out.close(); } video.open(file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true); + return video; } -void lastVideo(std::string &video_name, const std::string &filename_prefix){ - std::ifstream in(filename_prefix + "cnt.txt"); - int cnt = 0; - if (in.is_open()) - { - in >> cnt; - in.close(); +bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect){ + if(!is_gimble_connect){ + video_gimble = new CameraWrapper(0, "armor"); + if(!(is_gimble_connect = video_gimble->init())){ + delete video_gimble; + video_gimble = nullptr; + } } - if(cnt > 1) std::string video_name = filename_prefix + std::to_string(cnt) + ".avi"; + if(!is_chassis_connect){ + video_chassis = new CameraWrapper(1, "energy"); + if(!(is_chassis_connect = video_chassis->init())){ + delete video_chassis; + video_chassis = nullptr; + } + } + return is_gimble_connect && is_chassis_connect; } + +bool checkReconnect(bool is_gimble_connect){ + if(!is_gimble_connect){ + video_gimble = new CameraWrapper(0, "armor"); + if(!(is_gimble_connect = video_gimble->init())){ + delete video_gimble; + video_gimble = nullptr; + } + } + return is_gimble_connect; +} + +auto gimble_video_writer = initVideoWriter(PROJECT_DIR"/gimble_video/"); +auto chassis_video_writer = initVideoWriter(PROJECT_DIR"/chassis_video/"); + +void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src){ + if(!gimble_src.empty() && !chassis_src.empty()){ + gimble_video_writer.write(gimble_src); + Mat chassis_save = chassis_src.clone(); + cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR); + chassis_video_writer.write(chassis_save); + } + else if(!gimble_src.empty() && chassis_src.empty()){ + gimble_video_writer.write(gimble_src); + } + else if(gimble_src.empty() && !chassis_src.empty()){ + Mat chassis_save = chassis_src.clone(); + cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR); + chassis_video_writer.write(chassis_save); + } + else return; +} + +void saveVideos(const cv::Mat &gimble_src){ + if(!gimble_src.empty()){ + gimble_video_writer.write(gimble_src); + } + else return; +} + +void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src){ + if(!gimble_src.empty() && !chassis_src.empty()){ + imshow("gimble", gimble_src); + imshow("chassis", chassis_src); + } + else if(!gimble_src.empty() && chassis_src.empty()){ + imshow("gimble", gimble_src); + } + else if(gimble_src.empty() && !chassis_src.empty()){ + imshow("chassis", chassis_src); + } + else return; + cv::waitKey(1); +} + +void showOrigin(const cv::Mat &gimble_src){ + if(!gimble_src.empty()){ + imshow("gimble", gimble_src); + } + else return; + cv::waitKey(1); +} \ No newline at end of file diff --git a/others/src/camera/camera_wrapper.cpp b/others/src/camera/camera_wrapper.cpp index 3d4a6cc..b45982c 100644 --- a/others/src/camera/camera_wrapper.cpp +++ b/others/src/camera/camera_wrapper.cpp @@ -136,6 +136,7 @@ bool CameraWrapper::readRaw(cv::Mat &src) { return true; } else { + src = cv::Mat(); return false; } } @@ -155,6 +156,7 @@ bool CameraWrapper::readProcessed(cv::Mat &src) { CameraReleaseImageBuffer(h_camera, pby_buffer); return true; } else { + src = cv::Mat(); return false; } }