everybody. Our team using ROS Humble with Ubuntu 22.04. We have node that is using tf2 message filters to invoke message callbacks after needed transform has been published. The node has three subscriptions: wheel odometry, lidar and camera landmark detections. Last two are received through tf2 message filter according to this guide and wheel odometry is received using basic subscriber.
After implementing this approach we encountered the problem of map data structure corruption. And after long investigation it turned out that all three subscription callbacks can be called simultaneously for some reason. These callbacks are modifying common map object and eventually it leads to crash. Fixed the problem by adding a mutex. This all happens with SingleThreadedExecutor (actually there is no explicitly specified executor, but I believe that SingleThreaded is the default). Tried to provide the same callback group to these subscriptions but it didn't help.
So the question is how is it possible that there are several threads in SingleThreadedExecutor? Or it is intended behavior of message filters and I'm missing something? Haven't found any mention of concurrency in the tf2 message filter documentation.
I can try to provide simple example to reproduce this behavior if it is necessary. Here is the snippet from current code:
void SlamROS::createSubscriptions()
{
auto qos = rclcpp::SensorDataQoS();
qos.keep_last(1);
d_velocitiesSubscription =
create_subscription<CarVelocitiesStamped>(
d_velocitiesTopicName, qos,
std::bind(&SlamROS::topicCallbackVelocities, this, std::placeholders::_1));
}
void SlamROS::createTfFilters()
{
d_tfListenerBuffer = std::make_unique<tf2_ros::Buffer>(get_clock());
auto timerInterface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface(), get_node_timers_interface());
d_tfListenerBuffer->setCreateTimerInterface(timerInterface);
d_tfListener = std::make_shared<tf2_ros::TransformListener>(*d_tfListenerBuffer);
d_lidarMessageFilter.subscribe(this, d_lidarDetectionsTopicName, rmw_qos_profile_default);
d_cameraMessageFilter.subscribe(this, d_monocameraDetectionsTopicName, rmw_qos_profile_default);
std::chrono::duration<int> bufferTimeout(1);
d_lidarTfFilter =
std::make_shared<tf2_ros::MessageFilter<ConesStamped>>(
d_lidarMessageFilter, *d_tfListenerBuffer, d_globalLink, 10, get_node_logging_interface(),
get_node_clock_interface(), bufferTimeout);
d_lidarTfFilter->registerCallback(&SlamROS::topicCallbackLidar, this);
d_cameraTfFilter =
std::make_shared<tf2_ros::MessageFilter<ConesStamped>>(
d_cameraMessageFilter, *d_tfListenerBuffer, d_globalLink, 100, get_node_logging_interface(),
get_node_clock_interface(), bufferTimeout);
d_cameraTfFilter->registerCallback(&SlamROS::topicCallbackMonocamera, this);
}
void SlamROS::topicCallbackVelocities(
const CarVelocitiesStamped::SharedPtr message)
{ ... }
void SlamROS::topicCallbackMonocamera(
const ConesStamped::SharedPtr monocameraMessage)
{ ... }
void SlamROS::topicCallbackLidar(
const ConesStamped::SharedPtr lidarMessage)
{ ... }