Update rate incorrect if using sim time

[joshjowen]

Describe the bug
When the 'use_sim_time' parameter is set, the ros2_control_node's main loop runs at a rate different then what is provided to the 'update_rate' parameter. In my case, update_rate is set to 100 Hz and the loop runs at ~2.5 kHz.

To Reproduce
With a simulated robot and the /clock topic available
Steps to reproduce the behavior:

1. Set up the controller manager with the update_rate parameter set to 100 and the use_sim_time parameter set to true
2. Set up a joint state broadcaster
3. Run the sim and controllers
4. run "ros2 topic hz /joint_states"

Expected behavior
The /joint_states topic publishes at 100Hz or some number related to that and the real_time_factor of the sim

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version: Humble

I am able to get it to run at the correct rate if i modify

ros2_control/controller_manager/src/ros2_control_node.cpp

Lines 65 to 66 in 62a3d26

	std::chrono::system_clock::time_point next_iteration_time =
	std::chrono::system_clock::time_point(std::chrono::nanoseconds(cm->now().nanoseconds()));

to std::chrono::steady_clock::time_point next_iteration_time = std::chrono::steady_clock::now();
which will obviously run at 100 Hz in real time. I am not sure if the intended behavior would be for the rate to be sim time instead, but steady time would make more sense than system time for real hardware anyway.

[destogl]

Hi @joshjowen,

thanks for this report. Yes, this could be an issue. And you are right about usage of steady clock. I think we have this change in an open PR by @AndyZe about realtime.hpp file in hardware_inteface folder. what we have to check if we are then using "proper" time in ROS and not braking controllers when communicating with other nodes. Have you tested input of commands when using your proposal?

What I m confused about is that you mention usage of use_sim_time and real hardware together. This shouldn't be done because if simulation is not running then the time value is always 0 (had a misbehavior with that last just this week). So I am not sure if this is even "fixable" since the behavior can be undefined.

[joshjowen]

Hi @destogl,

I'm not sure exactly what you mean about "input of commands". I have been using it with a range of controllers including 'joint_trajectory_controller', 'forward_command_controller' and some of our own. It all works as expected.

When I said "steady time would make more sense than system time for real hardware" I was not referring to using sim time on the real hardware, i was saying that steady clock would be more suitable on real hardware than the system clock. In our machines, we will come in and out of GPS signals and in this scenario: PPS corrections to the system clock would skew the control rate when the system clock is used.

The only real issue I can see with my solution is that, when using sim time, the steady clock control rate would not scale with the real time factor of the sim. e.g. if you have a real time factor of 0.5, the 100Hz control rate would run at effectively 200Hz wrt the sim

[krzychut]

Hi,

Have there been any developments on this? My team is having this issue in our project and possibly we can fix it. Just want to make sure we're not duplicating someone else's work.

[saikishor]

Hello @krzychut,

Have there been any developments on this? My team is having this issue in our project and possibly we can fix it. Just want to make sure we're not duplicating someone else's work.

Are you facing the issue with humble or with the rolling branch?

[krzychut]

Hi @saikishor
We run humble, but I've just tested with ros-rolling-ros2-control and the same issue is present.

[dlindmark]

I experience the same thing. Both rolling and humble.

My simulator publishes its time on the /clock topic. One possible workaround is to set the simulation time to the system time when starting the simulation. And then make sure the simulation is active and publishing on /clock before starting the ros2_control_node's main loop.

Maybe not a workaround possible for all. And I do not like that my system depends on the startup order in that way. So would be nice with an update to the control_node that makes it possible to use it with use_sim_time.

[bmagyar]

Let's see what you have on a PR :)

[krzychut]

IMO the main issue to solve is giving the controller manager some idea of the simulations' real time factor. If our simulation is running at 0.5 realtime, we would want the controllers to reflect that. More importantly, if the simulation is running faster than real time, we need the controllers to keep up. TBH I'm not sure what the best solution to this would be. Just subscribing to /clock and comparing with that instead of the system clock is not enough, since the behavior will depend on how often /clock is published. In an extreme case, the control loop can be set up to run at a higher frequency than /clock, which implies that it would need more granular time steps than what is published to determine the update rate.

My only idea that "kind of" works is to spin a /clock subscriber on a separate thread, and use that together with steady_clock to estimate the current real time factor. We can then do a walking average of X such samples and have a decent idea of how fast the simulation is running. This information can then be used to scale the sleep time accordingly.

This is obviously not a perfect solution, but it's pretty simple, it adjusts the update rate match the simulation speed, and it's certainly better than spamming 5k commands per second. I will try to sketch something up and see how it works.

[saikishor]

Hello @krzychut!

I personally think this is has to be handled differently in a different node. For instance, in case of the Gazebo, the gazebo_ros2_control plugin takes care of the update of controllers as shown here : https://github.com/ros-controls/gazebo_ros2_control/blob/5dba0f95a03b136f39145c846ec5ebbfb5a09599/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp#L395-L397

I think it is very hard to make it very generic as one simulation might use /clock and another might use a different approach through API, in these cases, it needs to be handled locally by their respective simulator_ros2_control plugins or nodes.

@bmagyar correct me if I'm wrong.

Best Regards,
Sai

[christophfroehlich]

I tried the example reported in this issue. Following the suggestion of @saikishor I'd also suggest to do this in a different node which can be hosted here or in topic_based_ros2_control. @krzychut have you figured out a solution already and want to contribute it with a PR?
Maybe we should even throw an error if ros2_control_node is launched with use_sim_time. Or is there a usecase in doing so?

[daihen-hijikata]

The example I uploaded here is not use topic_based_ros2_control.
ros-controls/ros2_controllers#537 (comment)
I think this issue is caused by ros2_control not only topic_based_ros2_control.

[christophfroehlich]

@daihen-hijikata I know, but as written above: I don't see a use-case for running ros2_control_node with use_sime_time=true, which does not work well as reported here and by you in the other issue. Someone will have to write a different node with the synchronization to the clock.
And lots of users fall into this trap by using isaac sim with topic_based_ros2_control.

[christophfroehlich]

Related #802

[JayHerpin]

just FYI, I did try out changing the control loop to use use rclcpp::Rate and it worked as expected with use_sim_time:=true.

There are some gazebo plugins which don't play nice with the ros2 gazebo plugin and I found it was easier to just write gz topic based hardware interfaces for those cases and run the normal controller_manager node rather than having to go change those gazebo plugins.

For example, the thrusters gazebo plugin does not allow you to spin the propeller via control2 and have it generate thrust.

[saikishor]

just FYI, I did try out changing the control loop to use use rclcpp::Rate and it worked as expected with use_sim_time:=true.

There are some gazebo plugins which don't play nice with the ros2 gazebo plugin and I found it was easier to just write gz topic based hardware interfaces for those cases and run the normal controller_manager node rather than having to go change those gazebo plugins.

For example, the thrusters gazebo plugin does not allow you to spin the propeller via control2 and have it generate thrust.

Hello!

Are you referring to the ros2_control_node executable?

[JayHerpin]

just FYI, I did try out changing the control loop to use use rclcpp::Rate and it worked as expected with use_sim_time:=true.
There are some gazebo plugins which don't play nice with the ros2 gazebo plugin and I found it was easier to just write gz topic based hardware interfaces for those cases and run the normal controller_manager node rather than having to go change those gazebo plugins.
For example, the thrusters gazebo plugin does not allow you to spin the propeller via control2 and have it generate thrust.

Hello!

Are you referring to the ros2_control_node executable?

Yes. I dont have the exact code I tried in front of me, but here's the chatgpt version of that change

      rclcpp::Rate rate(cm->get_update_rate(), cm->get_lock ());

      // For calculating the measured period of the loop
      rclcpp::Time previous_time = cm->now();

      while (rclcpp::ok())
      {
        // Calculate measured period
        auto const current_time = cm->now();
        auto const measured_period = current_time - previous_time;
        previous_time = current_time;

        // Execute update loop
        cm->read(current_time, measured_period);
        cm->update(current_time, measured_period);
        cm->write(current_time, measured_period);

        // Wait for the next iteration using rclcpp::Rate
        rate.sleep();
      }