Move utility function to base class to reuse implementation

ros-navigation · Mar 28, 2024 · 2bd3588 · 2bd3588
1 parent 5fedb72
commit 2bd3588
Show file tree

Hide file tree

Showing 5 changed files with 50 additions and 68 deletions.
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/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

diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/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");

diff --git a/nav2_collision_monitor/src/range.cpp b/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

diff --git a/nav2_collision_monitor/src/scan.cpp b/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

diff --git a/nav2_collision_monitor/src/source.cpp b/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