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

文章目录

1 实现方法
2 实现代码
3 运行结果
参考

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 0ros::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消息传递——自定义消息

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

ROS开发系列（6）- zed深度信息与darknet的boundingbox信息融合
文章目录开发环境 1 darknet_ros安装 2 zed_ros安装 3.vscode下创建zed深度信息与darknet的boundingbox信息融合的功能包 3.1 功能包的创建方法 3. ...
ROS开发系列（1）- ROS基础知识
1.常用指令 1.1 roscore 启动一个roscore就相当于是开启了一个rosmaster,也就是管理器 1.2 rosrun rosrun指令的第一个参数是功能包的名字,如果这个时候双击ta ...
send函数获得已发送数据长度_蓝牙BLE开发1--起因与回调函数
1 起因笔者在开发蓝牙芯片CC2540,试图取实现大量数据的BLE透传功能,在认真学习了蓝牙4.0协议标准以及TI提供的SDK说明,我总结了两个发送函数,以及两个接收函数的位置. 两个发送函数包括 ...
spring返回数据使用ajax,【spring 后台跳转前台】使用ajax访问的后台，后台正常执行，返回数据，但是不能进入前台的ajax回调函数中...
问题1: 使用ajax访问的后台,后台正常执行,并且正常返回数据,但是不能进入前台的ajax回调函数中问题展示: 问题解决: 最后发现是因为后台的方法并未加注解:@ResponseBody,导致方法 ...
C++回调函数中调用Python函数出现的死锁问题调试及解决
一.查找死锁原因: 1.使用gdb exe指令进入gdb命令行,再输入r运行可执行文件 gdb /home/sdhm/catkin_ws/devel/lib/gpd_ros/gpd_server GN ...
来自Android客户端什么意思,如何通过回调函数中的Node.js来自Android客户端
我想从插座Android客户端将数据发送到服务器的Node.js .. 在服务器端做了什么香港专业教育学院:如何通过回调函数中的Node.js来自Android客户端 socket.on('new u ...
JavaScript 回调函数中的 return false 问题
今天一个同事问了我一个问题,就是在 Ajax 方法中,请求成功后(success)的回调函数中根据响应的值来判断程序是否继续执行,他不解的是在回调函数中已经 return false 了,但是 Aja ...
回调函数中window.open()被拦截
在回调函数中window.open默认是会被拦截的,因为浏览器判断它不是用户自己打开的,存在安全风险,所以可以伪造一个用户点击事件来避开,代码如下: function newWindow(url, i ...
python线程池回调函数_python回调函数中使用多线程的方法
下面的demo是根据需求写的简单测试脚本 #!/usr/bin/env python # coding: utf-8 # 第一个列表为依赖组件和版本号,后面紧跟负责人名称 # 接着出现第二个以来组建列 ...