Skip to content

Unify clock behavior between use_sim_time true/false #1540

@doisyg

Description

@doisyg

Feature request

It is my understanding that one can get very different behavior when asking for the node time when use_sim_time is true and the simulation time is published on the /clock topic, compared to when it is false.
See for instance this code that blocks forever in the timer_callback when use_sim_time is true and that behave properly when use_sim_time is false:

#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <std_msgs/msg/string.hpp>

using namespace std::chrono_literals;

class TimeTest: public rclcpp::Node
{
  public:
    TimeTest()
      : Node("time_test_node")
    {
      this->set_parameter(rclcpp::Parameter("use_sim_time", true));
      timer = this->create_wall_timer(2s, std::bind(&TimeTest::timer_callback, this));
    }

  private:
    rclcpp::TimerBase::SharedPtr timer;

    void timer_callback()
    {
      rclcpp::Time start_time = this->get_clock()->now();
      bool is_time_out = false;
      while(!is_time_out){
        rclcpp::Time time_now = this->get_clock()->now();
        rclcpp::Duration time_spent = time_now -start_time;
        RCLCPP_INFO_STREAM(this->get_logger(), " now (s): " << time_now.seconds());
        rclcpp::sleep_for(100ms);
        is_time_out = time_spent.seconds() > 1.0;
      }
    }
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TimeTest>());
  rclcpp::shutdown();
  return 0;
}

It would be really useful to have the same behavior in both case.

Feature description

The node clock should be updated in a similar way with use_sim_time true/false, i.e no clock freeze specific to use_sim_time true

Implementation considerations

I guess the clock freeze with use_sim_time is true in callbacks because a node is by default mono-threaded and hence the simulated clock cannot be updated (the internal callback of the /clock topic cannot be served in parallel of another callback).
One solution could be to add a specific thread to the node to serve the /clock topic and update the node clock when use_sim_time is true (in a transparent way for the user of the node).

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions