ROS开发系列（7）- 在回调函数中发布topic

1 实现方法

在ROS开发系列（6）- zed深度信息与datknet的boundingbox信息融合基础上进行的修改。

1. 创建一个自定义消息类型（后续单独写一下）
2. 更改Cmakelists文件
3. 更改package.xml文件
4. 在回调函数中发布topic

前3步都是为了实现自定义消息需要的，如果你没有自定义消息的话，直接使用下面的代码即可
具体实现回调函数中发布topic主要就是3个地方进行修改

• 实例化一个publisher，ros::Publisher DistanceInfo_pub;
• 在主函数中添加DistanceInfo_pub = n.advertise<display_test::Pub_class_distance>("/distance_info", 1);
• 在回调函数中发布数据 DistanceInfo_pub.publish(distance_msg);

2 实现代码

#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>

#include "display_test/Pub_class_distance.h"
using namespace message_filters;
//class_num 默认设定为20，如果检测类别为80 将上面的define 更改下即可
#define class_num  20
#define DEBUG 0

ros::Publisher DistanceInfo_pub;


void depth_callback(const darknet_ros_msgs::BoundingBoxes::ConstPtr& detect_msg, const sensor_msgs::Image::ConstPtr& msg)
{
    display_test::Pub_class_distance distance_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;       
        std::cout<<"X distance :  " << u[i] << "  pixel" << std::endl;
        std::cout<<"Y distance :  " << v[i] << "  pixel" << std::endl;
              
        else  //逗号不用非得单独在一个<< <<区间中写如，可以在名称后面添加  "Y distance :  ,"，因为csv文件靠的是逗号来分割，不管在哪里写 都会分开的
            write << detect_msg->bounding_boxes[i].Class << "," << detect_msg->bounding_boxes[i].id << "," << "Center distance :  " <<","<< depths[centerIdx[i]] <<"," << "X distance :  " << "," << u[i] <<"," << "Y distance :  ," << v[i] <<  std::endl;
 
        distance_msg.classes.push_back(detect_msg->bounding_boxes[i].Class);
        distance_msg.id.push_back(detect_msg->bounding_boxes[i].id);
        distance_msg.x_center.push_back(u[i]);
        distance_msg.y_center.push_back(u[i]);
        distance_msg.distance.push_back(depths[centerIdx[i]]);  
    }
    write.close();
 
    DistanceInfo_pub.publish(distance_msg);
    // 使用完之后需要将数据清掉 如果这个消息变量定义在函数体的外面的话，在每次回调函数中使用完之后需要clear下，否则数据是累加的，因为使用了vector的push_back的方法插入的数据
    // distance_msg.classes.clear();   
    // distance_msg.distance.clear();  
    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));

    DistanceInfo_pub = n.advertise<display_test::Pub_class_distance>("/distance_info", 1);

    ros::spin();
    return 0;
}

3 运行结果

使用的是yolov3的权重，得到的测试结果
自定义的消息类型数据

zero@zero:~/catkin_ws$ rosmsg show display_test/Pub_class_distance 
string[] classes
float32[] distance
int64[] x_center
int64[] y_center
int16[] id

数据输出结果

zero@zero:~/catkin_ws$ rostopic echo /distance_info 
classes: [orange]
distance: [0.44432592391967773]
x_center: [350]
y_center: [350]
---
classes: [person, orange, diningtable]
distance: [nan, nan, 0.30334311723709106]
x_center: [1015, 348, 643]
y_center: [1015, 348, 643]
id: [0, 49, 60]
---
classes: [bed]
distance: [-inf]
x_center: [623]
y_center: [623]
id: [59]
---

参考

message_filters同步点云和图像数据

ROS自定义msg类型及使用

ROS学习（五）ROS消息传递——自定义消息