Timed Behaviors do not adhere to Sim Time

[bobbleballs]

Bug report

• Operating System:
  Ubuntu 22.04

• ROS2 Version:
  Humble

• Version or commit hash:
  0d86469

• DDS implementation:

  • Cyclone-DDS

Steps to reproduce issue

I have an issue where I am running a simulation at greater than real time speed. My simulation platform (Flatland2D) is publishing sim time on /clock and use_sim_time is set on the Nav2 launch procedure. My issue is that my behaviour tree contains some waits, and backups however these behaviours occur at real time not sim time.

Expected behavior

Timed behaviours will work to sim time not real time. e.g. in a simulation running at 2x speed with a wait of 10 seconds I would expect this to take 5 Seconds.

Actual behavior

Waits take the real length of time.

Additional information

I've had a look at nav2_behaviors/plugins/wait and back_up and it looks like wait uses a steady_time from chrono which calls now(). I don't know much about chrono but I'm guessing this gets an accurate system time?
nav2_behaviors/include/timed_behaviors.hpp also uses rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; which I presume does something similar?

For the behaviours to adhere to use_sim_time these behaviours should be using RCL_ROS_TIME I think? Would this have any negative effects. I'm not sure if steady_time was used for a reason? Ideally I am trying to make the behaviour of the navigation stack at high sim rates as accurate as real time so this is a blocker to me.

[SteveMacenski]

Good question, I don't see any reason why we couldn't do that. Yes, chrono is C++ system time independent of ROS / simulation times. You'd probably need to update not only the behaviors themselves, but the TimedBehavior header class which also uses chrono to dish out behaviors. You should get the clock object from the root node so that it can use real/ros time as the node is set appropriately.

We used steady time because the ROS clock isn't guaranteed to move forward or be regular. There are certain things in the stack (e.g. server timeouts, BT node rate controllers) which should probably use wall time instead. I think there can be an argument for the behaviors to be moved to ROS time, that seems sensible. I think it would just be important as you're changing code in the behavior server to ask yourself "how could this go wrong?" and make sure to handle any edge cases that could be introduced if the clock either stopped or moved backwards (so B-A could be negative).

@jwallace42 any objection? @bobbleballs would you be open to contributing this?

[jwallace42]

Nope, this sounds like a good change to me!

[bobbleballs]

Great, thanks for your input. I'll have a look into this with your recommendations.

[SteveMacenski]

Any updates 😄

[SteveMacenski]

@bobbleballs what do you think?

[bobbleballs]

Sorry I've been swamped with other work and this hasn't been as blocking as I expected.
I will need to do it eventually but at the moment it's not a super high priority.
I was also thinking this is also an issue if your sim time is running at less than real time also. IE if your Gazebo sim is very complex and runs at half speed so I do think it should be fixed.

[AlexeyMerzlyakov]

More general, by grepping where we are using std::chrono's now() and rclcpp::WallRate (the same problem caused by rclcpp WallRate, described in #3325), we have the following places where ROS timings are being ignored (tests were excluded):

Where	Type	Comment
nav2_behavior_tree/plugins/decorator/rate_controller.cpp	std::chrono
nav2_behavior_tree/src/behavior_tree_engine.cpp	rclcpp::WallRate
nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp	rclcpp::WallRate
nav2_behaviors/plugins/wait.cpp	std::chrono	Discussed by #3303
nav2_controller/src/controller_server.cpp	rclcpp::WallRate
nav2_costmap_2d/src/costmap_2d_ros.cpp	rclcpp::WallRate	Covered by #3325
nav2_smac_planner/src/a_star.cpp	std::chrono
nav2_theta_star_planner/src/theta_star_planner.cpp	std::chrono
nav2_theta_star_planner/src/theta_star_planner.cpp	std::chrono
nav2_waypoint_follower/src/waypoint_follower.cpp	rclcpp::WallRate

[AlexeyMerzlyakov]

Update: node->create_wall_timer() are also have no respect to ROS-time. From first sight, the create_wall_timer() API is better to be changed to node->create_timer(), that generates rclcpp::GenericTimer class respective to clock pointer (and clock type as well) given in its constructor from the node->get_clock().

The usage of create_wall_timer is notable in the following places:

Where	Type	Comment
nav2_lifecycle_manager/src/lifecycle_manager.cpp	create_wall_timer()	Bond timer with 200ms acknowledgement time
nav2_velocity_smoother/src/velocity_smoother.cpp	create_wall_timer()	Timer for publish smoothed velocity

[alexbuyval]

Hi there, I have just faced with this issue too during using the MPPI. I have a simulation environment which 2-3 times slower that real time, so when the MPPI controller shifts the control sequence by 1 step it means that 50 ms passed, however, it passed only 15-20 ms in sim time. And it produce a bad performance of the controller.

Are there any plans to align the Nav2 with simulation time?

[pvandervelde]

I'm experiencing the same as above except in my case my simulation is a lot slower than real time (about 20% of real time), which means that behaviours time out too quickly. My initial thought was to change the time-outs so that the controller (using the dwb planner) has enough time to actually see the change. But I can't find any time outs to change.

[SteveMacenski]

@AlexeyMerzlyakov similar question here; are these points all addressable now? I'm not asking you to do it, but rather that all of the necessary changes in rclcpp are now in so I can do it / ask a community contributor to help on this one

[AlexeyMerzlyakov]

In ROS2 new rclcpp::Rate API was merged. So, we could use it for now.
The following changes to be made at-once (not only related to new rclcpp::Rate, but also be respective to ROS-time in all places):

• rclcpp::WallRate(rate) to be replaced to rclcpp::Rate(node->get_clock(), rate) (please refer to this comment)
• node->create_wall_timer() to be replaced to node->create_timer() (please refer to this comment)
• all durations using std::chrono should be considered to change to rclcpp::Duration, where it used for timers or publishers, since the last one counts with simulated ROS-time

[SteveMacenski]

Working on this today - will have a solution by EOD for everything except for costmap2dros which is covered in another ticket ( #3325) and has a open PR #3404 which I need an update on status separately.