zqjia
/
Guide-Vest
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Guide-Vest/src/main.cpp

956 lines
28 KiB
C++
Raw Normal View History

整理后代码2024/5/24
2024-05-24 22:06:52 +08:00
///摄像头
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <librealsense2/hpp/rs_processing.hpp>
#include "example.hpp"
#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>
#include <set>
#include <string>
#include <mutex>
#include <sstream>
#include <fstream>
#include <algorithm>
#include <cstring>
// opencv 用于图像数据读取与处理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <opencv2/core/eigen.hpp>
// 使用Eigen的Geometry模块处理3d运动
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
/*........................................................................*/
#include<librealsense2/rsutil.h>
///摄像头
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <librealsense2/hpp/rs_processing.hpp>
#include "../include/example.hpp"
#include <opencv2/opencv.hpp>
/*........................................................................*/
// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h> //条件滤波器头文件
#include <pcl/filters/passthrough.h> //直通滤波器头文件
#include <pcl/filters/radius_outlier_removal.h> //半径滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/voxel_grid.h> //体素滤波器头文件
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/PointIndices.h>
// boost.format 字符串处理
//#include <boost/format.hpp>
#include "Yolo.hpp"
#include "DepthProcessor.hpp"
#include "IMUProcessor.hpp"
// 语音
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include "serialport.h"
#include<map>
#include<chrono>
using namespace std;
// 超声波
#include "../include/dyp-a05/sonar_driver.h"
#include <bitset>
#include <thread>
#include <condition_variable>
#include<functional>
// 蜂鸣器
#include <JetsonGPIO.h>
#include <signal.h>
/* Private variables ---------------------------------------------------------*/
static int addr = 0;
static int error = 0;
static int fdcom = 0;
static PortInfo_t portinfo = {9600, 8, 0, 0, 1, 0};
/*
=========================================
| header | id | 指令 | 校验和 |
-----------------------------------------
| 0x55 0xaa | 0x01 | 0x01 | 0x01 |
=========================================
*/
static unsigned char tx_buffer[TX_BUFFER_MAX] = {0x55, 0xaa, 0x01, 0x01, 0x01};
/*
=========================================
| 帧头 | 地址 | 指令 | 校验和 |
-----------------------------------------
| 0x55 0xaa | 0x01 | 0x01 | 0x01 |
=========================================
*/
static unsigned char rx_buffer[RX_BUFFER_MAX];
static double Distance[4] = {0, 0, 0, 0};
std::string port = "/dev/ultrasound";
/*初始化参数定义*/
int ini = 0;
int initial_times = 10;
bool initial = false;
double max_wrong =2.5;
double min_wrong =0.25;
double dis0[10];
double dis1[10];
bool end_this_program = false;
// 差值
double dis0_d = 0.0;
double dis1_d = 0.0;
// 差值阈值
double Thr = 0.5;
// 播报选择
static int voice_status = 0;
std::mutex mtx;
bool ToPlay = false;
int distt;
bool isObjectt;
// declare
void play_voice(string);
// 检查校验和
int data_check(unsigned char buf[RX_BUFFER_MAX], int datalen)
{
int checksum = 0;
if((buf[0]==0xFF) && datalen == 10)
{
for(int i=0; i<RX_BUFFER_MAX-1; i++)
{
checksum += buf[i];
}
if((checksum&0x00ff)== buf[RX_BUFFER_MAX-1])
{
return 1;
}
}
printf("first : %x,校验和不对, 数据长度datalen: %d\n", buf[0],datalen);
error = 2;
return 0;
}
// 超声波高低八位合并
unsigned int data_merge(unsigned short dt_h, unsigned short dt_l)
{
unsigned int tmp;
tmp = dt_h;
tmp <<= 8;
tmp |= dt_l;
return tmp;
}
// 物体转16进制
map<string, char> Objects = {
{"person", 0x51},
{"bicycle", 0x52},
{"motorcycle", 0x53},
{"bus", 0x54},
{"cat", 0x55},
{"dog", 0x56},
{"fire hydrant", 0x57},
{"bottle", 0x58},
{"cup", 0x59},
{"chair", 0x5a},
{"couch", 0x5b},
{"potted plant", 0x5c},
{"bed", 0x5d},
{"dining table", 0x5e},
{"toilet", 0x5f},
{"tv", 0x60},
{"refrigerator", 0x61},
{"object", 0x6e},
// 待增物体todo
};
// 播报函数
// 10->16进制，conver使用
char tenTo16(int i){
char output = 'a' ;
switch(i){
case 1: output = 0x01; break;
case 2: output = 0x02; break;
case 3: output = 0x03; break;
case 4: output = 0x04; break;
case 5: output = 0x05; break;
case 6: output = 0x06; break;
case 7: output = 0x07; break;
case 8: output = 0x08; break;
case 9: output = 0x09; break;
case 0: output = 0x0e; break;
}
return output;
}
// 待传输数组，结合距离和物体，未用
void conver(char out[], double dis, string obj){
// todo 四舍五入一下 0.29->0.3
int ge = -1,shi=-1,bai=-1,qian=-1, dian= -1, dian2 = -1;;
//char out[] = {0x8b};
//auto *beg = begin(out);
int p = 0;
//printf("qian: %d,bai:%d,shi:%d,ge:%d,dian:%d\n",qian,bai,shi,ge,dian);
//将距离转为十六进制
if(dis >= 1000){
qian = (int)dis/1000;
bai = (int)dis/100%10;
shi = (int)dis/10%10;
ge = (int)dis%10;
}else if(dis >= 100){
bai = (int)dis/100;
shi = (int)dis/10%10;
ge = (int)dis%10;
}else if(dis >= 10){
shi = (int)dis/10;
ge = (int)dis%10;
}else if(dis >=1){
ge = (int)dis%10;
}else{
ge = 0;
}
dian = (int)(dis*10)%10;
dian2 = (int)(dis*100)%10;
//printf("qian: %d,bai:%d,shi:%d,ge:%d,dian:%d\n",qian,bai,shi,ge,dian);
if(qian != -1){
out[p++] = tenTo16(qian);
out[p++] = 0x0c;
out[p++] = tenTo16(bai);
out[p++] = 0x0b;
out[p++] = tenTo16(shi);
out[p++] = 0x0a;
}else if(bai != -1){
out[p++] = tenTo16(bai);
out[p++] = 0x0b;
out[p++] = tenTo16(shi);
out[p++] = 0x0a;
}else if(shi != -1){
out[p++] = tenTo16(shi);
out[p++] = 0x0a;
}
// 个位数
out[p++] = tenTo16(ge);
// 点数
out[p++] = 0x0d;
// 小数点后一位
out[p++] = tenTo16(dian);
// 小数点后两位
out[p++] = tenTo16(dian2);
//printf("p:%d, out[0]:%x\n",p,out[1]);
//将string转换成对应的十六进制
auto iter = Objects.find(obj);
if(iter != Objects.end()){
out[p++] = iter->second;
}
//return out;
}
// 串口发送起始120，十六进制78
bool serial_com(char n16){
SerialPort serialport;
SerialPort::Port_INFO *pPort;
// Init SerialPort
int COM_id = 0;
pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO));
if(pPort == NULL) {
fprintf(stderr, "Error malloc... [FAIL]\n");
}
memset(pPort, 0, sizeof(SerialPort::Port_INFO));
int result = serialport.InitPort(pPort, COM_id);
if(result < 0) {
fprintf(stderr, "Error Running SerialPort... [FAIL]\n");
}
// fprintf(stdout, "SerialPort [%s] running... [OK]\n", pPort->name);
int send_size = 15;
char serial_sendmsg[send_size] = {n16};
int msg_len = strlen(serial_sendmsg);
int send_nbyte = serialport.sendnPort(pPort,const_cast<char*>(serial_sendmsg),msg_len);
if (send_nbyte > 0) {
// 收到数据
printf("Send: [%d] ... [OK]\n", send_nbyte);
return true;
}
return false;
}
// 串口发送接口函数-发送单个物体
bool serial_com(string object){
SerialPort serialport;
SerialPort::Port_INFO *pPort;
// Init SerialPort
int COM_id = 0;
pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO));
if(pPort == NULL) {
fprintf(stderr, "Error malloc... [FAIL]\n");
}
memset(pPort, 0, sizeof(SerialPort::Port_INFO));
int result = serialport.InitPort(pPort, COM_id);
if(result < 0) {
fprintf(stderr, "Error Running SerialPort... [FAIL]\n");
}
// COM Send
// 设置参数
// 物体
//string object= "bed";
// 距离
//double Distance= 5.5;
int send_size = 15;
char serial_sendmsg[send_size] = {};
auto iter = Objects.find(object);
if(iter != Objects.end()){
serial_sendmsg[0] = iter->second;
}
// conver(serial_sendmsg, Distance, object);
int msg_len = strlen(serial_sendmsg);
int send_nbyte = serialport.sendnPort(pPort,const_cast<char*>(serial_sendmsg),msg_len);
if (send_nbyte > 0) {
// todo...
fprintf(stdout, "Send: [%d] %s... [OK]\n", send_nbyte, serial_sendmsg);
return true;
}
return false;
}
// 串口发送接口函数-发送多个物体类别
bool serial_com(set<string> objects){
SerialPort serialport;
SerialPort::Port_INFO *pPort;
/* Init SerialPort */
int COM_id = 0;
pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO));
if(pPort == NULL) {
fprintf(stderr, "Error malloc... [FAIL]\n");
}
memset(pPort, 0, sizeof(SerialPort::Port_INFO));
int result = serialport.InitPort(pPort, COM_id);
if(result < 0) {
fprintf(stderr, "Error Running SerialPort... [FAIL]\n");
}
int send_size = 50;
char serial_sendmsg[send_size] = {0x02};
// conver(serial_sendmsg, distance, object);
int p=0;
for(auto e: objects){
// Objects
auto iter = Objects.find(e);
if(iter != Objects.end()){
// 找到
serial_sendmsg[p++] = iter->second;
}
}
int msg_len = strlen(serial_sendmsg);
int send_nbyte = serialport.sendnPort(pPort,const_cast<char*>(serial_sendmsg),msg_len);
if (send_nbyte > 0) {
// todo...
fprintf(stdout, "Send: [%d] %s... [OK]\n", send_nbyte, serial_sendmsg);
return true;
}
return false;
}
// 串口接收函数
void serial_receive(char response_buf[]){
SerialPort serialport;
SerialPort::Port_INFO *pPort;
// Init SerialPort
int COM_id = 0;
pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO));
if(pPort == NULL) {
fprintf(stderr, "Error malloc... [FAIL]\n");
}
memset(pPort, 0, sizeof(SerialPort::Port_INFO));
int result = serialport.InitPort(pPort, COM_id);
if(result < 0) {
fprintf(stderr, "Error Running SerialPort... [FAIL]\n");
}
// COM Receive
int receive_nbyte = 0;
int size = COM_BUFFER_SIZE;
// char response_buf[COM_BUFFER_SIZE] = {'\0'};
while (1) {
// 收取一次
receive_nbyte = serialport.signal_recvnPort(pPort, response_buf, size);
if(receive_nbyte > 0) {
for(int i = 0; i < receive_nbyte; i++) {
response_buf[i] = response_buf[i];
}
response_buf[receive_nbyte] = '\0';
break;
}
// if (receive_nbyte == 0) break;
}
// fprintf(stdout, "Receive: [%d] %s... [OK]\n", receive_nbyte, response_buf);
}
// 为了剔除类别中不属于检测出的类别
std::vector<std::string> currentLabel= {
"person", "bicycle", "motorcycle", "bus", "cat", "dog", "fire hydrant",
"bottle", "cup", "chair", "couch", "potted plant", "bed",
"dining table", "toilet", "tv", "refrigerator",
};
// 检查label是否在currentLabel中
bool labelCheck(std::string label){
// 使用 std::find() 在 vector 中查找目标字符串
auto it = std::find(currentLabel.begin(), currentLabel.end(), label);
if(it != currentLabel.end()) {
return 1;
}else{
return 0;
}
}
// 超声波检测线程函数
void ultrasound_play(){
fdcom = uart_init(port.c_str());
std::cout << fdcom << std::endl;
if(fdcom > 0)
{
uart_set(fdcom, &portinfo);
printf("[板卡%d通信参数]: %d, 8, 0, 0, 1, 0.\n",addr,9600);
}
while (true)
{
if(error)
{
Distance[0] = 0;
Distance[1] = 0;
Distance[2] = 0;
Distance[3] = 0;
// std::cout<< "串口打开错误！"<<std::endl;
uart_deinit(fdcom);
fdcom = 0;
// 尝试恢复
while(fdcom <= 0 ){
fdcom = uart_init(port.c_str());
printf("超声波串口打开错误,正在恢复..........\n");
}
error = 0;
if(fdcom > 0)
{
uart_set(fdcom, &portinfo);
printf("fdcom:%d重启成功！！！！！！！！！！\n", fdcom);
}
continue;
}
else
{
uart_txd(fdcom, tx_buffer, TX_BUFFER_MAX);
// 接收数据
int datalen = uart_rxd(fdcom, rx_buffer, RX_BUFFER_MAX, portinfo.baudrate);
if(data_check(rx_buffer, datalen))
{
Distance[0] = double(data_merge(rx_buffer[1], rx_buffer[2]))/1000;// 前一个为high，后一个为low
Distance[1] = double(data_merge(rx_buffer[3], rx_buffer[4]))/1000;
Distance[2] = double(data_merge(rx_buffer[5], rx_buffer[6]))/1000;
Distance[3] = double(data_merge(rx_buffer[7], rx_buffer[8]))/1000;
// std::cout<<"Distance[0]:"<<Distance[0]<<"米 "<<"Distance[1]:"<<Distance[1]<<"米 "<<"Distance[2]:"<<Distance[2]<<"米 "<<"Distance[3]:"<<Distance[3]<<"米 "<<std::endl;
if(((Distance[0] <= Thr)&&(Distance[0]>min_wrong)) || ((Distance[1] <= Thr)&&(Distance[1]>min_wrong))){
// 定义全局变量
{
std::cout<<"Distance[0]:"<<Distance[0]<<"米 "<<"Distance[1]:"<<Distance[1]<<"米 "<<"Distance[2]:"<<Distance[2]<<"米 "<<"Distance[3]:"<<Distance[3]<<"米 "<<std::endl;
ToPlay = true;
voice_status = 3;
}
continue;
}
else ToPlay = false;
}
}
}
uart_deinit(fdcom);
}
// 要播报的物体类别
// 播报前赋值，播报完清空
set<string> classes = {};
// 播报全部类别, 新开一个线程等待串口的指令
void play_all_class(){
// 接收到202就发送全部物体类别
char resp[COM_BUFFER_SIZE];
std::string resp_s;
while(1){
usleep(200000);
serial_receive(resp);
resp_s = resp;
if(resp_s == "202"){
if(classes.empty()){
// std::cout<< "wbw111"<< std::endl;
if(!serial_com(0x73)){
std::cout<< "115发送失败,没有物体不播报"<< std::endl;
}
}else{
voice_status = 4;
// 任意传入，不会播报传入的物体
play_voice("cup");
}
}
}
}
// 语音播报 play_voice(label)
void play_voice(string object){
// 上锁
std::lock_guard<std::mutex> lock(mtx);
// 发送开始的200
if(!serial_com(0x78)){
std::cout<< "200发送失败,此次不播报"<< std::endl;
}
switch(voice_status){
case 1: {
// 发送距离和障碍物
if(serial_com(object)){
std::cout << "finish_object_label" << std::endl;
}
break;
}
case 2:{
// 发送距离
if(serial_com("object")){
std::cout << "finish_object" << std::endl;
}
break;
}
case 3:{
// 超声波检测,传入无用的一个字符，语音不做处理
if(serial_com(0x0f)){
std::cout << "finish_ultrasound" << std::endl;
}
break;
}
case 4:{
// 全部类别播报
if(serial_com(classes)){
std::cout<< classes.size() << std::endl;
std::cout << "finish_all_classes" << std::endl;
classes.clear();
}
break;
}
case 0:{
std::cout << "仍在初始化阶段" << std::endl;
}
}
}
// 蜂鸣器播报
void signalHandler(int s) { end_this_program = true; }
void Bee()// 启动
{
int output_pin = 13;
GPIO::setmode(GPIO::BCM);
//signal(SIGINT, signalHandler);
GPIO::setup(output_pin, GPIO::OUT, GPIO::LOW);
std::cout << "Bee" << std::endl;
while(!end_this_program){
if(ToPlay || (isObjectt && distt <= 0.5))
{
// set pin as an output pin with optional initial state of HIGH
GPIO::output(output_pin, GPIO::HIGH);
usleep(100000);// 300000
GPIO::output(output_pin, GPIO::LOW);
usleep(100000);
}else if(isObjectt && distt > 0.5){
GPIO::output(output_pin, GPIO::HIGH);
usleep(100000);// 300000
GPIO::output(output_pin, GPIO::LOW);
usleep(300000);
}
else GPIO::output(output_pin, GPIO::LOW);
}
GPIO::cleanup();
}
int main(){
rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
// 声明 RealSense 管道，封装实际设备和传感器
rs2::pipeline cameraPipe, imuPipe;
// 使用推荐的默认配置开始流式传输
// 默认视频配置包含深度流和色彩流
// 如果设备能够流式传输 IMU 数据，则默认启用陀螺仪和加速计
rs2::config cameraCfg, imuCfg;
// IMU数据流设置帧率会报错，或是帧率设置过大？
imuCfg.enable_stream(rs2_stream::RS2_STREAM_ACCEL, rs2_format::RS2_FORMAT_MOTION_XYZ32F);
imuCfg.enable_stream(rs2_stream::RS2_STREAM_GYRO, rs2_format::RS2_FORMAT_MOTION_XYZ32F);
// Declare object that handles camera pose calculations
IMUProcessor imuProcessor;
rs2::pipeline_profile imuProfile = imuPipe.start(imuCfg, [&](rs2::frame frame)
{
// Cast the frame that arrived to motion frame
auto motion = frame.as<rs2::motion_frame>();
// If casting succeeded and the arrived frame is from gyro stream
if (motion && motion.get_profile().stream_type() == RS2_STREAM_GYRO && motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F)
{
// Get the timestamp of the current frame
double ts = motion.get_timestamp();
// Get gyro measures
rs2_vector gyroData = motion.get_motion_data();
// Call function that computes the angle of motion based on the retrieved measures
imuProcessor.process_gyro(gyroData, ts);
}
// If casting succeeded and the arrived frame is from accelerometer stream
if (motion && motion.get_profile().stream_type() == RS2_STREAM_ACCEL && motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F)
{
// Get accelerometer measures
rs2_vector accelData = motion.get_motion_data();
// Call function that computes the angle of motion based on the retrieved measures
imuProcessor.process_accel(accelData);
}
});
cameraCfg.enable_stream(rs2_stream::RS2_STREAM_COLOR, 640, 480, rs2_format::RS2_FORMAT_BGR8, 30);
cameraCfg.enable_stream(rs2_stream::RS2_STREAM_DEPTH, 640, 480, rs2_format::RS2_FORMAT_Z16, 30);
rs2::pipeline_profile cameraProfile = cameraPipe.start(cameraCfg);
// 创建一个对齐模块
rs2::align align2Depth(RS2_STREAM_DEPTH);
// 对掩码进行腐蚀，定义腐蚀核
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
// 创建yolov8
Yolo yoloV8("/home/jetson/tmp/obstacle-detection-new1/workspace/best.transd.engine");
// 创建点云处理器
string cfgPath = "";
cv::Mat mK(4, 4, CV_32F);
DepthProcessor depthProcessor(cfgPath, mK);
depthProcessor.fx = 388.588;
depthProcessor.fy = 388.588;
depthProcessor.cx = 322.771;
depthProcessor.cy = 236.947;
bool isObject = false, isLabel = false;
std::string label = "";
auto startTime = std::chrono::high_resolution_clock::now();
int count = 1;
// 初始化时必须让摄像头看到地面
rs2::frameset allFrames = cameraPipe.wait_for_frames();
float3 theta = imuProcessor.get_theta();
rs2::frameset alignedFrames = align2Depth.process(allFrames);
rs2::depth_frame depthFrame = alignedFrames.get_depth_frame();
rs2::video_frame colorFrame = alignedFrames.get_color_frame();
//获取深度图的长宽
int widthD = depthFrame.get_width();
int heightD = depthFrame.get_height();
cv::Mat depthMat(cv::Size(widthD, heightD), CV_16U, (void*)depthFrame.get_data(), cv::Mat::AUTO_STEP);
// 将彩色图像转换为cv::Mat类型
int widthC = colorFrame.get_width();
int heightC = colorFrame.get_height();
cv::Mat colorMat(cv::Size(widthC, heightC), CV_8UC3, (void*)colorFrame.get_data(), cv::Mat::AUTO_STEP);
// 定义变换矩阵（平移和旋转）
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translation() << 0.0, 0.0, 0.0; // 平移
transform.rotate(Eigen::AngleAxisf(-theta.x, Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(theta.z + PI_FL/2, Eigen::Vector3f::UnitX()));
int init_count = 0;
float tmp_height = 0;
// 关闭蜂鸣器
int output_pin_ = 13;
GPIO::setmode(GPIO::BCM);
//signal(SIGINT, signalHandler);
GPIO::setup(output_pin_, GPIO::OUT, GPIO::LOW);
GPIO::output(output_pin_, GPIO::LOW);
//相机初始化 10次
while(true){
if(!depthProcessor.camHighInit(depthMat, transform)){
std::cout << "初始化失败" << std::endl;
}
else{
tmp_height += depthProcessor.camHigh;
init_count ++;
if(init_count == 10)
break;
}
theta = imuProcessor.get_theta();
transform = Eigen::Affine3f::Identity();
transform.translation() << 0.0, 0.0, 0.0;
transform.rotate(Eigen::AngleAxisf(-theta.x, Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(theta.z + PI_FL/2, Eigen::Vector3f::UnitX()));
allFrames = cameraPipe.wait_for_frames();
alignedFrames = align2Depth.process(allFrames);
depthFrame = alignedFrames.get_depth_frame();
depthMat = cv::Mat(cv::Size(widthD, heightD), CV_16U, (void*)depthFrame.get_data(), cv::Mat::AUTO_STEP);
}
depthProcessor.camHigh = tmp_height / init_count;
std::cout << "相机初始化:" << depthProcessor.camHigh << std::endl;
// 超声波模块参数初始化
for(int i = 0 ; i < initial_times; i++)// 先初始化数组里的数为0
{
dis0[i] = 0.0;
dis1[i] = 0.0;
}
// 创建超声波线程
std::thread ultrasonicThread(ultrasound_play);
ultrasonicThread.detach();
// 创建蜂鸣器线程
std::thread BeeThread(Bee);
BeeThread.detach();
// // classes
std::thread playAllClassThread(play_all_class);
playAllClassThread.detach();
// 相机主循环
while(true) {
//等待来自摄像机的下一组图像
rs2::frameset allFrames = cameraPipe.wait_for_frames();
float3 theta = imuProcessor.get_theta();
// 对齐深度图像和彩色图像
rs2::frameset alignedFrames = align2Depth.process(allFrames);
// 获取对齐后的深度图像和彩色图像
rs2::depth_frame depthFrame = alignedFrames.get_depth_frame();
rs2::video_frame colorFrame = alignedFrames.get_color_frame();
// 检查是否成功获取数据
if (depthFrame && colorFrame) {
auto start = std::chrono::high_resolution_clock::now();
// 将深度图像转换为cv::Mat类型
int widthD = depthFrame.get_width();
int heightD = depthFrame.get_height();
cv::Mat depthMat(cv::Size(widthD, heightD), CV_16U, (void*)depthFrame.get_data(), cv::Mat::AUTO_STEP);
// 将彩色图像转换为cv::Mat类型
int widthC = colorFrame.get_width();
int heightC = colorFrame.get_height();
cv::Mat colorMat(cv::Size(widthC, heightC), CV_8UC3, (void*)colorFrame.get_data(), cv::Mat::AUTO_STEP);
// 定义变换矩阵（平移和旋转）
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translation() << 0.0, 0.0, 0.0; // 平移
transform.rotate(Eigen::AngleAxisf(-theta.x, Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(theta.z + PI_FL/2, Eigen::Vector3f::UnitX()));
isObject = false;
// 在深度图中检测最近障碍物,返回是否存在障碍物，0不存在，1存在
try {
isObject = depthProcessor.obstacleDetect(depthMat, transform);
if(isObject)
{
isObjectt = true;
distt = depthProcessor.dist;
}
else isObjectt = false;
}
catch (const std::exception& e) {
std::cerr << "Caught exception: " << e.what() << std::endl;
isObject = false;
}
// YOLOv8分割+检测
BoxArray objs = yoloV8.detect(colorMat);
if(isObject) {
cv::rectangle(colorMat, cv::Point(depthProcessor.min_x, depthProcessor.min_y),
cv::Point(depthProcessor.max_x, depthProcessor.max_y),
cv::Scalar(0, 0, 255), 1);
cv::String text = cv::format("dis:%.2f", depthProcessor.dist);
cv::putText(colorMat, text, cv::Point(depthProcessor.min_x, depthProcessor.min_y+10),
0, 0.7, cv::Scalar(0,0,255), 1, 8);
}
float tmp_depth = 100;
if(!classes.empty()){
classes.clear();
}
// 输出识别框信息
for(Box &obj : objs) {
cout << yoloV8.cocoLabels[obj.classLabel] << " ";
}
cout << endl;
for(Box &obj : objs) {
float depth = 0;
int i = 0;
cv::Mat mask(obj.seg->height, obj.seg->width, CV_8U, obj.seg->data);
int objWidth = obj.right - obj.left;
int objHeight = obj.bottom - obj.top;
cv::resize(mask, mask, cv::Size(objWidth, objHeight));
// 进行腐蚀操作
cv::erode(mask, mask, kernel);
// 将掩码绘制到RGB图像上
//cv::Rect roi(obj.left, obj.top, obj_width, obj_height);
//cv::Mat maskColor;
//cv::cvtColor(mask, maskColor, cv::COLOR_GRAY2BGR);
//maskColor.copyTo(colorMat(roi));
for(int x = 0; x < objWidth; x++){
for(int y = 0 ;y < objHeight; y++){
if(mask.at<uchar>(y, x) > 10){
uint16_t d = depthMat.at<uint16_t>(obj.top + y, obj.left + x);
if(d > 300) {
depth += depthMat.at<uint16_t>(obj.top + y, obj.left + x);
i++;
}
}
}
}
depth = depth/(i*1000);
if(isObject){
if (depthProcessor.max_x > obj.left && depthProcessor.min_x < obj.right
&& depthProcessor.max_y > obj.top && depthProcessor.min_y < obj.bottom){
double rate = depthProcessor.IOU(depthProcessor.max_x, depthProcessor.min_x, depthProcessor.max_y, depthProcessor.min_y,
obj.right, obj.left, obj.bottom, obj.top);
if(rate >= 0.3){
isLabel = true;
if(tmp_depth > depth){
tmp_depth = depth;
label = yoloV8.cocoLabels[obj.classLabel];
}
else isLabel = false;
if(!labelCheck(label)){
// 如果标签不在所需要的标签中
isLabel = false;
}
}
}
}
// if(/*isObject && */isLabel){
// // if(abs(depthProcessor.dist - depth) > 0.2){
// // isLabel = false;
// // std::cout << " depthProcessor.dist = " << depthProcessor.dist << " depth =" << depth << std::endl;
// // }
// // else
// if(tmp_depth > depth){
// tmp_depth = depth;
// std::cout << "something near" <<std::endl;
// }
// }
// 要播报的全部类别
classes.insert(yoloV8.cocoLabels[obj.classLabel]);
cv::String depthText = cv::format("d:%.2f", depth);
cv::putText(colorMat, depthText, cv::Point(obj.left, obj.top+10), 0, 0.7, cv::Scalar(0,255,0), 1, 8);
}
auto endTime = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::seconds>(endTime - startTime);
// 语音播报,间隔大于等于秒在进行下一次
if(duration.count() >= 7 || count == 1){
if(ToPlay){
// 超声波播报
std::cout << std::endl << "************************有障碍物;dis0_d: " << dis0_d << "|| dis1_d: " << dis1_d << std::endl;
// play_voice("object");
// Bee(300000);
// 回置全局变量
// ToPlay = false;
voice_status = 0;
startTime = std::chrono::high_resolution_clock::now();
count++;
}else{
// 相机播报
if(isObject && isLabel){
isLabel = false;
std::cout << std::endl;
std::cout << "前方" << depthProcessor.dist << "米存在" << label << std::endl;
{
voice_status = 1;
}
play_voice(label);
voice_status = 0;
startTime = std::chrono::high_resolution_clock::now();
count++;
}else if(isObject){
std::cout << std::endl;
std::cout << "前方" << depthProcessor.dist << "米有障碍物"<< std::endl;
{
voice_status = 2;
}
play_voice(label);
voice_status = 0;
startTime = std::chrono::high_resolution_clock::now();
count++;
}
}
}
isLabel = false;
// 回置count
if(count == 100){
count = 2;
}
cv::imshow("RGB Image", colorMat);
if (cv::waitKey(5) == 27) break;
auto end = std::chrono::high_resolution_clock::now();
auto duration_test = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
std::cout << "Time taken: " << duration_test << " microseconds" << std::endl;
}
}
return 0;
}