Nacho/cleanup get transform util (ros-navigation#4181)

* 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>

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -24,9 +24,9 @@
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 
-#include "nav2_util/lifecycle_node.hpp"
-
 #include "nav2_collision_monitor/types.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "std_msgs/msg/header.hpp"
 
 namespace nav2_collision_monitor
 {
@@ -123,6 +123,21 @@ class Source
  rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
  std::vector<rclcpp::Parameter> parameters);
 
+ /**
+ * @brief Obtain 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_ is true) or the transform without time
+ * shift considered which is less accurate but much more faster option not dependent on state
+ * estimation frames.
+ * @param curr_time Current node time
+ * @param data_header Current header which contains the frame_id and the stamp
+ * @param tf_transform Output source->base_frame_id_ transform
+ * @return True if got correct transform, otherwise false
+ */
+ bool getTransform(
+ const rclcpp::Time & curr_time,
+ const std_msgs::msg::Header & data_header,
+ tf2::Transform & tf_transform) const;
+
  // ----- Variables -----
 
  /// @brief Collision Monitor node

nav2_collision_monitor/src/pointcloud.cpp

@@ -79,28 +79,8 @@ bool PointCloud::getData(
  }
 
  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;
- }
+ if (!getTransform(curr_time, data_->header, tf_transform)) {
+ return false;
  }
 
  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*data_, "x");

nav2_collision_monitor/src/range.cpp

@@ -88,28 +88,8 @@ bool Range::getData(
  }
 
  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;
- }
+ if (!getTransform(curr_time, data_->header, tf_transform)) {
+ return false;
  }
 
  // Calculate poses and refill data array

nav2_collision_monitor/src/scan.cpp

@@ -78,28 +78,8 @@ bool Scan::getData(
  }
 
  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;
- }
+ if (!getTransform(curr_time, data_->header, tf_transform)) {
+ return false;
  }
 
  // Calculate poses and refill data array

nav2_collision_monitor/src/source.cpp

@@ -19,6 +19,7 @@
 #include "geometry_msgs/msg/transform_stamped.hpp"
 
 #include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_collision_monitor
 {
@@ -131,4 +132,30 @@ Source::dynamicParametersCallback(
  return result;
 }
 
+bool Source::getTransform(
+ const rclcpp::Time & curr_time,
+ const std_msgs::msg::Header & data_header,
+ tf2::Transform & tf_transform) const
+{
+ if (base_shift_correction_) {
+ 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 {
+ if (
+ !nav2_util::getTransform(
+ data_header.frame_id, base_frame_id_,
+ transform_tolerance_, tf_buffer_, tf_transform))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
 } // namespace nav2_collision_monitor

nav2_util/include/nav2_util/robot_utils.hpp

@@ -23,6 +23,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -99,6 +100,45 @@ bool getTransform(
  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
  tf2::Transform & tf2_transform);
 
+/**
+ * @brief Obtains a transform from source_frame_id -> to target_frame_id
+ * @param source_frame_id Source frame ID to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param transform_tolerance Transform tolerance
+ * @param tf_buffer TF buffer to use for the transformation
+ * @param transform_msg Output source->target transform msg
+ * @return True if got correct transform, otherwise false
+ */
+bool getTransform(
+ const std::string & source_frame_id,
+ const std::string & target_frame_id,
+ const tf2::Duration & transform_tolerance,
+ const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+ geometry_msgs::msg::TransformStamped & transform_msg);
+
+/**
+ * @brief Obtains a transform from source_frame_id at source_time ->
+ * to target_frame_id at target_time time
+ * @param source_frame_id Source frame ID to convert from
+ * @param source_time Source timestamp to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param target_time Current node time to interpolate to
+ * @param fixed_frame_id The frame in which to assume the transform is constant in time
+ * @param transform_tolerance Transform tolerance
+ * @param tf_buffer TF buffer to use for the transformation
+ * @param transform_msg Output source->target transform msg
+ * @return True if got correct transform, otherwise false
+ */
+bool getTransform(
+ const std::string & source_frame_id,
+ const rclcpp::Time & source_time,
+ const std::string & target_frame_id,
+ const rclcpp::Time & target_time,
+ const std::string & fixed_frame_id,
+ const tf2::Duration & transform_tolerance,
+ const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+ geometry_msgs::msg::TransformStamped & transform_msg);
+
 /**
  * @brief Validates a twist message contains no nans or infs
  * @param msg Twist message to validate

nav2_util/src/robot_utils.cpp

@@ -82,19 +82,16 @@ bool getTransform(
  const std::string & target_frame_id,
  const tf2::Duration & transform_tolerance,
  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
- tf2::Transform & tf2_transform)
+ geometry_msgs::msg::TransformStamped & transform_msg)
 {
- geometry_msgs::msg::TransformStamped transform;
- tf2_transform.setIdentity(); // initialize by identical transform
-
  if (source_frame_id == target_frame_id) {
  // We are already in required frame
  return true;
  }
 
  try {
  // Obtaining the transform to get data from source to target frame
- transform = tf_buffer->lookupTransform(
+ transform_msg = tf_buffer->lookupTransform(
  target_frame_id, source_frame_id,
  tf2::TimePointZero, transform_tolerance);
  } catch (tf2::TransformException & e) {
@@ -104,29 +101,40 @@ bool getTransform(
  source_frame_id.c_str(), target_frame_id.c_str(), e.what());
  return false;
  }
-
- // Convert TransformStamped to TF2 transform
- tf2::fromMsg(transform.transform, tf2_transform);
  return true;
 }
 
 bool getTransform(
  const std::string & source_frame_id,
- const rclcpp::Time & source_time,
  const std::string & target_frame_id,
- const rclcpp::Time & target_time,
- const std::string & fixed_frame_id,
  const tf2::Duration & transform_tolerance,
  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
  tf2::Transform & tf2_transform)
 {
- geometry_msgs::msg::TransformStamped transform;
  tf2_transform.setIdentity(); // initialize by identical transform
+ geometry_msgs::msg::TransformStamped transform;
+ if (getTransform(source_frame_id, target_frame_id, transform_tolerance, tf_buffer, transform)) {
+ // Convert TransformStamped to TF2 transform
+ tf2::fromMsg(transform.transform, tf2_transform);
+ return true;
+ }
+ return false;
+}
 
+bool getTransform(
+ const std::string & source_frame_id,
+ const rclcpp::Time & source_time,
+ const std::string & target_frame_id,
+ const rclcpp::Time & target_time,
+ const std::string & fixed_frame_id,
+ const tf2::Duration & transform_tolerance,
+ const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+ geometry_msgs::msg::TransformStamped & transform_msg)
+{
  try {
  // Obtaining the transform to get data from source to target frame.
  // This also considers the time shift between source and target.
- transform = tf_buffer->lookupTransform(
+ transform_msg = tf_buffer->lookupTransform(
  target_frame_id, target_time,
  source_frame_id, source_time,
  fixed_frame_id, transform_tolerance);
@@ -138,11 +146,32 @@ bool getTransform(
  return false;
  }
 
- // Convert TransformStamped to TF2 transform
- tf2::fromMsg(transform.transform, tf2_transform);
  return true;
 }
 
+bool getTransform(
+ const std::string & source_frame_id,
+ const rclcpp::Time & source_time,
+ const std::string & target_frame_id,
+ const rclcpp::Time & target_time,
+ const std::string & fixed_frame_id,
+ const tf2::Duration & transform_tolerance,
+ const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+ tf2::Transform & tf2_transform)
+{
+ geometry_msgs::msg::TransformStamped transform;
+ tf2_transform.setIdentity(); // initialize by identical transform
+ if (getTransform(
+ source_frame_id, source_time, target_frame_id, target_time, fixed_frame_id,
+ transform_tolerance, tf_buffer, transform))
+ {
+ // Convert TransformStamped to TF2 transform
+ tf2::fromMsg(transform.transform, tf2_transform);
+ return true;
+ }
+ return false;
+}
+
 bool validateTwist(const geometry_msgs::msg::Twist & msg)
 {
  if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) {