1 实现方法
在ROS开发系列(6)- zed深度信息与datknet的boundingbox信息融合基础上进行的修改。
- 创建一个自定义消息类型(
后续单独写一下
) - 更改Cmakelists文件
- 更改package.xml文件
- 在回调函数中发布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]
---