Nacho/cleanup get transform util

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)) {