Option allowing to use simple lookupTransform API (ros-navigation#3412)

* Option allowing to use simple lookupTransform API
ignoring time shifts between source and base frame during the movement

* Refine comments

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -127,13 +127,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * source->base time inerpolated transform.
    * @param transform_tolerance Transform tolerance
    * @param source_timeout Maximum time interval in which data is considered valid
+   * @param base_shift_correction Whether to correct source data towards to base frame movement,
+   * considering the difference between current time and latest source time
    * @return True if all sources were configured successfully or false in failure case
    */
   bool configureSources(
     const std::string & base_frame_id,
     const std::string & odom_frame_id,
     const tf2::Duration & transform_tolerance,
-    const rclcpp::Duration & source_timeout);
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
 
   /**
    * @brief Main processing routine

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -41,6 +41,8 @@ class PointCloud : public Source
    * @param global_frame_id Global frame ID for correct transform calculation
    * @param transform_tolerance Transform tolerance
    * @param source_timeout Maximum time interval in which data is considered valid
+   * @param base_shift_correction Whether to correct source data towards to base frame movement,
+   * considering the difference between current time and latest source time
    */
   PointCloud(
     const nav2_util::LifecycleNode::WeakPtr & node,
@@ -49,7 +51,8 @@ class PointCloud : public Source
     const std::string & base_frame_id,
     const std::string & global_frame_id,
     const tf2::Duration & transform_tolerance,
-    const rclcpp::Duration & source_timeout);
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
   /**
    * @brief PointCloud destructor
    */

nav2_collision_monitor/include/nav2_collision_monitor/range.hpp

@@ -41,6 +41,8 @@ class Range : public Source
    * @param global_frame_id Global frame ID for correct transform calculation
    * @param transform_tolerance Transform tolerance
    * @param source_timeout Maximum time interval in which data is considered valid
+   * @param base_shift_correction Whether to correct source data towards to base frame movement,
+   * considering the difference between current time and latest source time
    */
   Range(
     const nav2_util::LifecycleNode::WeakPtr & node,
@@ -49,7 +51,8 @@ class Range : public Source
     const std::string & base_frame_id,
     const std::string & global_frame_id,
     const tf2::Duration & transform_tolerance,
-    const rclcpp::Duration & source_timeout);
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
   /**
    * @brief Range destructor
    */

nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp

@@ -41,6 +41,8 @@ class Scan : public Source
    * @param global_frame_id Global frame ID for correct transform calculation
    * @param transform_tolerance Transform tolerance
    * @param source_timeout Maximum time interval in which data is considered valid
+   * @param base_shift_correction Whether to correct source data towards to base frame movement,
+   * considering the difference between current time and latest source time
    */
   Scan(
     const nav2_util::LifecycleNode::WeakPtr & node,
@@ -49,7 +51,8 @@ class Scan : public Source
     const std::string & base_frame_id,
     const std::string & global_frame_id,
     const tf2::Duration & transform_tolerance,
-    const rclcpp::Duration & source_timeout);
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
   /**
    * @brief Scan destructor
    */

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -46,6 +46,8 @@ class Source
    * @param global_frame_id Global frame ID for correct transform calculation
    * @param transform_tolerance Transform tolerance
    * @param source_timeout Maximum time interval in which data is considered valid
+   * @param base_shift_correction Whether to correct source data towards to base frame movement,
+   * considering the difference between current time and latest source time
    */
   Source(
     const nav2_util::LifecycleNode::WeakPtr & node,
@@ -54,7 +56,8 @@ class Source
     const std::string & base_frame_id,
     const std::string & global_frame_id,
     const tf2::Duration & transform_tolerance,
-    const rclcpp::Duration & source_timeout);
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
   /**
    * @brief Source destructor
    */
@@ -110,6 +113,9 @@ class Source
   tf2::Duration transform_tolerance_;
   /// @brief Maximum time interval in which data is considered valid
   rclcpp::Duration source_timeout_;
+  /// @brief Whether to correct source data towards to base frame movement,
+  /// considering the difference between current time and latest source time
+  bool base_shift_correction_;
 };  // class Source
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -6,6 +6,7 @@ collision_monitor:
     cmd_vel_out_topic: "cmd_vel"
     transform_tolerance: 0.5
     source_timeout: 5.0
+    base_shift_correction: True
     stop_pub_timeout: 2.0
     # Polygons represent zone around the robot for "stop" and "slowdown" action types,
     # and robot footprint for "approach" action type.

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -205,6 +205,10 @@ bool CollisionMonitor::getParameters(
     node, "source_timeout", rclcpp::ParameterValue(2.0));
   source_timeout =
     rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double());
+  nav2_util::declare_parameter_if_not_declared(
+    node, "base_shift_correction", rclcpp::ParameterValue(true));
+  const bool base_shift_correction =
+    get_parameter("base_shift_correction").as_bool();
 
   nav2_util::declare_parameter_if_not_declared(
     node, "stop_pub_timeout", rclcpp::ParameterValue(1.0));
@@ -215,7 +219,10 @@ bool CollisionMonitor::getParameters(
     return false;
   }
 
-  if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) {
+  if (
+    !configureSources(
+      base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction))
+  {
     return false;
   }
 
@@ -271,7 +278,8 @@ bool CollisionMonitor::configureSources(
   const std::string & base_frame_id,
   const std::string & odom_frame_id,
   const tf2::Duration & transform_tolerance,
-  const rclcpp::Duration & source_timeout)
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
 {
   try {
     auto node = shared_from_this();
@@ -289,23 +297,23 @@ bool CollisionMonitor::configureSources(
       if (source_type == "scan") {
         std::shared_ptr<Scan> s = std::make_shared<Scan>(
           node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
-          transform_tolerance, source_timeout);
+          transform_tolerance, source_timeout, base_shift_correction);
 
         s->configure();
 
         sources_.push_back(s);
       } else if (source_type == "pointcloud") {
         std::shared_ptr<PointCloud> p = std::make_shared<PointCloud>(
           node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
-          transform_tolerance, source_timeout);
+          transform_tolerance, source_timeout, base_shift_correction);
 
         p->configure();
 
         sources_.push_back(p);
       } else if (source_type == "range") {
         std::shared_ptr<Range> r = std::make_shared<Range>(
           node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
-          transform_tolerance, source_timeout);
+          transform_tolerance, source_timeout, base_shift_correction);
 
         r->configure();
 

nav2_collision_monitor/src/pointcloud.cpp

@@ -31,10 +31,11 @@ PointCloud::PointCloud(
   const std::string & base_frame_id,
   const std::string & global_frame_id,
   const tf2::Duration & transform_tolerance,
-  const rclcpp::Duration & source_timeout)
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
 : Source(
     node, source_name, tf_buffer, base_frame_id, global_frame_id,
-    transform_tolerance, source_timeout),
+    transform_tolerance, source_timeout, base_shift_correction),
   data_(nullptr)
 {
   RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str());
@@ -76,16 +77,29 @@ void PointCloud::getData(
     return;
   }
 
-  // Obtaining the transform to get data from source frame and time where it was received
-  // to the base frame and current time
   tf2::Transform tf_transform;
-  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;
+  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;
+    }
+  } 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;
+    }
   }
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(*data_, "x");

nav2_collision_monitor/src/range.cpp

@@ -31,10 +31,11 @@ Range::Range(
   const std::string & base_frame_id,
   const std::string & global_frame_id,
   const tf2::Duration & transform_tolerance,
-  const rclcpp::Duration & source_timeout)
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
 : Source(
     node, source_name, tf_buffer, base_frame_id, global_frame_id,
-    transform_tolerance, source_timeout),
+    transform_tolerance, source_timeout, base_shift_correction),
   data_(nullptr)
 {
   RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str());
@@ -85,16 +86,29 @@ void Range::getData(
     return;
   }
 
-  // Obtaining the transform to get data from source frame and time where it was received
-  // to the base frame and current time
   tf2::Transform tf_transform;
-  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;
+  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;
+    }
+  } 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;
+    }
   }
 
   // Calculate poses and refill data array

nav2_collision_monitor/src/scan.cpp

@@ -29,10 +29,11 @@ Scan::Scan(
   const std::string & base_frame_id,
   const std::string & global_frame_id,
   const tf2::Duration & transform_tolerance,
-  const rclcpp::Duration & source_timeout)
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
 : Source(
     node, source_name, tf_buffer, base_frame_id, global_frame_id,
-    transform_tolerance, source_timeout),
+    transform_tolerance, source_timeout, base_shift_correction),
   data_(nullptr)
 {
   RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str());
@@ -75,16 +76,29 @@ void Scan::getData(
     return;
   }
 
-  // Obtaining the transform to get data from source frame and time where it was received
-  // to the base frame and current time
   tf2::Transform tf_transform;
-  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;
+  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;
+    }
+  } 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;
+    }
   }
 
   // Calculate poses and refill data array

nav2_collision_monitor/src/source.cpp

@@ -30,10 +30,12 @@ Source::Source(
   const std::string & base_frame_id,
   const std::string & global_frame_id,
   const tf2::Duration & transform_tolerance,
-  const rclcpp::Duration & source_timeout)
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
 : node_(node), source_name_(source_name), tf_buffer_(tf_buffer),
   base_frame_id_(base_frame_id), global_frame_id_(global_frame_id),
-  transform_tolerance_(transform_tolerance), source_timeout_(source_timeout)
+  transform_tolerance_(transform_tolerance), source_timeout_(source_timeout),
+  base_shift_correction_(base_shift_correction)
 {
 }