956 lines
28 KiB
C++
956 lines
28 KiB
C++
///摄像头
|
||
#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;
|
||
}
|