Nacho/cleanup get transform util by nachovizzo · Pull Request #4181 · ros-navigation/navigation2

nachovizzo · 2024-03-14T17:52:22Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	None
Primary OS tested on	Ubuntu
Robotic platform tested on	Simulatin
Does this PR contain AI generated software?	NO

Description of contribution in a few bullet points

The following pattern is widely used in ROS applications, and (far from ideal) I'd like to extend the nav2_utils::getTransform to also spit TransformStamped messages. I know, these util functions should be in the tf library but here seemed like a lower-hanging fruit to bite:

try {
  // Obtaining the transform to get data from source to target frame
  return tf_buffer->lookupTransform(
    target_frame_id, source_frame_id,
    tf2::TimePointZero, transform_timeout);
} catch (tf2::TransformException & e) {
  RCLCPP_ERROR(
    rclcpp::get_logger("getTransform"),
    "Failed to get \"%s\"->\"%s\" frame transform: %s",
    source_frame_id.c_str(), target_frame_id.c_str(), e.what());
  return std::nullopt;
}

Some comments:

Open discussion PR. Not necessary to pretend that 100% gets merged
This PR introduces a hard dependency in C++17 on the nav2_util pkg. I'm happy to express this in the build system but I'm not sure if this is a common practice in nav2. Do you think this might bring a problem
I tried to be specific commit-wise. So the changes can be reviewed in the commits and not just the entire diff
I renamed some bits to follow more the traditional names from tf2 to avoid confusing users
I introduced a BREAKIN change by swapping the soruce and frame id from the getTransform function. I'm not sure why it was "inverted" (compared to tf2) in the first place. But I found it very confusing to read this:

// tf2
geometry_msgs::msg::TransformStamped
lookupTransform(const std::string & target_frame, const std::string & source_frame, ...)

// nav2
bool getTransform(const std::string & source_frame_id,   const std::string & target_frame_id, ...)

/// Function call
lookupTransform(target, source)

I know it's not common in ROS, but I'm trying to introduce a silly pattern that I think it's much better than returning booleans on functions, and forcing the user to provide an output argument:
Before:

OutputType out_variable;
bool res = FunctionCall(/*const*/ arg1,/*const*/ arg2, ..., /* non-const */ out_variable);
if(res) {
 // use out_variable
} else {
return;
}

For a more modern and easier to read + safer approach:

auto out_variable = FunctionCall(/*const*/ arg1,/*const*/ arg2);
if(out_variable.has_value()) {
 // use out_variable
} else {
return;
}

This improves the code (on my opinion) from:

 tf2::Transform tf_transform;
  if (base_shift_correction_) {
    // Obtaining the transform to get data from source frame and time where it was received
    // to the base frame and current time
    if (
      !nav2_util::getTransform(
        data_->header.frame_id, data_->header.stamp,
        base_frame_id_, curr_time, global_frame_id_,
        transform_tolerance_, tf_buffer_, tf_transform))
    {
      return false;
    }
  } else {
    // Obtaining the transform to get data from source frame to base frame without time shift
    // considered. Less accurate but much more faster option not dependent on state estimation
    // frames.
    if (
      !nav2_util::getTransform(
        data_->header.frame_id, base_frame_id_,
        transform_tolerance_, tf_buffer_, tf_transform))
    {
      return false;
    }
  }

, to:

  const auto tf_transform = [&]() -> std::optional<tf2::Transform> {
      // Obtaining the transform to get data from source frame and time where it was received to the
      // base frame and current time
      if (base_shift_correction_) {
        return nav2_util::getTransform(
          base_frame_id_, curr_time, data_->header.frame_id,
          data_->header.stamp, global_frame_id_, transform_timeout_,
          tf_buffer_);
      }

      // Obtaining the transform to get data from source frame to base frame without time shift
      // considered. Less accurate but much more faster option not dependent on state estimation
      // frames.
      return nav2_util::getTransform(
        base_frame_id_, data_->header.frame_id, transform_timeout_,
        tf_buffer_);
    }();

  if (!tf_transform.has_value()) {return false;}

Future work that may be required in bullet points

Make sure we aren't using the "bare" pattern without calling nav2_utils::getTransform

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

mergify · 2024-03-14T17:54:17Z

@nachovizzo, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2024-03-14T17:57:00Z

This PR introduces a hard dependency in C++17 on the nav2_util pkg. I'm happy to express this in the build system but I'm not sure if this is a common practice in nav2. Do you think this might bring a problem

This is fine!

SteveMacenski

Heed the linter

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

nav2_collision_monitor/src/pointcloud.cpp

mergify · 2024-03-14T18:33:32Z

@nachovizzo, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

nav2_util/src/robot_utils.cpp

SteveMacenski · 2024-03-27T22:50:34Z

@nachovizzo waddup here? 💯

nachovizzo · 2024-03-28T08:27:11Z

@nachovizzo waddup here? 💯

Done!

mergify · 2024-03-28T08:27:48Z

@nachovizzo, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

SteveMacenski · 2024-03-29T19:20:17Z

2 things:

Pull in main, CI should run again
Do the DCO bits

Otherwise, love it, super clean, I love abstracting out nonsense

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Otherwise is very confusing for any user who is user to the tf2::Buffer::lookupTransform method Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

I find this "transform tolerance" to specify something else. Once again, I believe that sticking to tf2 name conventions is better and improve readability for users already used to the tf2 API Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

This allow us to also query for the TransformStamped message from the lookupTransform method. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

This reverts commit ca7d06b. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

nachovizzo · 2024-04-02T15:56:13Z

@SteveMacenski done with this, I pulled main but the CI still fails to run 😢

SteveMacenski · 2024-04-02T19:34:52Z

It ran (i.e. built) there are some new test failures that popped up while our CI was down. I just haven't had a chance to look at them yet but not caused by this PR (probably; or at least in whole)

* remove unused header Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * First step. Return an optional value instead of user provided output. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Second step, update the consumers of this utility function Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Third step: swap source/target to make it consistent with tf lookups Otherwise is very confusing for any user who is user to the tf2::Buffer::lookupTransform method Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * transform tolerance -> transform timeout I find this "transform tolerance" to specify something else. Once again, I believe that sticking to tf2 name conventions is better and improve readability for users already used to the tf2 API Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Last step, convert functions to template functions This allow us to also query for the TransformStamped message from the lookupTransform method. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add nodiscard to avoid confusiong API calls Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Update docs Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Revert "transform tolerance -> transform timeout" This reverts commit ca7d06b. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix linter 🤦 Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * reset to main Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add 2 new utils functions to query transforms using messages Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Move utility function to base class to reuse implementation Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix Typo Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> --------- Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

* remove unused header Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * First step. Return an optional value instead of user provided output. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Second step, update the consumers of this utility function Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Third step: swap source/target to make it consistent with tf lookups Otherwise is very confusing for any user who is user to the tf2::Buffer::lookupTransform method Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * transform tolerance -> transform timeout I find this "transform tolerance" to specify something else. Once again, I believe that sticking to tf2 name conventions is better and improve readability for users already used to the tf2 API Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Last step, convert functions to template functions This allow us to also query for the TransformStamped message from the lookupTransform method. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add nodiscard to avoid confusiong API calls Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Update docs Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Revert "transform tolerance -> transform timeout" This reverts commit ca7d06b. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix linter 🤦 Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * reset to main Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add 2 new utils functions to query transforms using messages Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Move utility function to base class to reuse implementation Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix Typo Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> --------- Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> Signed-off-by: enricosutera <enricosutera@outlook.com>

* remove unused header Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * First step. Return an optional value instead of user provided output. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Second step, update the consumers of this utility function Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Third step: swap source/target to make it consistent with tf lookups Otherwise is very confusing for any user who is user to the tf2::Buffer::lookupTransform method Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * transform tolerance -> transform timeout I find this "transform tolerance" to specify something else. Once again, I believe that sticking to tf2 name conventions is better and improve readability for users already used to the tf2 API Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Last step, convert functions to template functions This allow us to also query for the TransformStamped message from the lookupTransform method. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add nodiscard to avoid confusiong API calls Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Update docs Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Revert "transform tolerance -> transform timeout" This reverts commit ca7d06b. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix linter 🤦 Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * reset to main Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add 2 new utils functions to query transforms using messages Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Move utility function to base class to reuse implementation Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix Typo Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> --------- Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

* remove unused header Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * First step. Return an optional value instead of user provided output. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Second step, update the consumers of this utility function Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Third step: swap source/target to make it consistent with tf lookups Otherwise is very confusing for any user who is user to the tf2::Buffer::lookupTransform method Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * transform tolerance -> transform timeout I find this "transform tolerance" to specify something else. Once again, I believe that sticking to tf2 name conventions is better and improve readability for users already used to the tf2 API Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Last step, convert functions to template functions This allow us to also query for the TransformStamped message from the lookupTransform method. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add nodiscard to avoid confusiong API calls Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Update docs Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Revert "transform tolerance -> transform timeout" This reverts commit ca7d06b. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix linter 🤦 Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * reset to main Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Add 2 new utils functions to query transforms using messages Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Move utility function to base class to reuse implementation Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> * Fix Typo Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> --------- Signed-off-by: Ignacio Vizzo <ignacio@dexory.com> Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

nachovizzo mentioned this pull request Mar 14, 2024

Extend util function by also allowing to get the transform msg instea… botsandus/navigation2#30

Merged

SteveMacenski requested changes Mar 14, 2024

View reviewed changes

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp Outdated Show resolved Hide resolved

nav2_collision_monitor/src/pointcloud.cpp Outdated Show resolved Hide resolved

nachovizzo added a commit to botsandus/navigation2 that referenced this pull request Mar 14, 2024

Synch with ros-navigation#4181

9a36c17

SteveMacenski requested changes Mar 14, 2024

View reviewed changes

nav2_util/src/robot_utils.cpp Outdated Show resolved Hide resolved

SteveMacenski approved these changes Mar 29, 2024

View reviewed changes

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp Outdated Show resolved Hide resolved

nachovizzo added 13 commits April 2, 2024 09:38

remove unused header

829578a

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

First step. Return an optional value instead of user provided output.

1cbdb40

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Second step, update the consumers of this utility function

a65880b

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Third step: swap source/target to make it consistent with tf lookups

250311d

Otherwise is very confusing for any user who is user to the tf2::Buffer::lookupTransform method Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

transform tolerance -> transform timeout

b1bc26f

I find this "transform tolerance" to specify something else. Once again, I believe that sticking to tf2 name conventions is better and improve readability for users already used to the tf2 API Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Last step, convert functions to template functions

41f8920

This allow us to also query for the TransformStamped message from the lookupTransform method. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Add nodiscard to avoid confusiong API calls

0b33a52

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Update docs

0f0a75e

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Revert "transform tolerance -> transform timeout"

d839f3b

This reverts commit ca7d06b. Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Fix linter 🤦

d21db92

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

reset to main

2a45a68

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Add 2 new utils functions to query transforms using messages

c4c651d

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

Move utility function to base class to reuse implementation

d450502

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

nachovizzo force-pushed the nacho/cleanup_get_transform_util branch from 2bd3588 to d450502 Compare April 2, 2024 07:38

nachovizzo added 2 commits April 2, 2024 09:39

Merge remote-tracking branch main into nacho/cleanup_get_transform_util

4457000

Fix Typo

47ef228

Signed-off-by: Ignacio Vizzo <ignacio@dexory.com>

SteveMacenski merged commit 911555a into ros-navigation:main Apr 2, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Nacho/cleanup get transform util#4181

Nacho/cleanup get transform util#4181
SteveMacenski merged 15 commits intoros-navigation:mainfrom
nachovizzo:nacho/cleanup_get_transform_util

nachovizzo commented Mar 14, 2024 •

edited

Loading

Uh oh!

mergify bot commented Mar 14, 2024

Uh oh!

SteveMacenski commented Mar 14, 2024

Uh oh!

SteveMacenski left a comment

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Mar 14, 2024

Uh oh!

Uh oh!

SteveMacenski commented Mar 27, 2024

Uh oh!

nachovizzo commented Mar 28, 2024

Uh oh!

mergify bot commented Mar 28, 2024

Uh oh!

Uh oh!

SteveMacenski commented Mar 29, 2024

Uh oh!

nachovizzo commented Apr 2, 2024

Uh oh!

SteveMacenski commented Apr 2, 2024

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

nachovizzo commented Mar 14, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mergify bot commented Mar 14, 2024

Uh oh!

SteveMacenski commented Mar 14, 2024

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Mar 14, 2024

Uh oh!

Uh oh!

SteveMacenski commented Mar 27, 2024

Uh oh!

nachovizzo commented Mar 28, 2024

Uh oh!

mergify bot commented Mar 28, 2024

Uh oh!

Uh oh!

SteveMacenski commented Mar 29, 2024

Uh oh!

nachovizzo commented Apr 2, 2024

Uh oh!

SteveMacenski commented Apr 2, 2024

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

nachovizzo commented Mar 14, 2024 •

edited

Loading