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

auto-aim

Browse Source
This commit is contained in:
xinyang
2019-04-14 17:12:43 +08:00
commit 42c5434dc8
47 changed files with 6423 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

230
src/camera/camera_wrapper.cpp Normal file
View File

@@ -0,0 +1,230 @@
//
// Created by zhikun on 18-11-7.
//
#include <camera/camera_wrapper.h>
using std::cout;
using std::endl;
using namespace cv;
CameraWrapper::CameraWrapper()
{
camera_cnts = 2;
camera_status0 = -1;
camera_status1 = -1;
iplImage0 = nullptr;
iplImage1 = nullptr;
channel0 = 3;
channel1 = 3;
}
bool CameraWrapper::init() {
CameraSdkInit(1);
//枚举设备，并建立设备列表
int camera_enumerate_device_status = CameraEnumerateDevice(camera_enum_list, &camera_cnts);
//cout<<"camera enumerate device status: "<<camera_enumerate_device_status<<endl;
//cout<<"camera number: "<<camera_cnts<<endl;
//没有连接设备
if (camera_cnts == 0) {
cout<<"No device detected!"<<endl;
return false;
}
else if(camera_cnts == 1)
{
cout<<"Only one camera device detected"<<endl;
return false;
}
else if(camera_cnts == 2)
{
cout<<"Two camera devices detected."<<endl;
}
else
{
cout<<"More than 2 cameras detected or some other error occurs."<<endl;
return false;
}
//相机初始化。初始化成功后，才能调用任何其他相机相关的操作接口
camera_status0 = CameraInit(&camera_enum_list[0], -1, -1, &h_camera0);
//初始化失败
if (camera_status0 != CAMERA_STATUS_SUCCESS) {
cout<<"Camera 0 initialization failed with code "<<camera_status0<<". See camera_status.h to find the code meaning."<<endl;
return false;
}
camera_status1 = CameraInit(&camera_enum_list[1], -1, -1, &h_camera1);
if (camera_status1 != CAMERA_STATUS_SUCCESS) {
cout<<"Camera 1 initialization failed with code "<<camera_status1<<". See camera_status.h to find the code meaning."<<endl;
return false;
}
CameraGetFriendlyName(h_camera0, camera_name0);
CameraGetFriendlyName(h_camera1, camera_name1);
cout<<"camera names: "<<camera_name0<<" "<<camera_name1<<endl;
// cout<<camera_name0<<endl;
// cout<<camera_name1<<endl;
//如果读取的相机列表不是0在左，1在右，则交换相机句柄
// if(strcmp(camera_name0, "camera0") != 0)
// {
// swapCameraHandle();
// }
//获得相机的特性描述结构体。该结构体中包含了相机可设置的各种参数的范围信息。决定了相关函数的参数
CameraGetCapability(h_camera0, &tCapability0);
CameraGetCapability(h_camera1, &tCapability1);
// set resolution to 320*240
// CameraSetImageResolution(hCamera, &(tCapability.pImageSizeDesc[2]));
rgb_buffer0 = (unsigned char *)malloc(tCapability0.sResolutionRange.iHeightMax *
tCapability0.sResolutionRange.iWidthMax * 3);
rgb_buffer1 = (unsigned char *)malloc(tCapability1.sResolutionRange.iHeightMax *
tCapability1.sResolutionRange.iWidthMax * 3);
CameraSetAeState(h_camera0, true); //设置是否自动曝光
CameraSetAeState(h_camera1, true);
/*让SDK进入工作模式，开始接收来自相机发送的图像
数据。如果当前相机是触发模式，则需要接收到
触发帧以后才会更新图像。 */
CameraPlay(h_camera0);
CameraPlay(h_camera1);
/*其他的相机参数设置
例如 CameraSetExposureTime CameraGetExposureTime 设置/读取曝光时间
CameraSetImageResolution CameraGetImageResolution 设置/读取分辨率
CameraSetGamma、CameraSetConrast、CameraSetGain等设置图像伽马、对比度、RGB数字增益等等。
CameraGetFriendlyName CameraSetFriendlyName 获取/设置相机名称（该名称可写入相机硬件）
*/
// double exposure_time0, exposure_time1;
// CameraGetExposureTime(h_camera0, &exposure_time0);
// CameraGetExposureTime(h_camera1, &exposure_time1);
// cout<<"exposure time "<<exposure_time0<<" "<<exposure_time1<<endl;
// 抗频闪
CameraSetAntiFlick(h_camera0, true);
CameraSetAntiFlick(h_camera1, true);
if (tCapability0.sIspCapacity.bMonoSensor) {
channel0 = 1;
CameraSetIspOutFormat(h_camera0, CAMERA_MEDIA_TYPE_MONO8);
cout<<"camera0 mono "<<endl;
} else {
channel0 = 3;
CameraSetIspOutFormat(h_camera0, CAMERA_MEDIA_TYPE_BGR8);
cout<<"camera0 color mode"<<endl;
}
if (tCapability1.sIspCapacity.bMonoSensor) {
channel1 = 1;
CameraSetIspOutFormat(h_camera1, CAMERA_MEDIA_TYPE_MONO8);
cout<<"camera1 mono "<<endl;
} else {
channel1 = 3;
CameraSetIspOutFormat(h_camera1, CAMERA_MEDIA_TYPE_BGR8);
cout<<"camera1 color mode"<<endl;
}
return true;
}
bool CameraWrapper::read(cv::Mat& src0, cv::Mat& src1) {
return readRaw(src0, src1); //suit for using bayer hacking in armor_finder to replace process, fast and it can filter red and blue.
//return readProcessed(src0, src1); // processed color image, but this runs slowly, about half fps of previous one.
}
bool CameraWrapper::readRaw(cv::Mat &src0, cv::Mat &src1) {
if (CameraGetImageBuffer(h_camera0, &frame_info0, &pby_buffer0, 1000) == CAMERA_STATUS_SUCCESS &&
CameraGetImageBuffer(h_camera1, &frame_info1, &pby_buffer1, 1000) == CAMERA_STATUS_SUCCESS)
{
if (iplImage0) {
cvReleaseImageHeader(&iplImage0);
}
if (iplImage1){
cvReleaseImageHeader(&iplImage1);
}
iplImage0 = cvCreateImageHeader(cvSize(frame_info0.iWidth, frame_info0.iHeight), IPL_DEPTH_8U, 1);
iplImage1 = cvCreateImageHeader(cvSize(frame_info1.iWidth, frame_info1.iHeight), IPL_DEPTH_8U, 1);
cvSetData(iplImage0, pby_buffer0, frame_info0.iWidth); //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
cvSetData(iplImage1, pby_buffer1, frame_info1.iWidth);
src0 = cv::cvarrToMat(iplImage0);
src1 = cv::cvarrToMat(iplImage1);
//在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
//否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
CameraReleaseImageBuffer(h_camera0, pby_buffer0);
CameraReleaseImageBuffer(h_camera1, pby_buffer1);
return true;
} else {
return false;
}
}
bool CameraWrapper::readProcessed(cv::Mat &src0, cv::Mat &src1) {
if (CameraGetImageBuffer(h_camera0, &frame_info0, &pby_buffer0, 1000) == CAMERA_STATUS_SUCCESS &&
CameraGetImageBuffer(h_camera1, &frame_info1, &pby_buffer1, 1000) == CAMERA_STATUS_SUCCESS)
{
CameraImageProcess(h_camera0, pby_buffer0, rgb_buffer0, &frame_info0); // this function is super slow, better not to use it.
CameraImageProcess(h_camera1, pby_buffer1, rgb_buffer1, &frame_info1);
if (iplImage0) {
cvReleaseImageHeader(&iplImage0);
}
if (iplImage1){
cvReleaseImageHeader(&iplImage1);
}
iplImage0 = cvCreateImageHeader(cvSize(frame_info0.iWidth, frame_info0.iHeight), IPL_DEPTH_8U, channel0);
iplImage1 = cvCreateImageHeader(cvSize(frame_info1.iWidth, frame_info1.iHeight), IPL_DEPTH_8U, channel1);
cvSetData(iplImage0, rgb_buffer0, frame_info0.iWidth * channel0); //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
cvSetData(iplImage1, rgb_buffer1, frame_info1.iWidth * channel1);
src0 = cv::cvarrToMat(iplImage0);
src1 = cv::cvarrToMat(iplImage1);
//在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
//否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
CameraReleaseImageBuffer(h_camera0, pby_buffer0);
CameraReleaseImageBuffer(h_camera1, pby_buffer1);
return true;
} else {
return false;
}
}
CameraWrapper::~CameraWrapper()
{
CameraUnInit(h_camera0);
CameraUnInit(h_camera1);
//注意，先反初始化后再free
free(rgb_buffer0);
free(rgb_buffer1);
}
void CameraWrapper::swapCameraHandle() {
int tmp_h_camera = h_camera0;
h_camera0 = h_camera1;
h_camera1 = tmp_h_camera;
}

23
src/camera/video_wrapper.cpp Normal file
View File

@@ -0,0 +1,23 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "camera/video_wrapper.h"
VideoWrapper::VideoWrapper(const std::string &filename0, const std::string &filename1) {
video0.open(filename0);
video1.open(filename1);
}
VideoWrapper::~VideoWrapper() = default;
bool VideoWrapper::init() {
return video0.isOpened() && video1.isOpened();
}
bool VideoWrapper::read(cv::Mat &src_left, cv::Mat &src_right) {
return video0.read(src_left) && video1.read(src_right);
}

67
src/options/additions.cpp Normal file
View File

@@ -0,0 +1,67 @@
//
// Created by xinyang on 19-4-7.
//
#include <options/additions.h>
#include <options/options.h>
#include <stdio.h>
#include <log.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
using namespace std;
#define VIDEO_SAVE_DIR "/home/sjturm/Desktop/video/"
static cv::VideoWriter *create_video_writer(){
FILE* info = fopen(VIDEO_SAVE_DIR"info.txt", "r");
int cnt=0;
fscanf(info, "%d", &cnt);
fclose(info);
info = fopen(VIDEO_SAVE_DIR"info.txt", "w");
fprintf(info, "%d", ++cnt);
char name[100];
sprintf(name, VIDEO_SAVE_DIR"%d.avi", cnt);
return new cv::VideoWriter(name, cv::VideoWriter::fourcc('P','I','M','1'), 80, cv::Size(640,480),false);
}
void save_video_file(cv::Mat &src){
static cv::VideoWriter *video = create_video_writer();
video->write(src);
}
#define SAVE_DIR "/home/sjturm/Desktop/labelled/"
int get_labelled_cnt(){
FILE *fp = fopen(SAVE_DIR"info.txt", "r");
int cnt=0;
fscanf(fp, "%d", &cnt);
fclose(fp);
return cnt+1;
}
void save_labelled_cnt(int cnt){
FILE *fp = fopen(SAVE_DIR"info.txt", "w");
fprintf(fp, "%d", cnt);
fclose(fp);
}
void save_labelled_image(cv::Mat &src, cv::Rect2d box){
static int cnt=get_labelled_cnt();
char name[50];
sprintf(name, SAVE_DIR"%d.jpg", cnt);
cv::imwrite(name, src);
sprintf(name, SAVE_DIR"%d.txt", cnt);
FILE *fp = fopen(name, "w");
if(fp == NULL){
LOGW("Can't create file: %s!\nStop saving!", name);
save_labelled = false;
return;
}
fprintf(fp, "%lf %lf %lf %lf\n", box.x, box.y, box.width, box.height);
fclose(fp);
save_labelled_cnt(cnt);
}

68
src/options/options.cpp Normal file
View File

@@ -0,0 +1,68 @@
//
// Created by xinyang on 19-3-27.
//
#include <options/options.h>
#include <log.h>
#include <cstring>
bool show_armor_box = false;
bool show_armor_boxes = false;
bool show_light_blobs = false;
bool show_origin = false;
bool save_labelled = false;
bool run_with_camera = false;
bool save_video = false;
bool collect_data = false;
void process_options(int argc, char *argv[]){
if(argc >= 2){
for(int i=1; i<argc; i++){
if(strcmp(argv[i], "--help") == 0){
LOGM("--show-armor-box: show the aim box.");
LOGM("--show-armor-boxes: show the candidate aim boxes.");
LOGM("--show-light-blobs: show the candidate light blobs.");
LOGM("--show-origin: show the origin image.");
LOGM("--save-label: save the image when box found.");
LOGM("--run-with-camera: start the program with camera directly without asking.");
LOGM("--save-video: save the video.");
LOGM("--collect-data: collect data sent from mcu.");
}else if(strcmp(argv[i], "--show-armor-box") == 0){
show_armor_box = true;
LOGM("Enable show armor box");
}else if(strcmp(argv[i], "--show-armor-boxes") == 0){
show_armor_boxes = true;
LOGM("Enable show armor boxes");
}else if(strcmp(argv[i], "--show-light-blobs") == 0) {
show_light_blobs = true;
LOGM("Enable show light blobs");
}else if(strcmp(argv[i], "--show-origin") == 0) {
show_origin = true;
LOGM("Enable show origin");
}else if(strcmp(argv[i], "--show-all") ==0 ) {
show_armor_box = true;
LOGM("Enable show armor box");
show_armor_boxes = true;
LOGM("Enable show armor boxes");
show_light_blobs = true;
LOGM("Enable show light blobs");
show_origin = true;
LOGM("Enable show origin");
}else if(strcmp(argv[i], "--save-labeled") == 0){
save_labelled = true;
LOGM("Enable save labeled");
}else if(strcmp(argv[i], "--run-with-camera") == 0){
run_with_camera = true;
LOGM("Run with camera!");
}else if(strcmp(argv[i], "--save-video") == 0){
save_video = true;
LOGM("Save video!");
}else if(strcmp(argv[i], "--collect-data") == 0){
collect_data = true;
LOGM("Enable data collection!");
}else{
LOGW("Unknown option: %s. Use --help to see options.", argv[i]);
}
}
}
}

171
src/uart/uart.cpp Normal file
View File

@@ -0,0 +1,171 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include <uart/uart.h>
#include <energy/param_struct_define.h>
using std::cout;
using std::cerr;
using std::clog;
using std::dec;
using std::endl;
using std::hex;
GMAngle_t aim;
Uart::Uart(){
fd = open("/dev/ttyUSB0", O_RDWR);
if(fd < 0)
{
cerr<<"open port error"<<endl;
return;
}
if(set_opt(fd, 115200, 8, 'N', 1) < 0 )
{
cerr<<"set opt error"<<endl;
return;
}
cout<<"uart port success"<<endl;
buff[0] = 's';
buff[1] = '+';
buff[2] = (0 >> 8) & 0xFF;
buff[3] = 0 & 0xFF;
buff[4] = '+';
buff[5] = (0 >> 8) & 0xFF;
buff[6] = (0 & 0xFF);
buff[7] = 'e';
fps = 0;
cur_time = time(nullptr);
}
int Uart::set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop) {
termios newtio{}, oldtio{};
if (tcgetattr(fd, &oldtio) != 0) {
perror("SetupSerial 1");
return -1;
}
bzero(&newtio, sizeof(newtio));
newtio.c_cflag |= CLOCAL | CREAD;
newtio.c_cflag &= ~CSIZE;
switch (nBits) {
case 7:
newtio.c_cflag |= CS7;break;
case 8:
newtio.c_cflag |= CS8;break;
default:break;
}
switch (nEvent) {
case 'O': //奇校验
newtio.c_cflag |= PARENB;
newtio.c_cflag |= PARODD;
newtio.c_iflag |= (INPCK | ISTRIP);
break;
case 'E': //偶校验
newtio.c_iflag |= (INPCK | ISTRIP);
newtio.c_cflag |= PARENB;
newtio.c_cflag &= ~PARODD;
break;
case 'N': //无校验
newtio.c_cflag &= ~PARENB;
break;
default:break;
}
switch (nSpeed) {
case 2400:
cfsetispeed(&newtio, B2400);
cfsetospeed(&newtio, B2400);
break;
case 4800:
cfsetispeed(&newtio, B4800);
cfsetospeed(&newtio, B4800);
break;
case 9600:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
case 115200:
cfsetispeed(&newtio, B115200);
cfsetospeed(&newtio, B115200);
break;
default:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
}
if (nStop == 1) {
newtio.c_cflag &= ~CSTOPB;
} else if (nStop == 2) {
newtio.c_cflag |= CSTOPB;
}
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 0;
tcflush(fd, TCIFLUSH);
if ((tcsetattr(fd, TCSANOW, &newtio)) != 0) {
perror("com set error");
return -1;
}
printf("set done!\n");
return 0;
}
void Uart::sendTarget(float x, float y, float z) {
static short x_tmp, y_tmp, z_tmp;
time_t t = time(nullptr);
if(cur_time != t)
{
cur_time = t;
cout<<"fps:"<<fps<<", ("<<x<<","<<y<<","<<z<<")"<<endl;
fps = 0;
}
fps += 1;
x_tmp= static_cast<short>(x * (32768 - 1) / 100);
y_tmp= static_cast<short>(y * (32768 - 1) / 100);
z_tmp= static_cast<short>(z * (32768 - 1) / 1000);
buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 'e';
write(fd, buff, 8);
}
// 's' + (x) ( 8bit + 8bit ) + (y) ( 8bit + 8bit ) + (z) ( 8bit + 8bit ) + 'e'
char Uart::receive() {
char data;
while(read(fd, &data, 1) < 1);
return data;
}
void Uart::receive_data() {
char Enemy_Info[6] = {0};
read(fd, &Enemy_Info, 6);
if(Enemy_Info[0]=='s'&&Enemy_Info[5]=='e'){
aim.yaw = static_cast<float>(((Enemy_Info[1]<<8)|(Enemy_Info[2]))*(100.0 / (32768.0 - 1.0)));
aim.pitch = static_cast<float>(((Enemy_Info[3]<<8)|(Enemy_Info[4]))*(100.0 / (32768.0 - 1.0)));
}
else return;
}