fixing a second merge conflict resolution error (#3621)

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -91,21 +91,6 @@ class Source
     const rclcpp::Time & source_time,
     const rclcpp::Time & curr_time) const;
 
-  /**
-   * @brief Obtains a transform from source_frame_id at source_time ->
-   * to base_frame_id_ at curr_time time
-   * @param source_frame_id Source frame ID to convert data from
-   * @param source_time Source timestamp to convert data from
-   * @param curr_time Current node time to interpolate data to
-   * @param tf_transform Output source->base transform
-   * @return True if got correct transform, otherwise false
-   */
-  bool getTransform(
-    const std::string & source_frame_id,
-    const rclcpp::Time & source_time,
-    const rclcpp::Time & curr_time,
-    tf2::Transform & tf_transform) const;
-
   // ----- Variables -----
 
   /// @brief Collision Monitor node

nav2_collision_monitor/src/source.cpp

@@ -78,33 +78,4 @@ bool Source::sourceValid(
   return true;
 }
 
-bool Source::getTransform(
-  const std::string & source_frame_id,
-  const rclcpp::Time & source_time,
-  const rclcpp::Time & curr_time,
-  tf2::Transform & tf2_transform) const
-{
-  geometry_msgs::msg::TransformStamped transform;
-  tf2_transform.setIdentity();  // initialize by identical transform
-
-  try {
-    // Obtaining the transform to get data from source to base frame.
-    // This also considers the time shift between source and base.
-    transform = tf_buffer_->lookupTransform(
-      base_frame_id_, curr_time,
-      source_frame_id, source_time,
-      global_frame_id_, transform_tolerance_);
-  } catch (tf2::TransformException & e) {
-    RCLCPP_ERROR(
-      logger_,
-      "[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s",
-      source_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what());
-    return false;
-  }
-
-  // Convert TransformStamped to TF2 transform
-  tf2::fromMsg(transform.transform, tf2_transform);
-  return true;
-}
-
 }  // namespace nav2_collision_monitor

nav2_util/include/nav2_util/robot_utils.hpp

@@ -56,6 +56,57 @@ bool transformPoseInTargetFrame(
   tf2_ros::Buffer & tf_buffer, const std::string target_frame,
   const double transform_timeout = 0.1);
 
+/**
+ * @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 Target time to interpolate to
+ * @param transform_tolerance Transform tolerance
+ * @param tf_transform Output source->target transform
+ * @return True if got correct transform, otherwise false
+ */
+
+/**
+ * @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 tf_transform Output source->target transform
+ * @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,
+  tf2::Transform & tf2_transform);
+
+/**
+ * @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 tf_transform Output source->target transform
+ * @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,
+  tf2::Transform & tf2_transform);
+
 }  // end namespace nav2_util
 
 #endif  // NAV2_UTIL__ROBOT_UTILS_HPP_

nav2_util/src/robot_utils.cpp

@@ -75,4 +75,70 @@ bool transformPoseInTargetFrame(
   return 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,
+  tf2::Transform & tf2_transform)
+{
+  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(
+      target_frame_id, source_frame_id,
+      tf2::TimePointZero, transform_tolerance);
+  } 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 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
+
+  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(
+      target_frame_id, target_time,
+      source_frame_id, source_time,
+      fixed_frame_id, transform_tolerance);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("getTransform"),
+      "Failed to get \"%s\"->\"%s\" frame transform: %s",
+      source_frame_id.c_str(), target_frame_id.c_str(), ex.what());
+    return false;
+  }
+
+  // Convert TransformStamped to TF2 transform
+  tf2::fromMsg(transform.transform, tf2_transform);
+  return true;
+}
+
 }  // end namespace nav2_util