Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support simulated time #88

Merged
merged 12 commits into from
Mar 25, 2024
Merged

Support simulated time #88

merged 12 commits into from
Mar 25, 2024

Conversation

skoudmar
Copy link
Contributor

Motivation

The simulated time feature (enabled with the use_sim_time parameter) is useful when the node is run in a simulated environment, e.g., with ros2 bag. It allows the simulator to run slower or faster than in real time.

Implementation Notes

This implementation is based on the existing implementation in rclcpp.

  • Each node has its own RosTime clock that is used to create timers.
  • Time source is responsible for subscribing to the /clock topic and updating attached clocks.
  • The callback of TimeSourceSubscriber does not use async runtime. It is run in the handle_incoming() method during node spinning.
  • The destruction of TimeSourceSubscriber is done after disabling the simulated time when the next time message is received.

rclcpp class diagram

rclcpp implements this feature with the help of several classes which are related as shown in the following diagram. In the simplest (default) case, each node has a NodeClock, which is referenced from both the Node and its TimeSource. If one wants, other clocks can be added to the TimeSource via the attachClock() method.

clock-class-diagram

r2r class diagram

For r2r we decided to implement this in a simpler way, as shown in the following diagram. A TimeSource can drive one or more clocks as in rclcpp. The difference is that the TimeSourceSubscriber is stored in Node alongside other subscribers instead of in TimeSource as in rclcpp.

r2r-timesource-classdiagram

Enabling simulated time

Simulated time can be enabled in two ways:

  1. In the rust code,
node.get_time_source().enable_sim_time(&mut node)?;
  1. Optionally, by setting use_sime_time parameter at node initialization time if the node uses a derived parameter handler.
  • It is enabled during the registration of the derived parameter handler.
  • The simulated time cannot be enabled later because it would require storing a &mut node in a parameter change callback.

Feature gate

This functionality uses cargo features because the message on the /clock topic has the type Clock defined in rosgraph_msgs, and if the user does not declare it as a dependency, the compilation will fail. This would break existing projects.

Final notes

We already use this feature with @wentasah and have not encountered any problems.

We suggest to review this commit-by-commit.

skoudmar and others added 10 commits March 20, 2024 13:27
This commit is preparation for next commit with timer using RosTime clock.

In the previous implementation the function rcl_timer_init is called
with zero initialized timer handle being stored on stack and after
initialization with rcl_timer_init the timer handle is moved by rust
inside Timer_ struct.

The problem occurs if the type of clock being passed to rcl_timer_init()
is RosTime because the function also registers callbacks in the clock
that would notify the timer about sudden time change (jump callback).
In RCL the jump callback is registered with rcl_clock_add_jump_callback()
and the argument callback.user_data is pointer to timer handle that in
our case references timer stored om stack. When returning from function
create_wall_timer() the pointer becomes dangling and therefore when the
callback is actually called (by function _rcl_timer_time_jump) it could
result in SIGSEGV or could access some variable that happens to be in
the same place as the timer_handle was on stack.

This is fixed by storing timer_handle inside pinned box to prevent
moving the handle.
This allows to implement timers that can share single clock among them.

This commit is preparation for feature sim time.
This adds Node::create_timer() method. The method is similar to
already existing Node::create_wall_timer(), but the timer is based on
ClockType::RosTime rather than on ClockType::SteadyTime.

The advantage of ClockType::RosTime is that one can drive the clock
from a different time source. This will be used in the next commits to
implement for clock source on the /clock topic.
Added feature flag sim-time because functions that are feature gated does
not make sense without TimeSource that is implemented in later commit
and needs the feature flag.
The TimeSource subscribes to /clock topic and publishes the time to all
attached clocks (by default only clock used by create_timer is attached).

This module is feature gated because the clock is published in message
rosgraph_msgs::msg::Clock and rosgraph_msgs needs to be specified as
dependency by the end users.
…meter

add `--ros-args -p use_sim_time:=true` to starting command line
@m-dahl
Copy link
Collaborator

m-dahl commented Mar 22, 2024

Hi,

Thanks for the really well put together PR. Code looks good (thanks for the well defined separate commits, made it really easy to look through). I have one suggestion which I pushed to here for now since I didn't have write access to your PR branch. This would make it so that you don't need the feature to active sim-time, it would be enough to have the message compiled. What do you think about that? Regardless I am happy to merge this.

@wentasah
Copy link
Contributor

Hi, I think that your suggestion makes a lot of sense. I didn't know that cfg can be used this way. Let's wait for @skoudmar if he has some comments.

cfg(r2r__rosgraph_msgs__msg__Clock) attribute is not propagated to user
code so this macro can be used to intentionally fail compilation if
sim time is not supported.
@skoudmar
Copy link
Contributor Author

Hi,

I liked that if the cargo feature was used, the compilation would fail when the dependency was missing. I added the macro assert_compiled_with_use_sim_time_support! to reenable this if desired. The #[cfg(not(r2r__rosgraph_msgs__msg__Clock))] attribute guard cannot be used in user code because cfg attributes are not propagated.

I have also enabled edits for you.

@m-dahl m-dahl merged commit 11ec445 into sequenceplanner:master Mar 25, 2024
6 checks passed
@m-dahl
Copy link
Collaborator

m-dahl commented Mar 25, 2024

That's a good solution, merging now. Thanks again for your contribution.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants