diff --git a/CMakeLists.txt b/CMakeLists.txt index 94e1d0f..d8ce09c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,7 @@ INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include) INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/others/include) FILE(GLOB_RECURSE sourcefiles "others/src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp") -ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} ) +ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} others/include/additions/additions.h) TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT}) TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS}) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index f4dbae2..5321491 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -9,14 +9,11 @@ #include #include #include - -typedef enum{ - ENEMY_BLUE, ENEMY_RED -} EnemyColor; +#include "additions/additions.h" class ArmorFinder{ public: - ArmorFinder(EnemyColor &color, Serial &u, string paras_folder, const bool &use); + ArmorFinder(uint8_t &color, Serial &u, string paras_folder, const uint8_t &use); ~ArmorFinder() = default; private: @@ -26,7 +23,7 @@ private: SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE } State; - const EnemyColor &enemy_color; + const uint8_t &enemy_color; State state; cv::Rect2d armor_box; cv::Ptr tracker; @@ -36,7 +33,7 @@ private: int contour_area; Serial &serial; - const bool &use_classifier; + const uint8_t &use_classifier; bool stateSearchingTarget(cv::Mat &src); bool stateTrackingTarget(cv::Mat &src); diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 7f88257..cb37844 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -8,7 +8,7 @@ #include #include -ArmorFinder::ArmorFinder(EnemyColor &color, Serial &u, string paras_folder, const bool &use) : +ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, string paras_folder, const uint8_t &use) : serial(u), enemy_color(color), state(STANDBY_STATE), diff --git a/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp b/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp index a7a22c9..0c04fca 100644 --- a/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp +++ b/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.cpp @@ -21,7 +21,7 @@ static void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) { } } -void imageColorSplit(cv::Mat &src_input, cv::Mat &split, EnemyColor color) { +void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color) { cv::Mat blue(240, 320, CV_8UC1), red(240, 320, CV_8UC1); if(src_input.type() == CV_8UC1){ splitBayerBG(src_input, blue, red); diff --git a/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.h b/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.h index 69fcecc..61dbbb8 100644 --- a/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.h +++ b/armor/src/armor_finder/state_machine/searching_state/image_process/image_process.h @@ -8,7 +8,7 @@ #include #include -void imageColorSplit(cv::Mat &src_input, cv::Mat &split, EnemyColor color); +void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color); void imagePreProcess(cv::Mat &src); #endif /* _IMAGE_PROCESS_H_ */ diff --git a/energy/include/energy/constant.h b/energy/include/energy/constant.h index cf78798..802c421 100644 --- a/energy/include/energy/constant.h +++ b/energy/include/energy/constant.h @@ -4,10 +4,12 @@ #ifndef CONSTANT_H #define CONSTANT_H +#include "additions/additions.h" + #define d2r (CV_PI / 180.0) -const int ALLY_BLUE = 123; -const int ALLY_RED = 456; +const int ALLY_BLUE = ENEMY_RED; +const int ALLY_RED = ENEMY_BLUE; const int SRC_WIDTH_CAMERA = 640; const int SRC_HEIGHT_CAMERA = 480; const int SRC_WIDTH = 320; diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index b62627e..8bdee63 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -15,12 +15,13 @@ #include "energy/constant.h" #include "energy/param_struct_define.h" #include "serial/serial.h" +#include "additions/additions.h" using std::vector; class Energy { public: - Energy(Serial &u, int &ally_color); + Energy(Serial &u, uint8_t &enemy_color); ~Energy(); int run(cv::Mat &src); @@ -28,7 +29,7 @@ public: clock_t start; Serial &serial; - void setAllyColor(int color); +// void setAllyColor(int color); void setRotation(int rotation); void extract(cv::Mat &src); @@ -51,7 +52,7 @@ private: double last_target_position; double last_hit_position; float target_armor; - int &ally_color_; + uint8_t &ally_color; int energy_part_rotation; float attack_distance; int send_cnt; diff --git a/energy/src/energy/calibrate/split.cpp b/energy/src/energy/calibrate/split.cpp index f0b8049..427fb08 100644 --- a/energy/src/energy/calibrate/split.cpp +++ b/energy/src/energy/calibrate/split.cpp @@ -28,10 +28,10 @@ void Energy::imagePreprocess(cv::Mat &src) { if(src.type() == CV_8UC1) { splitBayerBG(src, src_blue, src_red); - if(ally_color_ == ALLY_RED) + if(ally_color == ALLY_RED) { src = src_red - src_blue; - }else if(ally_color_ == ALLY_BLUE){ + }else if(ally_color == ALLY_BLUE){ src = src_blue - src_red; } } @@ -44,11 +44,11 @@ void Energy::imagePreprocess(cv::Mat &src) { resize(channels.at(0), src_blue, Size(SRC_WIDTH, SRC_HEIGHT)); resize(channels.at(1), src_green, Size(SRC_WIDTH, SRC_HEIGHT)); resize(channels.at(2), src_red, Size(SRC_WIDTH, SRC_HEIGHT)); - if(ally_color_ == ALLY_RED) + if(ally_color == ALLY_RED) { src = src_red-src_blue; //src=src_red; - }else if(ally_color_ == ALLY_BLUE){ + }else if(ally_color == ALLY_BLUE){ src = src_blue-src_red; //src=src_blue; } diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index 6020aa8..2d2a049 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -8,7 +8,7 @@ using std::cout; using std::endl; using std::vector; -Energy::Energy(Serial &u, int &ally_color):serial(u),ally_color_(ally_color), +Energy::Energy(Serial &u, uint8_t &enemy_color):serial(u),ally_color(enemy_color), src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1), src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1) { @@ -26,7 +26,7 @@ Energy::Energy(Serial &u, int &ally_color):serial(u),ally_color_(ally_color), target_armor = -1; radius = 0; - ally_color_ = ALLY_RED; +// ally_color = ALLY_RED; energy_part_rotation = CLOCKWISE; attack_distance = ATTACK_DISTANCE; count = 1; @@ -58,11 +58,11 @@ Energy::Energy(Serial &u, int &ally_color):serial(u),ally_color_(ally_color), Energy::~Energy() = default; - -void Energy::setAllyColor(int color) -{ - ally_color_ = color; -} +// +//void Energy::setAllyColor(int color) +//{ +// ally_color_ = color; +//} void Energy::setRotation(int rotation){ energy_part_rotation = rotation; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 876780d..4e58b52 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -8,8 +8,9 @@ using std::cout; using std::endl; using std::vector; -extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch; -extern int mark; +//extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch; +//extern int mark; + int Energy::run(cv::Mat &src){ imshow("src",src); @@ -78,11 +79,12 @@ int Energy::run(cv::Mat &src){ sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance); cout<<"yaw: "< #include #include - #include #include #include @@ -14,84 +13,44 @@ #include #include #include +#include #include #define DO_NOT_CNT_TIME + #include -#define ENERGY_STATE 1 -#define ARMOR_STATE 0 +#define ENERGY_STATE 'e' +#define ARMOR_STATE 'a' using namespace cv; using namespace std; - -int state = ENERGY_STATE; -float curr_yaw = 0, curr_pitch = 0; -float mark_yaw = 0, mark_pitch = 0; -int mark = 0; -EnemyColor enemy_color = ENEMY_BLUE; -int ally_color = ALLY_RED; -bool use_classifier = true; - -void uartReceive(Serial *uart); +mcu_data mcuData = { + .curr_yaw = 0, + .curr_pitch = 0, + .state = ENERGY_STATE, + .mark = 0, + .use_classifier = 0, + .enemy_color = ENEMY_BLUE +}; int main(int argc, char *argv[]) { process_options(argc, argv); - Serial uart(115200); - thread receive(uartReceive, &uart); - bool flag = true; + Serial serial(115200); + thread receive(uartReceive, &serial); - while (flag) { + int from_camera = 1; + if (!run_with_camera) { + cout << "Input 1 for camera, 0 for video files" << endl; + cin >> from_camera; + } + + while (true) { VideoWriter armor_video_writer, energy_video_writer; - - if (state == ARMOR_STATE) { - string armor_filename_prefix = "../armor_video/"; - ifstream in_1(armor_filename_prefix + "cnt.txt"); - int cnt_1 = 0; - if (in_1.is_open()) - { - in_1 >> cnt_1; - in_1.close(); - } - string armor_file_name = armor_filename_prefix + std::to_string(cnt_1) + ".avi"; - //cout << "recording video to " << armor_file_name << endl; - cnt_1++; - ofstream out_1(armor_filename_prefix + "cnt.txt"); - if (out_1.is_open()) { - out_1 << cnt_1 << endl; - out_1.close(); - } - armor_video_writer.open(armor_file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true); - } - - if (state == ENERGY_STATE) { - string energy_filename_prefix = "../energy_video/"; - ifstream in_2(energy_filename_prefix + "cnt.txt"); - int cnt_2 = 0; - if (in_2.is_open()) - { - in_2 >> cnt_2; - in_2.close(); - } - string energy_file_name = energy_filename_prefix + std::to_string(cnt_2) + ".avi"; - //cout << "recording video to " << armor_file_name << endl; - cnt_2++; - ofstream out_2(energy_filename_prefix + "cnt.txt"); - if (out_2.is_open()) { - out_2 << cnt_2 << endl; - out_2.close(); - } - energy_video_writer.open(energy_file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), false); - } - - - int energy_part_rotation = CLOCKWISE; - - int from_camera = 1; - if (!run_with_camera) { - cout << "Input 1 for camera, 0 for video files" << endl; - cin >> from_camera; + if (save_video) { + initVideoWriter(armor_video_writer, PROJECT_DIR"/armor_video/"); + initVideoWriter(energy_video_writer, PROJECT_DIR"/energy_video/"); } WrapperHead *video_armor; @@ -100,128 +59,100 @@ int main(int argc, char *argv[]) { video_armor = new CameraWrapper(0, "armor"); video_energy = new CameraWrapper(1, "energy"); } else { - video_armor = new VideoWrapper("r_l_640.avi"); - video_energy = new VideoWrapper("r_l_640.avi"); + video_armor = new VideoWrapper("/home/xinyang/Desktop/DataSets/video/blue_4.mp4"); + video_energy = new VideoWrapper("/home/xinyang/Desktop/DataSets/video/blue_4.mp4"); } - if (video_armor->init() && video_energy->init()) { - cout << "Video source initialization successfully." << endl; + if (video_armor->init()) { + LOGM("video_armor source initialization successfully."); } else { + LOGW("video_armor source unavailable!"); delete video_armor; + video_armor = nullptr; + } + if (video_energy->init()) { + LOGM("video_energy source initialization successfully."); + } else { + LOGW("video_energy source unavailable!"); delete video_energy; - cout << "Program fails. Restarting" << endl; - continue; + video_energy = nullptr; } Mat energy_src, armor_src; - for(int i=0; i<10; i++){ - video_armor->read(armor_src); - video_energy->read(armor_src); + for (int i = 0; i < 10; i++) { + if (video_armor) { + video_armor->read(armor_src); + } + if (video_energy) { + video_energy->read(armor_src); + } } - ArmorFinder armorFinder(enemy_color, uart, PROJECT_DIR"/tools/para/", use_classifier); + ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier); - Energy energy(uart, ally_color); + Energy energy(serial, mcuData.enemy_color); // energy.setAllyColor(ally_color); - energy.setRotation(energy_part_rotation); + energy.setRotation(CLOCKWISE); - bool ok = video_armor->read(armor_src) && video_energy->read(armor_src); - - while (ok) { + bool ok = true; + do { CNT_TIME("Total", { - if (state == ENERGY_STATE) { - ok = video_energy->read(energy_src); - energy_video_writer.write(energy_src); - if (show_origin) { - imshow("energy src", energy_src); + if (mcuData.state == ENERGY_STATE) { + if (video_energy) { + ok = video_energy->read(energy_src); + if (!ok) { + delete video_energy; + video_energy = nullptr; + } + if(save_video){ + energy_video_writer.write(energy_src); + } + if (show_origin) { + imshow("energy src", energy_src); + } + if (from_camera == 0) { + energy.extract(energy_src); + } + energy.run(energy_src); + } else { + video_energy = new CameraWrapper(1, "energy"); + if(!video_energy->init()){ + delete video_energy; + video_energy = nullptr; + } } - if (from_camera == 0) { - energy.extract(energy_src); + + } else if (mcuData.state == ARMOR_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; + } } - energy.run(energy_src); - } else { - ok = video_armor->read(armor_src); - 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); - }); - } - if (waitKey(1) == 'q') { - flag = false; - break; } }); - } + } while (ok); + delete video_armor; delete video_energy; cout << "Program fails. Restarting" << endl; } - return 0; } -#define RECEIVE_LOG_LEVEL LOG_NOTHING - -char uartReadByte(Serial &uart) { - char byte; - if (uart.ReadData((uint8_t*)&byte, 1) == false) { - LOGE("serial error!"); - } - return byte; -} - -void uartReceive(Serial* uart) { - char buffer[100]; - int cnt = 0; - LOGM("data receive start!"); - while (true) { - char data; - while ((data = uartReadByte(*uart)) != '\n') { - buffer[cnt++] = data; - if (cnt >= 100) { - LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); - cnt = 0; - } - } - if (cnt == 12) { - if (buffer[8] == 'e') { - state = ENERGY_STATE; - LOG(RECEIVE_LOG_LEVEL, "Energy state"); - } else if (buffer[8] == 'a') { - state = ARMOR_STATE; - LOG(RECEIVE_LOG_LEVEL, "Armor state"); - } - if (buffer[10] == 0){ - use_classifier = false; - LOG(RECEIVE_LOG_LEVEL, "Classifier off!"); - } else if(buffer[10] == 1){ - use_classifier = true; - LOG(RECEIVE_LOG_LEVEL, "Classifier on!"); - } - if (buffer[11] == ENEMY_BLUE) { - LOG(RECEIVE_LOG_LEVEL, "ENEMY_BLUE!"); - ally_color = ALLY_RED; - enemy_color = ENEMY_BLUE; - } else if (buffer[11] == ENEMY_RED) { - LOG(RECEIVE_LOG_LEVEL, "ENEMY_RED!"); - ally_color = ALLY_BLUE; - enemy_color = ENEMY_RED; - } - memcpy(&curr_yaw, buffer, 4); - memcpy(&curr_pitch, buffer + 4, 4); - LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); - if (buffer[9] == 1) { - if (mark == 0) { - mark = 1; - mark_yaw = curr_yaw; - mark_pitch = curr_pitch; - } - LOG(RECEIVE_LOG_LEVEL, "Marked"); - } - } - cnt = 0; - } -} diff --git a/others/include/additions/additions.h b/others/include/additions/additions.h new file mode 100644 index 0000000..8c42c57 --- /dev/null +++ b/others/include/additions/additions.h @@ -0,0 +1,32 @@ +// +// Created by sjturm on 19-5-17. +// + +#ifndef _ADDITIONS_H_ +#define _ADDITIONS_H_ + +#include +#include +#include +#include + +#define ENEMY_BLUE 0 +#define ENEMY_RED 1 + +struct mcu_data{ + float curr_yaw; + float curr_pitch; + uint8_t state; + uint8_t mark; + uint8_t use_classifier; + uint8_t enemy_color; + float mark_yaw; + float mark_pitch; +}; + +extern mcu_data mcuData; + +void uartReceive(Serial *pSerial); +void initVideoWriter(cv::VideoWriter& video, const std::string &armor_filename_prefix); + +#endif /* _ADDITIONS_H_ */ diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp new file mode 100644 index 0000000..7627d8b --- /dev/null +++ b/others/src/additions/additions.cpp @@ -0,0 +1,50 @@ +// +// Created by sjturm on 19-5-17. +// + +#include +#include +#include +#include +#include + +#define RECEIVE_LOG_LEVEL LOG_NOTHING + +void uartReceive(Serial* pSerial) { + char buffer[100]; + int cnt = 0; + LOGM("data receive start!"); + while (true) { + char byte; + while (pSerial->ReadData((uint8_t*)&byte, 1) && byte!='\n') { + buffer[cnt++] = byte; + if (cnt >= 100) { + LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); + cnt = 0; + } + } + if (cnt == sizeof(mcuData)) { + memcpy(&mcuData, buffer, sizeof(mcuData)); + } + cnt = 0; + } +} + +void initVideoWriter(cv::VideoWriter& video, const std::string &armor_filename_prefix){ + std::ifstream in_1(armor_filename_prefix + "cnt.txt"); + int cnt_1 = 0; + if (in_1.is_open()) + { + in_1 >> cnt_1; + in_1.close(); + } + std::string armor_file_name = armor_filename_prefix + std::to_string(cnt_1) + ".avi"; + cnt_1++; + std::ofstream out_1(armor_filename_prefix + "cnt.txt"); + if (out_1.is_open()) { + out_1 << cnt_1 << std::endl; + out_1.close(); + } + video.open(armor_file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true); +} +