Extend util function by also allowing to get the transform msg instea…

nav2_util/include/nav2_util/robot_utils.hpp

@@ -22,6 +22,7 @@
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist.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"
@@ -98,6 +99,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)) {