How to set up tfbuffer clock in ROS 2 so it works with sim time?

asked 2018-11-05 15:55:43 -0500

Carl D gravatar image

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.

edit retag flag offensive close merge delete