ros2下回调函数不调用的解决方法(相关经验ros1也适用)

问题描述

在跑通这个代码的过程遇到了诸多问题，值得记录下来(事后总结，图片较少)：

https://github.com/berkealgul/ros2-octomap-depth-mapping

这个ros节点主要订阅两个话题：一个是深度图，image类型，一个是pose，pose类型

之后我也确实发布了这两个话题，但是发现该节点的回调函数没有被调用，也就是说这个节点的启动了但是没用。以下是解决思路：

步骤一

在回调函数中添加一些输出语句来确保我们能知道其被调用与否

RCLCPP_INFO(this->get_logger(), "Start updating map");

步骤二

使用相关ros命令查看话题是否被成功发布出来以及被订阅

rostopic list
rqtgraph
rostopic info /话题名

步骤三

确定订阅成功后还是没有作用，怀疑是两个话题发布频率不匹配所致，修改后查看两者的发布频率

rostopic hz /话题名

发现还是没有作用，怀疑是发布的图片格式和订阅端不符，使用echo查看图片格式并修改统一

rostopic echo /话题名

DeclareLaunchArgument('encoding', default_value='16UC1'),

步骤四

以上操作还是没有作用，怀疑是时间戳不同步所致，修改相关的时间同步的语句。

修改前的回调函数：

// 订阅 "image_in" 话题的深度图像消息
depth_sub_.subscribe(this, "image_in", rmw_qos_profile);
RCLCPP_INFO_STREAM(this->get_logger(), "Subscribed to image_in topic with QoS profile");

// 订阅 "pose_in" 话题的位姿消息
pose_sub_.subscribe(this, "pose_in", rmw_qos_profile);
RCLCPP_INFO_STREAM(this->get_logger(), "Subscribed to pose_in topic with QoS profile");

// 创建时间同步器，用于同步 "image_in" 和 "pose_in" 的消息，缓冲区大小为10
sync_ = std::make_shared<message_filters::TimeSynchronizer<sensor_msgs::msg::Image, 
    geometry_msgs::msg::PoseStamped>>(depth_sub_, pose_sub_, 50);
RCLCPP_INFO_STREAM(this->get_logger(), "Created TimeSynchronizer with buffer size 10");

// 注册回调函数，在接收到同步的图像和位姿消息后调用 `demap_callback`
sync_->registerCallback(std::bind(&OctomapDemap::demap_callback, this, ph::_1, ph::_2));
RCLCPP_INFO_STREAM(this->get_logger(), "Callback function registered for TimeSynchronizer");

修改后的回调函数：

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h> // 添加在开头

// 订阅 "image_in" 话题的深度图像消息
depth_sub_.subscribe(this, "image_in", rmw_qos_profile);
RCLCPP_INFO_STREAM(this->get_logger(), "Subscribed to image_in topic with QoS profile");

// 订阅 "pose_in" 话题的位姿消息
pose_sub_.subscribe(this, "pose_in", rmw_qos_profile);
RCLCPP_INFO_STREAM(this->get_logger(), "Subscribed to pose_in topic with QoS profile");

//使用 message_filters 中的 ApproximateTime 策略进行左右图像和深度图像的近似时间同步，该策略将根据消息的时间戳匹配近似时间的左右图像和深度图像
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, geometry_msgs::msg::PoseStamped> approximate_policy;
//使用 Synchronizer 类创建一个同步器，并传入 ApproximateTime 策略和左右图像的订阅器允许消息在时间上有一定的误差（这里设置为100毫秒）
static message_filters::Synchronizer<approximate_policy> syncApproximate(approximate_policy(100), depth_sub_, pose_sub_);
//通过syncApproximate.setMaxIntervalDuration方法设置同步器的最大时间间隔，这里设置为3秒
// syncApproximate.setMaxIntervalDuration(rclcpp::Duration(3,0)); 
//使用 registerCallback 方法注册一个回调函数，该回调函数会在近似时间同步成功后被调用。在回调函数中调用 GrabDepth 方法处理左右图像和深度图像数据
syncApproximate.registerCallback(std::bind(&OctomapDemap::demap_callback, this, ph::_1, ph::_2));

成功