VSAG/vision_sjtu_19
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

energy changed

Browse Source
This commit is contained in:
sun
2019-07-05 23:13:18 +08:00
parent ba85a1e27f
commit 5fb6db440b
10 changed files with 243 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
energy/include/energy/energy.h
View File

@@ -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();//记录操作手标定的云台初始角度

2
energy/src/energy/param_init.cpp
View File

@@ -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;

60
energy/src/energy/run.cpp
View File

@@ -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();
@@ -30,30 +41,24 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
// 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: "<<fans_cnt<<endl;
if(fans_cnt==-1) return 0;//滤去漏判的帧
// if(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: "<<armors_cnt<<endl;
if(armors_cnt==-1) return 0;//滤去漏判的帧
// if(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,6 +68,8 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
getArmorPolarAngle();
findTarget();
if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
if (energy_rotation_init) {
initRotation();
return 0;
@@ -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){
}

29
energy/src/energy/show/show.cpp
View File

@@ -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++)
for (int i = 0; i < 4; i++){
if(pointDistance(static_cast<cv::Point2f>(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);
// cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle;
// cout << endl;
}
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
// ---------------------------------------------------------------------------------------------------------------------

12
energy/src/energy/tool/tool.cpp
View File

@@ -59,3 +59,15 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){
point_2.x = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0);
point_2.y = static_cast<float >( 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<double>(point_1.x - point_2.x),2)
+ pow(static_cast<double>(point_1.y - point_2.y),2));
return distance;
}

188
main.cpp
View File

@@ -26,16 +26,23 @@ using namespace std;
mcu_data mcuData = {
0,
0,
BIG_ENERGY_STATE,
ARMOR_STATE,
0,
1,
ENEMY_RED,
};
int main(int argc, char *argv[]) {
process_options(argc, argv);
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);
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"<<endl;
do {
CNT_TIME("Total", {
if (mcuData.state == BIG_ENERGY_STATE) {
if (last_state != BIG_ENERGY_STATE) {
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);
// energy.extract(energy_src);
// }
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
energy.setEnergyRotationInit();
cout << "set" << endl;
}
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;
energy.runBig(gimble_src, chassis_src);//击打大符
}
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;
}
}
}
else if (mcuData.state == SMALL_ENERGY_STATE) {
energy.runSmall(gimble_src, chassis_src);//击打小符
}
last_state = mcuData.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;
}
}
}
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);
}
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(armor_src);
armorFinder.run(gimble_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;

8
others/include/additions/additions.h
View File

@@ -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_ */

1
others/include/camera/wrapper_head.h
View File

@@ -15,7 +15,6 @@ public:
virtual ~WrapperHead() = default;;
virtual bool init() = 0;
virtual bool read(cv::Mat &src) = 0;
};

105
others/src/additions/additions.cpp
View File

@@ -4,14 +4,30 @@
#include <cstring>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <additions/additions.h>
#include <camera/camera_wrapper.h>
#include <log.h>
#include <opencv2/videoio/videoio_c.h>
#include <iostream>
#include <energy/energy.h>
#include <armor_finder/armor_finder.h>
#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);
}

2
others/src/camera/camera_wrapper.cpp
View File

@@ -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;
}
}