Nacho/cleanup get transform util

[nachovizzo]

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