devbox
我家小花儿
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

基于Qt和ros的深度相机的识别测距框架_astra pro 深度相机测距

作者：我家小花儿 | 2024-03-16 16:40:55

赞

踩

astra pro 深度相机测距

 效果

实现

本项目采用ros框架，用yolov5作为识别模块，采用奥比中光astra深度相机测距，ui界面显示实时图像，点击显示距离。

代码解释：

思路是把yolov5封装成功能包，用python写一个ros收发接口调用模型。主函数接受yolov5模块识别到的数据，调用深度图像得到距离，再把图像和距离显示在ui界面上。

本程序预留串口通信，可与嵌入式端进行通信，扩展成更多项目。

main.cpp

  1. #include <ros/ros.h>
  2. #include <sensor_msgs/Image.h>
  3. #include <sensor_msgs/image_encodings.h>
  4. #include <cv_bridge/cv_bridge.h>
  5. #include <iostream>
  6. #include <opencv2/opencv.hpp>
  7. #include <opencv2/imgproc/imgproc.hpp>
  8. #include <image_transport/image_transport.h>
  9. #include <sensor_msgs/Image.h>
  10. #include <sensor_msgs/CameraInfo.h>
  11. #include <opencv2/highgui/highgui.hpp>
  12. #include <cv_bridge/cv_bridge.h>
  13. #include <message_filters/subscriber.h>
  14. #include <message_filters/time_synchronizer.h>
  15. #include<message_filters/sync_policies/approximate_time.h>
  16. #include <pcl/point_cloud.h>
  17. #include<thread>
  18. #include<pcl/point_types.h>
  19. #include<pcl_conversions/pcl_conversions.h>
  20. #include<pcl/visualization/cloud_viewer.h>
  21. #include <sensor_msgs/PointCloud2.h>
  22. #include "widget.h"
  23. #include <QApplication>
  24. #include "std_msgs/String.h"
  25. using namespace std;
  26. using namespace sensor_msgs;
  27. using namespace message_filters;
  28. using namespace cv;
  29. typedef pcl::PointXYZRGB PointT;
  30. typedef pcl::PointCloud<PointT> testcloud;
  31. //#include<realsense_dev/depth_value.h>       //自定义一个消息类型
  32. cv::Mat depth_pic;        //定义全局变量，图像矩阵Ｍat形式
  33. String some="someee";
  34. char some1[30];
  35. //realsense_dev::depth_value command_to_pub;   //待发布数据
  36. double x,y,z;
  37. int n=0;
  38.  
  39. void depthCallback(const sensor_msgs::Image::ConstPtr&msg)
  40. {
  41.     cv_bridge::CvImagePtr Dest ;
  42.     Dest = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::TYPE_16UC1);
  43.  
  44.     depth_pic = Dest->image;
  45.     //cout<<"Output some info about the depth image in cv format"<<endl;
  46.     //cout<< "Rows of the depth iamge = "<<depth_pic.rows<<endl;                       //获取深度图的行数height
  47.     //cout<< "Cols of the depth iamge = "<<depth_pic.cols<<endl;                           //获取深度图的列数width
  48.     //cout<< "Type of depth_pic's element = "<<depth_pic.type()<<endl;             //深度图的类型
  49.     ushort Ld = depth_pic.at<ushort>(command_to_pub2.x+5,command_to_pub2.y+5);           //读取深度值，数据类型为ushort单位为ｍｍ
  50.     ushort Rd = depth_pic.at<ushort>((command_to_pub2.x1+command_to_pub2.x)/2,(command_to_pub2.y1+command_to_pub2.y)/2);           //读取深度值，数据类型为ushort单位为ｍｍ
  51.      //ushort Ld = depth_pic.at<ushort>(400,788);           //读取深度值，数据类型为ushort单位为ｍｍ
  52.     //ushort Rd = depth_pic.at<ushort>(406,800);           //读取深度值，数据类型为ushort单位为ｍｍ
  53.     Rd_value = float(Rd)/1000 ;      //强制转换
  54.     Ld_value = float(Ld)/1000 ;      //强制转换
  55.     cout<< "Value of Rdepth_pic's pixel= "<<Rd_value<< "Value of Ldepth_pic's pixel= "<<Ld_value<<endl;    //读取深度值
  56. }
  57.  
  58. void imageCallback(const sensor_msgs::ImageConstPtr& msg)
  59. {
  60. // clock_t start,ends;
  61. // start=clock();
  62.     try
  63.     {
  64.         cv::Mat image = cv_bridge::toCvShare(msg, "bgra8")->image; //image_raw就是我们得到的图像了
  65.         int rows = image.rows;//std::cout<<"rows="<<rows<<std::endl;
  66.         int cols = image.cols;//std::cout<<"cols="<<cols<<std::endl;
  67.         cvimage=image;
  68.     }
  69.     catch (cv_bridge::Exception &e)
  70.     {
  71.         ROS_ERROR("Could not conveextrinsicMat_RT from '%s' to 'rgb8'.", msg->encoding.c_str());
  72.     }
  73. // ends=clock();
  74. // std::cout<<"img call:"<<(double (ends-start)/CLOCKS_PER_SEC)<<std::endl;
  75. }
  76.  
  77. void leidaCallback(const std_msgs::String::ConstPtr& msg_p)
  78. {
  79. ROS_INFO("%s",msg_p->data.c_str());
  80. some=msg_p->data.c_str();
  81. //ROS_INFO("我听见:%s",(*msg_p).data.c_str());
  82. char x[6],y[6],x1[6],y1[6];
  83. int i;
  84. for(i=0;i<some.length();i++)
  85. {
  86.     if(some[i-1]=='F'  && some[i]=='C')
  87.     {
  88.         command_to_pub1.Value[0]=some[i+1];
  89.         command_to_pub1.Value[1]=some[i+2]; 
  90.     }
  91.     if(some[i-1]=='y')  //x y q w h
  92.     {
  93.         for(int q=0;q<6;q++)
  94.         {
  95.                 if(some[i+q]!= '.' )
  96.                 {
  97.                         y[q]=some[i+q];
  98.                         if(some[i+q]== 'q' )
  99.                         {
  100.                             break;
  101.                         }
  102.  
  103.                 }  
  104.                 else
  105.                 {
  106.                     continue;
  107.                 }
  108.         }
  109.         
  110.     }
  111.     if(some[i-1]=='x')
  112.     {
  113.        for(int q=0;q<6;q++)
  114.         {
  115.                 if(some[i+q]!= '.' )
  116.                 {
  117.                         x[q]=some[i+q];
  118.                         if(some[i+q]== 'y' )
  119.                         {
  120.                             break;
  121.                         }
  122.                 }  
  123.                 else
  124.                 {
  125.                     continue;
  126.                 }
  127.         }
  128.     }
  129.     if(some[i-1]== 'w'   )
  130.     {
  131.        for(int q=0;q<6;q++)
  132.         {
  133.                 if(some[i+q]!= '.' )
  134.                 {
  135.                         y1[q]=some[i+q];
  136.                         if(some[i+q]== 'h' )
  137.                         {
  138.                             break;
  139.                         }
  140.  
  141.                 }  
  142.                 else
  143.                 {
  144.                     continue;
  145.                 }
  146.         }
  147.     }
  148.     if( some[i-1]== 'q' )
  149.     {
  150.        for(int q=0;q<6;q++)
  151.         {
  152.                 if(some[i+q]!= '.' )
  153.                 {
  154.                         x1[q]=some[i+q];
  155.                         if(some[i+q]== 'w' )
  156.                         {
  157.                             break;
  158.                         }
  159.  
  160.                 }  
  161.                 else
  162.                 {
  163.                     continue;
  164.                 }
  165.         }
  166.     }
  167. }
  168.     command_to_pub2.x = atoi(x);
  169.     command_to_pub2.y= atoi(y);
  170.     command_to_pub2.x1= atoi(x1);
  171.     command_to_pub2.y1= atoi(y1);
  172.     printf("\nsome:%d,%d,%d,%d",command_to_pub2.x,command_to_pub2.y,command_to_pub2.x1,command_to_pub2.y1);
  173. }
  174.  
  175.  
  176. int uuu(int argc, char *argv[])
  177. {
  178.     QApplication a(argc, argv);
  179.     Widget w;
  180.     w.show();
  181.     return a.exec();
  182. }
  183. void ttt(int argc, char *argv[])
  184. {
  185.     ros::init(argc, argv, "stream_pub");               // ros节点初始化
  186.     ros::NodeHandle nh;                                           //创建节点句柄
  187.      image_transport::ImageTransport it(nh);
  188.     ros::AsyncSpinner spinner(1);
  189.     spinner.start();
  190.     ros::Subscriber sub1 = nh.subscribe<std_msgs::String>("/leida_num",1,leidaCallback);
  191.     ros::Subscriber image_sub = nh.subscribe<sensor_msgs::Image>("/camera/depth/image_raw",1,depthCallback);   //订阅深度图像
  192.     image_transport::Subscriber sub = it.subscribe("/camera/color/image_raw", 1, &imageCallback);
  193.     //ros::Subscriber element_sub = nh.subscribe<sensor_msgs::Image>("/camera/aligned_depth_to_color/image_raw",100,pixelCallback);     //订阅像素点坐标
  194.     //ros::Publisher mode_pub = nh.advertise<realsense_dev::depth_value>("/depth_info", 10);
  195.  
  196.     //command_to_pub.Value = 0;    //初始化深度值
  197.     ros::Rate rate(20.0);    //设定自循环的频率
  198.     while(ros::ok)
  199.     {
  200.         //command_to_pub.header.stamp      = ros::Time::now();
  201.         //command_to_pub.Value = d_value;     //depth_pic.rows/2,depth_pic.cols/2  为像素点
  202.     }
  203.     
  204.     ros::Duration(10).sleep();    //设定自循环的频率
  205. }
  206. int main(int argc,char *argv[])
  207. {
  208.     int yt;
  209.     thread th2(uuu,argc, argv);
  210.     thread th3(ttt,argc, argv);
  211.     //uuu(argc, argv);
  212.     th2.join();
  213.     th3.join();
  214.     return yt;
  215. }

widget.cpp片段

  1. void Widget::showww()
  2. {
  3.     Rect rect(command_to_pub2.x,command_to_pub2.y,
  4.      command_to_pub2.x1-command_to_pub2.x,command_to_pub2.y1-command_to_pub2.y);
  5.     Mat mat = cvimage;
  6.     //cvtColor( mat,  mat, COLOR_BGR2RGB);//BGR转化为RGB
  7.     QImage::Format format = QImage::Format_ARGB32;
  8.     switch ( mat.type())
  9.     {
  10.     case CV_8UC1:
  11.       format = QImage::Format_Indexed8;
  12.       break;
  13.     case CV_8UC3:
  14.       format = QImage::Format_RGB888;
  15.       break;
  16.     case CV_8UC4:
  17.       format = QImage::Format_ARGB32;
  18.       break;
  19.     }
  20.     QImage image = QImage((const uchar*) mat.data,  mat.cols,  mat.rows,
  21.        mat.cols *  mat.channels(), format);
  22.     /*cvtColor(mat, mat, CV_BGR2RGB);
  23.     QImage image(mat.data, 
  24. 			 mat.cols, 
  25. 			 mat.rows, 
  26. 			 mat.step, 
  27. 			 QImage::Format_RGB888);*/
  28.     //QString filename("/home/q/图片/标定二维码/image/1.bmp");
  29.     QImage* img=new QImage;
  30.     img=&image;
  31.     ui->labImgShow->setPixmap(QPixmap::fromImage(*img));
  32.     timer->start(20);//启动计时器
  33. }
  34.  
  35. void Widget::text()
  36. {
  37.     float uio=5.8;
  38.     String utf5=to_string( Ld_value);
  39.     char  utf8[20];
  40.     strcpy( utf8,utf5.c_str());
  41.     QString str2=QString::fromUtf8(utf8);
  42.     ui->Ldepth_textEdit->append(str2); //接收区显示 -- 追加方式
  43.     ui->Ldepth_textEdit->show();
  44.     String utf6=to_string( Rd_value);
  45.     char  utf9[20];
  46.     strcpy( utf9,utf6.c_str());
  47.     QString str3=QString::fromUtf8(utf9);
  48.     ui->Rdepth_textEdit->append(str3); //接收区显示 -- 追加方式
  49.     ui->Rdepth_textEdit->show();
  50.     String utf59= "people";
  51.     char  utf89[20];
  52.     strcpy( utf89,utf59.c_str());
  53.     QString str26=QString::fromUtf8(utf89);
  54.     ui->name_textEdit->append(str26); //接收区显示 -- 追加方式
  55.     ui->name_textEdit->show();
  56. }

yolov5识别片段

  1. # src=cv2.imread('biye.jpg')
  2. def detect(img):
  3.     time1 = time.time()
  4.  
  5.     global ros_image
  6.     global xytoxy
  7.     global label_name
  8.     cudnn.benchmark = True
  9.     dataset = loadimg(img)
  10.     # print(dataset[3])
  11.     # plt.imshow(dataset[2][:, :, ::-1])
  12.     names = model.module.names if hasattr(model, 'module') else model.names
  13.     # colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))]
  14.     # colors=[[0,255,0]]
  15.     augment = 'store_true'
  16.     conf_thres = 0.3
  17.     iou_thres = 0.45
  18.     classes = (0, 1, 2, 3, 5, 7)
  19.     agnostic_nms = 'store_true'
  20.     img = torch.zeros((1, 3, imgsz, imgsz), device=device)  # init img
  21.     _ = model(img.half() if half else img) if device.type != 'cpu' else None  # run once
  22.     path = dataset[0]
  23.     img = dataset[1]
  24.     im0s = dataset[2]
  25.     vid_cap = dataset[3]
  26.     img = torch.from_numpy(img).to(device)
  27.     img = img.half() if half else img.float()  # uint8 to fp16/32
  28.     img /= 255.0  # 0 - 255 to 0.0 - 1.0
  29.  
  30.     time2 = time.time()
  31.     if img.ndimension() == 3:
  32.         img = img.unsqueeze(0)
  33.     # Inference
  34.     pred = model(img, augment=augment)[0]
  35.     # Apply NMS
  36.     pred = non_max_suppression(pred, conf_thres, iou_thres, classes=classes, agnostic=agnostic_nms)
  37.  
  38.     view_img = 1
  39.     save_txt = 1
  40.     save_conf = 'store_true'
  41.     time3 = time.time()
  42.  
  43.     for i, det in enumerate(pred):  # detections per image
  44.         p, s, im0 = path, '', im0s
  45.         s += '%gx%g ' % img.shape[2:]  # print string
  46.         gn = torch.tensor(im0.shape)[[1, 0, 1, 0]]  # normalization gain whwh
  47.         if det is not None:
  48.             # print(det)
  49.             # Rescale boxes from img_size to im0 size
  50.             det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round()
  51.             # Print results
  52.             for c in det[:, -1].unique():
  53.                 n = (det[:, -1] == c).sum()  # detections per class
  54.                 s += '%g %ss, ' % (n, names[int(c)])  # add to string
  55.                 # Write results
  56.             for *xyxy, conf, cls in reversed(det):
  57.                 if save_txt:  # Write to file
  58.                     xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()  # normalized xywh
  59.                     line = (cls, conf, *xywh) if save_conf else (cls, *xywh)  # label format
  60.                     y = torch.tensor(xyxy).view(1, 4)
  61.                     xy = y.clone() if isinstance(y, torch.Tensor) else np.copy(y)
  62.                     xytoxy = xy.view(-1).tolist()
  63.                 if view_img:  # Add bbox to image
  64.                     label = '%s %.2f' % (names[int(cls)], conf)
  65.                     label_name = names[int(cls)]
  66.                     plot_one_box(xyxy, im0, label=label, color=[0, 255, 0], line_thickness=3)
  67.     time4 = time.time()
  68.     cv2.putText(im0, str(fps), (20, 20), 0, 0.75, [225, 255, 255], thickness=3, lineType=cv2.LINE_AA)
  69.     print('************')
  70.     print('2-1', time2 - time1)
  71.     print('3-2', time3 - time2)
  72.     print('4-3', time4 - time3)
  73.     print('total', time4 - time1)
  74.     print('xy2xy', xytoxy[0])
  75.     print("label_name:", label_name)
  76.     some = "FC" + str(label_name) + "x" + str(xytoxy[0]) + "y" + str(xytoxy[1]) + "q" + str(xytoxy[2]) + "w" + str(xytoxy[3]) +"h"+ "RA"
  77.     print("label_name:", some)
  78.     out_img = im0[:, :, [2, 1, 0]]
  79.     ros_image = out_img
  80.     cv2.imshow('YOLOV5', out_img)
  81.     a = cv2.waitKey(1)
  82.     #### Create CompressedIamge ####
  83.     # publish_image(im0)
  84.     publish_image1(some)

 CmakeList.cpp

  1. cmake_minimum_required(VERSION 3.5)
  2. project(picture2pcl)
  3.  
  4. ## Compile as C++11, supported in ROS Kinetic and newer
  5. #add_compile_options(-std=c++11)
  6. set(CMAKE_CXX_STANDARD 11)
  7. set(CMAKE_CXX_STANDARD_REQUIRED True)
  8.  
  9. set(CMAKE_AUTOUIC ON)
  10. set(CMAKE_AUTOMOC ON)
  11. set(CMAKE_AUTORCC ON)
  12.  
  13. find_library(librealsense REQUIRED)
  14.  
  15. find_package(catkin REQUIRED COMPONENTS
  16.   geometry_msgs
  17.   roscpp
  18.   rospy
  19.   std_msgs
  20.   cv_bridge
  21.   message_generation
  22.   image_transport
  23.   pcl_ros
  24.   sensor_msgs
  25.   tf
  26.   
  27. )
  28. find_package(Eigen3 REQUIRED)
  29. find_package(PCL REQUIRED)
  30. #find_package(realsense2 REQUIRED)
  31. find_package(OpenCV REQUIRED)
  32.  
  33.  
  34.  
  35. catkin_package(
  36. #  INCLUDE_DIRS include
  37. #  LIBRARIES realsense_dev
  38.    CATKIN_DEPENDS roscpp std_msgs 
  39.    DEPENDS EIGEN3 PCL OpenCV
  40.   INCLUDE_DIRS
  41. #  DEPENDS system_lib
  42. )
  43.  
  44.  
  45. include_directories(
  46. # include
  47.   ${catkin_INCLUDE_DIRS}
  48.   ${OpenCV_INCLUDE_DIRS}
  49.   ${EIGEN3_INCLUDE_DIR}
  50. 	${PCL_INCLUDE_DIRS}
  51. )
  52.  
  53. #add_executable(stream_pub src/stream_pub.cpp)
  54. #target_link_libraries(stream_pub ${catkin_LIBRARIES} ${OpenCV_LIBS})
  55.  
  56.  
  57.  
  58. #add_executable(main src/main.cpp  src/serial_communication.cpp )
  59.  
  60. #find_package(Qt5 COMPONENTS Widgets REQUIRED)
  61. #find_package(Qt5 COMPONENTS Core SerialPort REQUIRED)
  62. find_package(Qt5Widgets REQUIRED)
  63. find_package(Qt5Core REQUIRED)
  64. find_package(Qt5Gui REQUIRED)
  65. find_package(Qt5SerialPort REQUIRED)
  66. if(ANDROID)
  67.   add_library(tongxin11 SHARED
  68.     src/main.cpp  
  69.     src/serial_communication.cpp
  70.     src/serial_communication.h
  71.     src/main.cpp
  72.     src/widget.cpp
  73.     src/widget.h
  74.     src/widget.ui
  75.   )
  76. else()
  77.   add_executable(tongxin11
  78.     src/main.cpp  
  79.     src/serial_communication.cpp
  80.     src/serial_communication.h
  81.     src/main.cpp
  82.     src/main.cpp
  83.     src/widget.cpp
  84.     src/widget.h
  85.     src/widget.ui
  86.   )
  87. endif()
  88.  
  89. target_link_libraries(tongxin11 ${Qt5Core_LIBRARIES}  ${Qt5Widgets_LIBRARIES}  ${Qt5SerialPort_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}  -lpthread -luuid)
  90. #target_link_libraries(serialportLIB     ${Qt5Core_LIBRARIES}  ${Qt5Widgets_LIBRARIES}  ${Qt5SerialPort_LIBRARIES})
  91. #target_link_libraries(tongxin11 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
  92.

奥比中光深度相机驱动

奥比中光Orbbec Astra深度相机在ROS Melodic的使用_LCH南安的博客-CSDN博客
 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/我家小花儿/article/detail/250761
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号