How to set up tfbuffer clock in ROS 2 so it works with sim time?
I'm working on the tip of the ROS 2 master branch. In the example code below, I have a deadlock when use_sim_time
is active even though Gazebo is running and publishing /clock
messages. I call canTransform
with a timeout but it can't timeout because it is in a tight loop waiting for the clock to advance, and the clock can't advance because canTransform
never relinquishes control to let the node spin and service /clock
messages
#include <memory>
#include <string>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
using namespace std;
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node=rclcpp::Node::make_shared("sim_time_test");
tf2_ros::Buffer tfBuffer(node->get_clock());
tf2_ros::TransformListener tfListener(tfBuffer);
rclcpp::executors::SingleThreadedExecutor exec;
rclcpp::Rate loop_rate(1);
while(rclcpp::ok()) {
tfBuffer.canTransform("map", "odom",tf2::TimePointZero, 1s); // DEADLOCK HERE
RCLCPP_INFO(node->get_logger(), "Waiting on transform");
exec.add_node(node);
exec.spin_some(1s);
exec.remove_node(node);
loop_rate.sleep();
}
rclcpp::shutdown();
return 0;
}
The actual hang occurs in this snippet of code from buffer.cpp
. The code below keeps checking clock->now() but that never advances.
while (clock_->now() < start_time + rclcpp_timeout &&
!canTransform(target_frame, source_frame, time) &&
(clock_->now() + rclcpp::Duration(3, 0) >= start_time) && //don't wait when we detect a bag loop
(rclcpp::ok()// || !ros::isInitialized() //TODO(tfoote) restore
)) // Make sure we haven't been stopped (won't work for pytf)
{
// TODO(sloretz) sleep using clock_->sleep_for when implemented
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
I'm not sure if this is a bug in tf2_ros::Buffer or if I'm just using it wrong.