120 lines
3.4 KiB
C++
120 lines
3.4 KiB
C++
//
|
||
// Created by sjturm on 19-5-17.
|
||
//
|
||
|
||
#include <cstring>
|
||
#include <iostream>
|
||
#include <fstream>
|
||
#include <opencv2/core/core.hpp>
|
||
#include <opencv2/highgui/highgui.hpp>
|
||
#include <opencv2/imgproc/imgproc.hpp>
|
||
#include <opencv2/videoio/videoio_c.h>
|
||
#include <additions.h>
|
||
#include <camera/camera_wrapper.h>
|
||
#include <energy/energy.h>
|
||
#include <armor_finder/armor_finder.h>
|
||
#include <log.h>
|
||
|
||
#define RECEIVE_LOG_LEVEL LOG_MSG
|
||
|
||
using namespace std;
|
||
using namespace cv;
|
||
|
||
extern WrapperHead *video;
|
||
|
||
extern Serial serial;
|
||
extern uint8_t last_state;
|
||
|
||
extern ArmorFinder armor_finder;
|
||
extern Energy energy;
|
||
|
||
void uartReceive(Serial *pSerial) {
|
||
char buffer[40];
|
||
LOGM(STR_CTR(WORD_LIGHT_WHITE, "data receive start!"));
|
||
while (true) {
|
||
memset(buffer, 0, sizeof(buffer));
|
||
pSerial->ReadData((uint8_t *) buffer, sizeof(mcu_data)+1);
|
||
if (buffer[sizeof(mcu_data)] == '\n') {
|
||
memcpy(&mcu_data, buffer, sizeof(mcu_data));
|
||
// LOGM("Get, state:%c, mark:%d!", mcu_data.state, (int) mcu_data.mark);
|
||
// LOGM("Get yaw: %f, pitch: %f!", mcu_data.curr_yaw, mcu_data.curr_pitch);
|
||
// LOGM("Get delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.delta_y);
|
||
// static int t = time(nullptr);
|
||
// static int cnt = 0;
|
||
// if(time(nullptr) > t){
|
||
// t = time(nullptr);
|
||
// LOGM("fps:%d", cnt);
|
||
// cnt = 0;
|
||
// }else{
|
||
// cnt++;
|
||
// }
|
||
}else{
|
||
// LOGW("corrupt data!");
|
||
}
|
||
}
|
||
}
|
||
|
||
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()) {
|
||
in >> cnt;
|
||
in.close();
|
||
}
|
||
std::string file_name = filename_prefix + std::to_string(cnt) + ".avi";
|
||
cnt++;
|
||
std::ofstream out(filename_prefix + "cnt.txt");
|
||
if (out.is_open()) {
|
||
out << cnt << std::endl;
|
||
out.close();
|
||
}
|
||
video.open(file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true);
|
||
return video;
|
||
}
|
||
|
||
bool checkReconnect(bool is_camera_connect) {
|
||
if (!is_camera_connect) {
|
||
int curr_gain = ((CameraWrapper* )video)->gain;
|
||
int curr_exposure = ((CameraWrapper* )video)->exposure;
|
||
delete video;
|
||
video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
|
||
is_camera_connect = video->init();
|
||
}
|
||
return is_camera_connect;
|
||
}
|
||
|
||
auto video_writer = initVideoWriter(PROJECT_DIR"/video/");
|
||
|
||
void saveVideos(const cv::Mat &gimbal_src) {
|
||
if (!gimbal_src.empty()) {
|
||
video_writer.write(gimbal_src);
|
||
} else return;
|
||
}
|
||
|
||
void showOrigin(const cv::Mat &src) {
|
||
if (!src.empty()) {
|
||
imshow("origin", src);
|
||
cv::waitKey(1);
|
||
} else return;
|
||
}
|
||
|
||
void extract(cv::Mat &src) {//图像预处理，将视频切成640×480的大小
|
||
if (src.empty()) return;
|
||
float length = static_cast<float>(src.cols);
|
||
float width = static_cast<float>(src.rows);
|
||
if (length / width > 640.0 / 480.0) {
|
||
length *= 480.0 / width;
|
||
resize(src, src, cv::Size(length, 480));
|
||
src = src(Rect((length - 640) / 2, 0, 640, 480));
|
||
} else {
|
||
width *= 640.0 / length;
|
||
resize(src, src, cv::Size(640, width));
|
||
src = src(Rect(0, (width - 480) / 2, 640, 480));
|
||
}
|
||
}
|
||
|
||
double getPointLength(const cv::Point2f &p) {
|
||
return sqrt(p.x * p.x + p.y * p.y);
|
||
}
|