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

开发环境

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 功能包的作用

1. 这个功能包可以通过zed的深度信息实时显示darknet检测到的boundingbox的中心点到zed摄像头的距离信息
2. 将检测到距离信息存入到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 1

void 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 np

class 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 e

        depth_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/2
        v = depth_data.height/2

        print('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)