Skip to content

Commit

Permalink
Update after testing for Tf2 "Using Time (C++)" and "Time travel (C++…
Browse files Browse the repository at this point in the history
…)" (#3583)

* Update code references to match previous tutorials. Add hint to build before executing the launch commands.
  • Loading branch information
BorisBoutillier authored May 11, 2023
1 parent d0271c1 commit 551221e
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 29 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,12 @@ Open ``turtle_tf2_listener.cpp`` and take a look at the ``lookupTransform()`` ca

.. code-block:: C++

transformStamped = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
tf2::TimePointZero);
try {
t = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
tf2::TimePointZero);
} catch (const tf2::TransformException & ex) {

You can see that we specified a time equal to 0 by calling ``tf2::TimePointZero``.

Expand All @@ -58,12 +60,13 @@ Now, change this line to get the transform at the current time, ``this->get_cloc
.. code-block:: C++

rclcpp::Time now = this->get_clock()->now();
transformStamped = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
now);
try {
t = tf_buffer_->lookupTransform(
toFrameRel, fromFrameRel,
now);
} catch (const tf2::TransformException & ex) {

Now try to run the launch file.
Now build the package and try to run the launch file.

.. code-block:: console
Expand Down Expand Up @@ -94,19 +97,21 @@ To fix this, edit your code as shown below (add the last timeout parameter):
.. code-block:: C++

rclcpp::Time now = this->get_clock()->now();
transformStamped = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
now,
50ms);
try {
t = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
now,
50ms);
} catch (const tf2::TransformException & ex) {

The ``lookupTransform()`` can take four arguments, where the last one is an optional timeout.
It will block for up to that duration waiting for it to timeout.

3 Checking the results
^^^^^^^^^^^^^^^^^^^^^^

You can now run the launch file.
You can now build the package and run the launch file.

.. code-block:: console
Expand Down
32 changes: 18 additions & 14 deletions source/Tutorials/Intermediate/Tf2/Time-Travel-With-Tf2-Cpp.rst
Original file line number Diff line number Diff line change
Expand Up @@ -39,14 +39,16 @@ Edit the ``lookupTransform()`` call in ``turtle_tf2_listener.cpp`` file to
.. code-block:: C++

rclcpp::Time when = this->get_clock()->now() - rclcpp::Duration(5, 0);
t = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
when,
50ms);
try {
t = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
when,
50ms);
} catch (const tf2::TransformException & ex) {

Now if you run this, during the first 5 seconds, the second turtle would not know where to go because we do not yet have a 5-second history of poses of the carrot.
But what happens after these 5 seconds? Let's just give it a try:
But what happens after these 5 seconds? Build the package then let's just give it a try:

.. code-block:: console
Expand All @@ -71,13 +73,15 @@ Your code now would look like this:

rclcpp::Time now = this->get_clock()->now();
rclcpp::Time when = now - rclcpp::Duration(5, 0);
t = tf_buffer_->lookupTransform(
toFrameRel,
now,
fromFrameRel,
when,
"world",
50ms);
try {
t = tf_buffer_->lookupTransform(
toFrameRel,
now,
fromFrameRel,
when,
"world",
50ms);
} catch (const tf2::TransformException & ex) {

The advanced API for ``lookupTransform()`` takes six arguments:

Expand All @@ -101,7 +105,7 @@ And at the current time, tf2 computes the transform from the ``world`` to the ``
Checking the results
--------------------

Let's run the simulation again, this time with the advanced time-travel API:
Build the package then let's run the simulation again, this time with the advanced time-travel API:

.. code-block:: console
Expand Down

0 comments on commit 551221e

Please sign in to comment.