ROS开发系列（6）- zed深度信息与darknet的boundingbox信息融合

文章目录

开发环境
1 darknet_ros安装
2 zed_ros安装
3.vscode下创建zed深度信息与darknet的boundingbox信息融合的功能包
- 3.1 功能包的创建方法
- 3.2 功能包的作用
- 3.3 程序实现
3.4 可能遇到的问题
4 番外
5 待解决的问题
参考

开发环境

ubuntu16.04
显卡型号：GTX1650
CUDA10.0
CUDNN7.6.2

1 darknet_ros安装

安装方法见TX2使用记录系列文章（4）- TX2下安装

2 zed_ros安装

这里安装的SDK版本为了与tx2的统一，所以我也是安装的2.8.5版本的
具体版本链接找到对应2.8版本下的cuda为10的那个下载即可

安装方法见TX2使用记录系列文章（6）-TX2之ZED的使用

3.vscode下创建zed深度信息与darknet的boundingbox信息融合的功能包

3.1 功能包的创建方法

具体创建方法参考：ROS开发系列（2）-ROS工程创建（VSCODE）

3.2 功能包的作用

这个功能包可以通过zed的深度信息实时显示darknet检测到的boundingbox的中心点到zed摄像头的距离信息
将检测到距离信息存入到csv文件中

3.3 程序实现

#include <ros/ros.h>
#include <sensor_msgs/Image.h>#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>#include <boost/thread/thread.hpp>
#include <darknet_ros_msgs/BoundingBox.h>
#include <darknet_ros_msgs/BoundingBoxes.h>#include <fstream>
#include <iostream>
using namespace message_filters;//class_num 默认设定为20，如果检测类别为80 将上面的define 更改下即可
#define class_num  20
#define DEBUG 1void depth_callback(const darknet_ros_msgs::BoundingBoxes::ConstPtr& detect_msg, const sensor_msgs::Image::ConstPtr& msg)
{int u[class_num] = {};int v[class_num] = {};int centerIdx[class_num] = {};int sizes = msg->data.size();float* depths = (float*)(&msg->data[0]);int num = detect_msg->bounding_boxes.size();for (int i = 0; i < num; i++){u[i] = ((detect_msg->bounding_boxes[i].xmax - detect_msg->bounding_boxes[i].xmin)/2) + detect_msg->bounding_boxes[i].xmin; v[i] = ((detect_msg->bounding_boxes[i].ymax - detect_msg->bounding_boxes[i].ymin)/2) + detect_msg->bounding_boxes[i].ymin;centerIdx[i] = u[i] + msg->width * v[i];if (centerIdx[i] < 0) {centerIdx[i] = 0;}else if (centerIdx[i] > sizes /4){centerIdx[i] = sizes /4;}}std::cout<<"Bouding Boxes (header):" << detect_msg->header <<std::endl;std::cout<<"Bouding Boxes (image_header):" << detect_msg->image_header <<std::endl;std::cout<<"Bouding Boxes (Class):" << "\t";std::ofstream write;write.open("result-test.csv", std::ios::app);for (int i = 0; i < num; i++){std::cout << detect_msg->bounding_boxes[i].Class << "\t";std::cout<<"Center distance :  " << depths[centerIdx[i]] << "  m" << std::endl;write << detect_msg->bounding_boxes[i].Class << "," << "中心点距离" <<","<< depths[centerIdx[i]] <<  std::endl;   }write.close();std::cout << "\033[2J\033[1;1H";     // clear terminal
}int main(int argc, char** argv)
{ros::init(argc, argv, "fuse_data");ros::NodeHandle n;message_filters::Subscriber<darknet_ros_msgs::BoundingBoxes> object_sub(n, "/darknet_ros/bounding_boxes", 1);message_filters::Subscriber<sensor_msgs::Image> depth_sub(n, "/zed/zed_node/depth/depth_registered", 1);// 将两个话题的数据进行同步typedef sync_policies::ApproximateTime<darknet_ros_msgs::BoundingBoxes, sensor_msgs::Image> syncPolicy;Synchronizer<syncPolicy> sync(syncPolicy(10), object_sub, depth_sub); // 指定一个回调函数，就可以实现两个话题数据的同步获取sync.registerCallback(boost::bind(&depth_callback, _1, _2));ros::spin();return 0;
}

3.4 可能遇到的问题

1. 缺少头文件问题

这个需要在vscode文件夹下的c_cpp_properties.json文件中添加你的功能包路径，我创建的功能包名字为display_test 所以我在includePath中添加的"/home/zero/catkin_ws/src/display_test/src/**",
并且在编译工程的时候使用catkin_make -DCMAKE_EXPORT_COMPILE_COMMANDS=Yes，然后将"compileCommands": "${workspaceFolder}/build/compile_commands.json"添加到刚才说的json文件中。

{"configurations": [{"browse": {"databaseFilename": "","limitSymbolsToIncludedHeaders": true},"includePath": ["/home/zero/catkin_ws/devel/include/**","/opt/ros/kinetic/include/**","/home/zero/catkin_ws/src/darknet_ros/darknet_ros/include/**","/home/zero/catkin_ws/src/darknet_ros/darknet_ros/src/**",  "/home/zero/catkin_ws/src/darknet_ros/darknet_ros_msgs/**", "/home/zero/catkin_ws/src/display_test/src/**",  "/opt/ros/kinetic/include/ros/ros.h","/opt/ros/kinetic/include/sensor_msgs/Image.h","/usr/include/**","/opt/ros/kinetic/include"],"name": "ROS","intelliSenseMode": "gcc-x64","compilerPath": "/usr/bin/gcc","cStandard": "gnu11","cppStandard": "c++17","compileCommands": "${workspaceFolder}/build/compile_commands.json"}],"version": 4
}

2. 对message_filters::Connection::Connection(boost::function<void ()> const&)未定义的引用这个错误
是由于在cmakelists和package.xml文件中没有加入依赖image_transport导致的。这个是用于用了ROS下的同步功能，message_filters的缘故。

需要在cmakelists中添加

find_package(catkin REQUIRED COMPONENTS...image_transport...
)

在package.xml中添加

<build_depend>image_transport</build_depend>
<exec_depend>image_transport</exec_depend>

4 番外

附上一个忘了在哪里看到的python版本zed读取深度信息的程序

# -*- coding: utf-8 -*-
import roslib
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as npclass depth_processing():def __init__(self):rospy.init_node('zed_depth', anonymous=True)self.bridge = CvBridge()rospy.Subscriber("/zed/depth/depth_registered", Image, self.callback)def callback(self, depth_data):try:depth_image = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")except CvBridgeError, e:print edepth_array = np.array(depth_image, dtype=np.float32)print('Image size: {width}x{height}'.format(width=depth_data.width,height=depth_data.height))u = depth_data.width/2v = depth_data.height/2print('Center depth: {dist} m'.format(dist=depth_array[u,v]))if __name__ == '__main__': try:detector = depth_processing()rospy.spin()except rospy.ROSInterruptException:rospy.loginfo("Detector node terminated.")

5 待解决的问题

现在用vscode可以正常运行代码，但是不知道为什么我用debug的时候就会提示找不到ros.h头文件，如果有知道的大佬们可以告诉我下

我在Cmakelists中添加了SET(CMAKE_BUILD_TYPE Debug)，在c_cpp_properties.json也添加了ros.h的头文件地址。不知道哪里出了问题。
–更新 2020.08.04–
找到问题所在了，是由于设置的launch文件有问题，我设置成了"g++ - 生成和调试活动文件"，而正确的使用方法是使用(gdb) 启动，并且设置的Cmakelists中的SET(CMAKE_BUILD_TYPE Debug)并没有起作用，需要在编译的时候使用catkin_make -DCMAKE_BUILD_TYPE=Debug创建debug模式

参考

官方读取深度信息例程

融合策略

消息同步方法(1)

消息同步方法(2)