Closed bobbleballs closed 10 months ago
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?
Nope, this sounds like a good change to me!
Great, thanks for your input. I'll have a look into this with your recommendations.
Any updates :smile:
@bobbleballs what do you think?
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.
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 |
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 |
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?
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.
@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
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)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-timeWorking on this today - will have a solution by EOD for everything except for costmap2dros which is covered in another ticket ( https://github.com/ros-planning/navigation2/issues/3325) and has a open PR https://github.com/ros-planning/navigation2/pull/3404 which I need an update on status separately.
Bug report
Operating System: Ubuntu 22.04
ROS2 Version: Humble
Version or commit hash: 0d86469f1c31708117e68f3f44ad5392347e9a5d
DDS implementation:
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.