diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
index 9bd07f36c3..892f404d74 100644
--- a/nav2_bringup/params/nav2_multirobot_params_1.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml
@@ -266,7 +266,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
index 01f650e9aa..4fefcb0567 100644
--- a/nav2_bringup/params/nav2_multirobot_params_2.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml
@@ -266,7 +266,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index ad24fceafb..a5e5c180ba 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -307,7 +307,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
index 2e8ecb1aad..4f645422fd 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
@@ -29,11 +29,6 @@
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
-#include "nav2_util/robot_utils.hpp"
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wpedantic"
-#include "tf2/utils.h"
-#pragma GCC diagnostic pop
 
 namespace nav2_costmap_2d
 {
@@ -51,11 +46,8 @@ class CostmapTopicCollisionChecker
   CostmapTopicCollisionChecker(
     CostmapSubscriber & costmap_sub,
     FootprintSubscriber & footprint_sub,
-    tf2_ros::Buffer & tf,
-    std::string name = "collision_checker",
-    std::string global_frame = "map",
-    std::string robot_base_frame = "base_link",
-    double transform_tolerance = 0.1);
+    std::string name = "collision_checker");
+
   /**
    * @brief A destructor
    */
@@ -63,32 +55,45 @@ class CostmapTopicCollisionChecker
 
   /**
    * @brief Returns the obstacle footprint score for a particular pose
+   *
+   * @param pose Pose to get score at
+   * @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once,
+   * data should be fetched in the first check but fetching can be skipped in consequent checks for speedup
    */
-  double scorePose(const geometry_msgs::msg::Pose2D & pose, bool updateCostmap = true);
+  double scorePose(
+    const geometry_msgs::msg::Pose2D & pose,
+    bool fetch_costmap_and_footprint = true);
+
   /**
    * @brief Returns if a pose is collision free
+   *
+   * @param pose Pose to check collision at
+   * @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once,
+   * data should be fetched in the first check but fetching can be skipped in consequent checks for speedup
    */
-  bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose, bool updateCostmap = true);
+  bool isCollisionFree(
+    const geometry_msgs::msg::Pose2D & pose,
+    bool fetch_costmap_and_footprint = true);
 
 protected:
-  /**
-   * @brief Set a new footprint
-   */
-  void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
   /**
    * @brief Get a footprint at a set pose
+   *
+   * @param pose Pose to get footprint at
+   * @param fetch_latest_footprint Defaults to true. When checking with multiple poses at once,
+   * footprint should be fetched in the first check but fetching can be skipped in consequent checks for speedup
    */
-  Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose);
+  Footprint getFootprint(
+    const geometry_msgs::msg::Pose2D & pose,
+    bool fetch_latest_footprint = true);
 
   // Name used for logging
   std::string name_;
-  std::string global_frame_;
-  std::string robot_base_frame_;
-  tf2_ros::Buffer & tf_;
   CostmapSubscriber & costmap_sub_;
   FootprintSubscriber & footprint_sub_;
-  double transform_tolerance_;
   FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
+  rclcpp::Clock::SharedPtr clock_;
+  Footprint footprint_;
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
index d757db53bd..9993d9190a 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
@@ -21,6 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -38,7 +39,9 @@ class FootprintSubscriber
   FootprintSubscriber(
     const nav2_util::LifecycleNode::WeakPtr & parent,
     const std::string & topic_name,
-    const double & footprint_timeout);
+    tf2_ros::Buffer & tf,
+    std::string robot_base_frame = "base_link",
+    double transform_tolerance = 0.1);
 
   /**
    * @brief A constructor
@@ -46,7 +49,9 @@ class FootprintSubscriber
   FootprintSubscriber(
     const rclcpp::Node::WeakPtr & parent,
     const std::string & topic_name,
-    const double & footprint_timeout);
+    tf2_ros::Buffer & tf,
+    std::string robot_base_frame = "base_link",
+    double transform_tolerance = 0.1);
 
   /**
    * @brief A destructor
@@ -54,34 +59,38 @@ class FootprintSubscriber
   ~FootprintSubscriber() {}
 
   /**
-   * @brief Returns an oriented robot footprint at current time.
+   * @brief Returns the latest robot footprint, in the form as received from topic (oriented).
+   *
+   * @param footprint Output param. Latest received footprint
+   * @param footprint_header Output param. Header associated with the footprint
+   * @return False if no footprint has been received
    */
-  bool getFootprint(
+  bool getFootprintRaw(
     std::vector<geometry_msgs::msg::Point> & footprint,
-    rclcpp::Duration & valid_footprint_timeout);
-  /**
-   * @brief Returns an oriented robot footprint without timeout
-   */
-  bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
+    std_msgs::msg::Header & footprint_header);
 
   /**
-   * @brief Returns an oriented robot footprint at stamped time.
+   * @brief Returns the latest robot footprint, transformed into robot base frame (unoriented).
+   *
+   * @param footprint Output param. Latest received footprint, unoriented
+   * @param footprint_header Output param. Header associated with the footprint
+   * @return False if no footprint has been received or if transformation failed
    */
-  bool getFootprint(
+  bool getFootprintInRobotFrame(
     std::vector<geometry_msgs::msg::Point> & footprint,
-    rclcpp::Time & stamp, rclcpp::Duration valid_footprint_timeout);
+    std_msgs::msg::Header & footprint_header);
 
 protected:
-  rclcpp::Clock::SharedPtr clock_;
-
   /**
    * @brief Callback to process new footprint updates.
    */
   void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
 
   std::string topic_name_;
+  tf2_ros::Buffer & tf_;
+  std::string robot_base_frame_;
+  double transform_tolerance_;
   bool footprint_received_{false};
-  rclcpp::Duration footprint_timeout_;
   geometry_msgs::msg::PolygonStamped::SharedPtr footprint_;
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
 };
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 2fdc86f57b..1a194aa7cc 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -466,8 +466,7 @@ Costmap2DROS::updateMap()
       layered_costmap_->updateMap(x, y, yaw);
 
       auto footprint = std::make_unique<geometry_msgs::msg::PolygonStamped>();
-      footprint->header.frame_id = global_frame_;
-      footprint->header.stamp = now();
+      footprint->header = pose.header;
       transformFootprint(x, y, yaw, padded_footprint_, *footprint);
 
       RCLCPP_DEBUG(get_logger(), "Publishing footprint");
diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
index fefe1e0932..11168f6373 100644
--- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
+++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
@@ -35,28 +35,19 @@ namespace nav2_costmap_2d
 CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   CostmapSubscriber & costmap_sub,
   FootprintSubscriber & footprint_sub,
-  tf2_ros::Buffer & tf,
-  std::string name,
-  std::string global_frame,
-  std::string robot_base_frame,
-  double transform_tolerance)
+  std::string name)
 : name_(name),
-  global_frame_(global_frame),
-  robot_base_frame_(robot_base_frame),
-  tf_(tf),
   costmap_sub_(costmap_sub),
   footprint_sub_(footprint_sub),
-  transform_tolerance_(transform_tolerance),
   collision_checker_(nullptr)
-{
-}
+{}
 
 bool CostmapTopicCollisionChecker::isCollisionFree(
   const geometry_msgs::msg::Pose2D & pose,
-  bool updateCostmap)
+  bool fetch_costmap_and_footprint)
 {
   try {
-    if (scorePose(pose, updateCostmap) >= LETHAL_OBSTACLE) {
+    if (scorePose(pose, fetch_costmap_and_footprint) >= LETHAL_OBSTACLE) {
       return false;
     }
     return true;
@@ -74,9 +65,9 @@ bool CostmapTopicCollisionChecker::isCollisionFree(
 
 double CostmapTopicCollisionChecker::scorePose(
   const geometry_msgs::msg::Pose2D & pose,
-  bool updateCostmap)
+  bool fetch_costmap_and_footprint)
 {
-  if (updateCostmap) {
+  if (fetch_costmap_and_footprint) {
     try {
       collision_checker_.setCostmap(costmap_sub_.getCostmap());
     } catch (const std::runtime_error & e) {
@@ -90,43 +81,23 @@ double CostmapTopicCollisionChecker::scorePose(
     throw IllegalPoseException(name_, "Pose Goes Off Grid.");
   }
 
-  return collision_checker_.footprintCost(getFootprint(pose));
+  return collision_checker_.footprintCost(getFootprint(pose, fetch_costmap_and_footprint));
 }
 
-Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
+Footprint CostmapTopicCollisionChecker::getFootprint(
+  const geometry_msgs::msg::Pose2D & pose,
+  bool fetch_latest_footprint)
 {
-  Footprint footprint;
-  if (!footprint_sub_.getFootprint(footprint)) {
-    throw CollisionCheckerException("Current footprint not available.");
+  if (fetch_latest_footprint) {
+    std_msgs::msg::Header header;
+    if (!footprint_sub_.getFootprintInRobotFrame(footprint_, header)) {
+      throw CollisionCheckerException("Current footprint not available.");
+    }
   }
-
-  Footprint footprint_spec;
-  unorientFootprint(footprint, footprint_spec);
-  transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
+  Footprint footprint;
+  transformFootprint(pose.x, pose.y, pose.theta, footprint_, footprint);
 
   return footprint;
 }
 
-void CostmapTopicCollisionChecker::unorientFootprint(
-  const std::vector<geometry_msgs::msg::Point> & oriented_footprint,
-  std::vector<geometry_msgs::msg::Point> & reset_footprint)
-{
-  geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(
-      current_pose, tf_, global_frame_, robot_base_frame_,
-      transform_tolerance_))
-  {
-    throw CollisionCheckerException("Robot pose unavailable.");
-  }
-
-  double x = current_pose.pose.position.x;
-  double y = current_pose.pose.position.y;
-  double theta = tf2::getYaw(current_pose.pose.orientation);
-
-  Footprint temp;
-  transformFootprint(-x, -y, 0, oriented_footprint, temp);
-  transformFootprint(0, 0, -theta, temp, reset_footprint);
-}
-
-
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp
index 1efe6c5396..608a494a9d 100644
--- a/nav2_costmap_2d/src/footprint.cpp
+++ b/nav2_costmap_2d/src/footprint.cpp
@@ -112,14 +112,15 @@ void transformFootprint(
   std::vector<geometry_msgs::msg::Point> & oriented_footprint)
 {
   // build the oriented footprint at a given location
-  oriented_footprint.clear();
+  oriented_footprint.resize(footprint_spec.size());
   double cos_th = cos(theta);
   double sin_th = sin(theta);
   for (unsigned int i = 0; i < footprint_spec.size(); ++i) {
-    geometry_msgs::msg::Point new_pt;
-    new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
-    new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
-    oriented_footprint.push_back(new_pt);
+    double new_x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
+    double new_y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
+    geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
+    new_pt.x = new_x;
+    new_pt.y = new_y;
   }
 }
 
diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp
index f87d7d7f1b..45e60db65e 100644
--- a/nav2_costmap_2d/src/footprint_subscriber.cpp
+++ b/nav2_costmap_2d/src/footprint_subscriber.cpp
@@ -17,6 +17,10 @@
 #include <memory>
 
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
 
 namespace nav2_costmap_2d
 {
@@ -24,12 +28,15 @@ namespace nav2_costmap_2d
 FootprintSubscriber::FootprintSubscriber(
   const nav2_util::LifecycleNode::WeakPtr & parent,
   const std::string & topic_name,
-  const double & footprint_timeout)
+  tf2_ros::Buffer & tf,
+  std::string robot_base_frame,
+  double transform_tolerance)
 : topic_name_(topic_name),
-  footprint_timeout_(rclcpp::Duration::from_seconds(footprint_timeout))
+  tf_(tf),
+  robot_base_frame_(robot_base_frame),
+  transform_tolerance_(transform_tolerance)
 {
   auto node = parent.lock();
-  clock_ = node->get_clock();
   footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
     topic_name, rclcpp::SystemDefaultsQoS(),
     std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
@@ -38,22 +45,24 @@ FootprintSubscriber::FootprintSubscriber(
 FootprintSubscriber::FootprintSubscriber(
   const rclcpp::Node::WeakPtr & parent,
   const std::string & topic_name,
-  const double & footprint_timeout)
+  tf2_ros::Buffer & tf,
+  std::string robot_base_frame,
+  double transform_tolerance)
 : topic_name_(topic_name),
-  footprint_timeout_(rclcpp::Duration::from_seconds(footprint_timeout))
+  tf_(tf),
+  robot_base_frame_(robot_base_frame),
+  transform_tolerance_(transform_tolerance)
 {
   auto node = parent.lock();
-  clock_ = node->get_clock();
   footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
     topic_name, rclcpp::SystemDefaultsQoS(),
     std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
 }
 
 bool
-FootprintSubscriber::getFootprint(
+FootprintSubscriber::getFootprintRaw(
   std::vector<geometry_msgs::msg::Point> & footprint,
-  rclcpp::Time & stamp,
-  rclcpp::Duration valid_footprint_timeout)
+  std_msgs::msg::Header & footprint_header)
 {
   if (!footprint_received_) {
     return false;
@@ -62,29 +71,40 @@ FootprintSubscriber::getFootprint(
   auto current_footprint = std::atomic_load(&footprint_);
   footprint = toPointVector(
     std::make_shared<geometry_msgs::msg::Polygon>(current_footprint->polygon));
-  auto & footprint_stamp = current_footprint->header.stamp;
-
-  if (stamp - footprint_stamp > valid_footprint_timeout) {
-    return false;
-  }
+  footprint_header = current_footprint->header;
 
   return true;
 }
 
 bool
-FootprintSubscriber::getFootprint(
+FootprintSubscriber::getFootprintInRobotFrame(
   std::vector<geometry_msgs::msg::Point> & footprint,
-  rclcpp::Duration & valid_footprint_timeout)
+  std_msgs::msg::Header & footprint_header)
 {
-  rclcpp::Time t = clock_->now();
-  return getFootprint(footprint, t, valid_footprint_timeout);
-}
+  if (!getFootprintRaw(footprint, footprint_header)) {
+    return false;
+  }
 
-bool
-FootprintSubscriber::getFootprint(
-  std::vector<geometry_msgs::msg::Point> & footprint)
-{
-  return getFootprint(footprint, footprint_timeout_);
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, tf_, footprint_header.frame_id, robot_base_frame_,
+      transform_tolerance_, footprint_header.stamp))
+  {
+    return false;
+  }
+
+  double x = current_pose.pose.position.x;
+  double y = current_pose.pose.position.y;
+  double theta = tf2::getYaw(current_pose.pose.orientation);
+
+  std::vector<geometry_msgs::msg::Point> temp;
+  transformFootprint(-x, -y, 0, footprint, temp);
+  transformFootprint(0, 0, -theta, temp, footprint);
+
+  footprint_header.frame_id = robot_base_frame_;
+  footprint_header.stamp = current_pose.header.stamp;
+
+  return true;
 }
 
 void
diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
index 1fc0885ecf..147d79c1d0 100644
--- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
+++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
@@ -72,8 +72,9 @@ class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
 public:
   DummyFootprintSubscriber(
     nav2_util::LifecycleNode::SharedPtr node,
-    std::string & topic_name)
-  : FootprintSubscriber(node, topic_name, 10.0)
+    std::string & topic_name,
+    tf2_ros::Buffer & tf_)
+  : FootprintSubscriber(node, topic_name, tf_)
   {}
 
   void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
@@ -122,10 +123,11 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
 
     footprint_sub_ = std::make_shared<DummyFootprintSubscriber>(
       shared_from_this(),
-      footprint_topic);
+      footprint_topic,
+      *tf_buffer_);
 
     collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-      *costmap_sub_, *footprint_sub_, *tf_buffer_, get_name(), "map");
+      *costmap_sub_, *footprint_sub_, get_name());
 
     layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false);
     // Add Static Layer
@@ -175,13 +177,14 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
 
   bool testPose(double x, double y, double theta)
   {
-    publishPose(x, y, theta);
+    rclcpp::Time stamp = now();
+    publishPose(x, y, theta, stamp);
     geometry_msgs::msg::Pose2D pose;
     pose.x = x;
     pose.y = y;
     pose.theta = theta;
 
-    setPose(x, y, theta);
+    setPose(x, y, theta, stamp);
     publishFootprint();
     publishCostmap();
     rclcpp::sleep_for(std::chrono::milliseconds(1000));
@@ -198,23 +201,25 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
   }
 
 protected:
-  void setPose(double x, double y, double theta)
+  void setPose(double x, double y, double theta, const rclcpp::Time & stamp)
   {
     x_ = x;
     y_ = y;
     yaw_ = theta;
+    stamp_ = stamp;
 
     current_pose_.pose.position.x = x_;
     current_pose_.pose.position.y = y_;
     current_pose_.pose.position.z = 0;
     current_pose_.pose.orientation = orientationAroundZAxis(yaw_);
+    current_pose_.header.stamp = stamp;
   }
 
   void publishFootprint()
   {
     geometry_msgs::msg::PolygonStamped oriented_footprint;
     oriented_footprint.header.frame_id = global_frame_;
-    oriented_footprint.header.stamp = now();
+    oriented_footprint.header.stamp = stamp_;
     nav2_costmap_2d::transformFootprint(x_, y_, yaw_, footprint_, oriented_footprint);
     footprint_sub_->setFootprint(
       std::make_shared<geometry_msgs::msg::PolygonStamped>(oriented_footprint));
@@ -227,11 +232,11 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
       std::make_shared<nav2_msgs::msg::Costmap>(toCostmapMsg(layers_->getCostmap())));
   }
 
-  void publishPose(double x, double y, double /*theta*/)
+  void publishPose(double x, double y, double /*theta*/, const rclcpp::Time & stamp)
   {
     geometry_msgs::msg::TransformStamped tf_stamped;
     tf_stamped.header.frame_id = "map";
-    tf_stamped.header.stamp = now() + rclcpp::Duration(1.0ns);
+    tf_stamped.header.stamp = stamp;
     tf_stamped.child_frame_id = "base_link";
     tf_stamped.transform.translation.x = x;
     tf_stamped.transform.translation.y = y;
@@ -251,7 +256,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
 
     nav2_msgs::msg::Costmap costmap_msg;
     costmap_msg.header.frame_id = global_frame_;
-    costmap_msg.header.stamp = now();
+    costmap_msg.header.stamp = stamp_;
     costmap_msg.metadata.layer = "master";
     costmap_msg.metadata.resolution = resolution;
     costmap_msg.metadata.size_x = costmap->getSizeInCellsX();
@@ -280,6 +285,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
   nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
   std::string global_frame_;
   double x_, y_, yaw_;
+  rclcpp::Time stamp_;
   geometry_msgs::msg::PoseStamped current_pose_;
   std::vector<geometry_msgs::msg::Point> footprint_;
 };
@@ -305,7 +311,7 @@ class TestNode : public ::testing::Test
   std::shared_ptr<TestCollisionChecker> collision_checker_;
 };
 
-TEST_F(TestNode, uknownSpace)
+TEST_F(TestNode, unknownSpace)
 {
   collision_checker_->setFootprint(0, 1);
 
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
index a09d8aeef3..1a7060e0b9 100644
--- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp
+++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
@@ -30,6 +30,10 @@
 #include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_core/recovery.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
 
 namespace nav2_recoveries
 {
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp
index fed49e43f3..dfcb95bb0a 100644
--- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp
+++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp
@@ -92,8 +92,6 @@ class RecoveryServer : public nav2_util::LifecycleNode
   std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
   std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
   std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
-
-  double transform_tolerance_;
 };
 
 }  // namespace recovery_server
diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp
index 12ccc91cdc..d078b6c8b8 100644
--- a/nav2_recoveries/plugins/back_up.cpp
+++ b/nav2_recoveries/plugins/back_up.cpp
@@ -139,7 +139,7 @@ bool BackUp::isCollisionFree(
   const double diff_dist = abs(command_x_) - distance;
   const int max_cycle_count = static_cast<int>(cycle_frequency_ * simulate_ahead_time_);
   geometry_msgs::msg::Pose2D init_pose = pose2d;
-  bool updateCostmap = true;
+  bool fetch_data = true;
 
   while (cycle_count < max_cycle_count) {
     sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_);
@@ -151,10 +151,10 @@ bool BackUp::isCollisionFree(
       break;
     }
 
-    if (!collision_checker_->isCollisionFree(pose2d, updateCostmap)) {
+    if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) {
       return false;
     }
-    updateCostmap = false;
+    fetch_data = false;
   }
   return true;
 }
diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp
index 6b7fc43298..893e3b58e5 100644
--- a/nav2_recoveries/plugins/spin.cpp
+++ b/nav2_recoveries/plugins/spin.cpp
@@ -169,7 +169,7 @@ bool Spin::isCollisionFree(
   double sim_position_change;
   const int max_cycle_count = static_cast<int>(cycle_frequency_ * simulate_ahead_time_);
   geometry_msgs::msg::Pose2D init_pose = pose2d;
-  bool updateCostmap = true;
+  bool fetch_data = true;
 
   while (cycle_count < max_cycle_count) {
     sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_);
@@ -180,10 +180,10 @@ bool Spin::isCollisionFree(
       break;
     }
 
-    if (!collision_checker_->isCollisionFree(pose2d, updateCostmap)) {
+    if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) {
       return false;
     }
-    updateCostmap = false;
+    fetch_data = false;
   }
   return true;
 }
diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp
index e9c28c0fdb..be98df7250 100644
--- a/nav2_recoveries/src/recovery_server.cpp
+++ b/nav2_recoveries/src/recovery_server.cpp
@@ -73,21 +73,19 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   tf_->setCreateTimerInterface(timer_interface);
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
-  std::string costmap_topic, footprint_topic;
+  std::string costmap_topic, footprint_topic, robot_base_frame;
+  double transform_tolerance;
   this->get_parameter("costmap_topic", costmap_topic);
   this->get_parameter("footprint_topic", footprint_topic);
-  this->get_parameter("transform_tolerance", transform_tolerance_);
+  this->get_parameter("transform_tolerance", transform_tolerance);
+  this->get_parameter("robot_base_frame", robot_base_frame);
   costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
     shared_from_this(), costmap_topic);
   footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
-    shared_from_this(), footprint_topic, 1.0);
+    shared_from_this(), footprint_topic, *tf_, robot_base_frame, transform_tolerance);
 
-  std::string global_frame, robot_base_frame;
-  get_parameter("global_frame", global_frame);
-  get_parameter("robot_base_frame", robot_base_frame);
   collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(),
-    global_frame, robot_base_frame, transform_tolerance_);
+    *costmap_sub_, *footprint_sub_, this->get_name());
 
   recovery_types_.resize(recovery_ids_.size());
   if (!loadRecoveryPlugins()) {
diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp
index 6f5c83912e..47a687538b 100644
--- a/nav2_recoveries/test/test_recoveries.cpp
+++ b/nav2_recoveries/test/test_recoveries.cpp
@@ -125,11 +125,11 @@ class RecoveryTest : public ::testing::Test
       node_lifecycle_, costmap_topic);
     std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_ =
       std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
-      node_lifecycle_, footprint_topic, 1.0);
+      node_lifecycle_, footprint_topic, *tf_buffer_);
     std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_ =
       std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-      *costmap_sub_, *footprint_sub_, *tf_buffer_,
-      node_lifecycle_->get_name(), "odom");
+      *costmap_sub_, *footprint_sub_,
+      node_lifecycle_->get_name());
 
     recovery_ = std::make_shared<DummyRecovery>();
     recovery_->configure(node_lifecycle_, "Recovery", tf_buffer_, collision_checker_);
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
index b22b5feb0a..1d3546f3be 100644
--- a/nav2_smoother/src/nav2_smoother.cpp
+++ b/nav2_smoother/src/nav2_smoother.cpp
@@ -49,7 +49,6 @@ SmootherServer::SmootherServer()
     "footprint_topic",
     rclcpp::ParameterValue(
       std::string("global_costmap/published_footprint")));
-  declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
   declare_parameter(
     "robot_base_frame",
     rclcpp::ParameterValue(std::string("base_link")));
@@ -84,23 +83,20 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &)
   tf_->setCreateTimerInterface(timer_interface);
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
-  std::string costmap_topic, footprint_topic;
+  std::string costmap_topic, footprint_topic, robot_base_frame;
   double transform_tolerance;
   this->get_parameter("costmap_topic", costmap_topic);
   this->get_parameter("footprint_topic", footprint_topic);
   this->get_parameter("transform_tolerance", transform_tolerance);
+  this->get_parameter("robot_base_frame", robot_base_frame);
   costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
     shared_from_this(), costmap_topic);
   footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
-    shared_from_this(), footprint_topic, 1.0);
+    shared_from_this(), footprint_topic, *tf_, robot_base_frame, transform_tolerance);
 
-  std::string global_frame, robot_base_frame;
-  get_parameter("global_frame", global_frame);
-  get_parameter("robot_base_frame", robot_base_frame);
   collision_checker_ =
     std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), global_frame,
-    robot_base_frame, transform_tolerance);
+    *costmap_sub_, *footprint_sub_, this->get_name());
 
   if (!loadSmootherPlugins()) {
     return nav2_util::CallbackReturn::FAILURE;
@@ -293,13 +289,13 @@ void SmootherServer::smoothPlan()
     // Check for collisions
     if (goal->check_for_collisions) {
       geometry_msgs::msg::Pose2D pose2d;
-      bool updateCostmap = true;
+      bool fetch_data = true;
       for (const auto & pose : result->path.poses) {
         pose2d.x = pose.pose.position.x;
         pose2d.y = pose.pose.position.y;
         pose2d.theta = tf2::getYaw(pose.pose.orientation);
 
-        if (!collision_checker_->isCollisionFree(pose2d, updateCostmap)) {
+        if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) {
           RCLCPP_ERROR(
             get_logger(),
             "Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf",
@@ -307,7 +303,7 @@ void SmootherServer::smoothPlan()
           action_server_->terminate_current(result);
           return;
         }
-        updateCostmap = false;
+        fetch_data = false;
       }
     }
 
diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp
index 1899dc3c9f..a8bee7bdb4 100644
--- a/nav2_smoother/test/test_smoother_server.cpp
+++ b/nav2_smoother/test/test_smoother_server.cpp
@@ -161,11 +161,11 @@ class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
   DummyFootprintSubscriber(
     nav2_util::LifecycleNode::SharedPtr node,
     const std::string & topic_name,
-    double footprint_timeout)
-  : FootprintSubscriber(node, topic_name, footprint_timeout)
+    tf2_ros::Buffer & tf_)
+  : FootprintSubscriber(node, topic_name, tf_)
   {
     auto footprint = std::make_shared<geometry_msgs::msg::PolygonStamped>();
-    footprint->header.frame_id = "base_link";
+    footprint->header.frame_id = "base_link";  // global frame = robot frame to avoid tf lookup
     footprint->header.stamp = node->get_clock()->now();
     geometry_msgs::msg::Point32 point;
     point.x = -0.2f;
@@ -215,12 +215,11 @@ class DummySmootherServer : public nav2_smoother::SmootherServer
       node, "costmap_topic");
     footprint_sub_ =
       std::make_shared<DummyFootprintSubscriber>(
-      node, "footprint_topic", 10.0);
+      node, "footprint_topic", *tf_);
     collision_checker_ =
       std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-      *costmap_sub_, *footprint_sub_, *tf_,
-      node->get_name(),
-      "base_link", "base_link");  // global frame = robot frame to avoid tf lookup
+      *costmap_sub_, *footprint_sub_,
+      node->get_name());
 
     return result;
   }
diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml
index f40566e634..2bef5dcda2 100644
--- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml
+++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml
@@ -304,7 +304,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml
index d9b18f8656..084a1e3537 100644
--- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml
+++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml
@@ -295,7 +295,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml
index db445b6135..b12eaae4b3 100644
--- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml
+++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml
@@ -295,7 +295,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp
index 290deff031..7d8bb826fe 100644
--- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp
+++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp
@@ -53,6 +53,8 @@ BackupRecoveryTester::BackupRecoveryTester()
   subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&BackupRecoveryTester::amclPoseCallback, this, std::placeholders::_1));
+
+  stamp_ = node_->now();
 }
 
 BackupRecoveryTester::~BackupRecoveryTester()
@@ -195,7 +197,7 @@ void BackupRecoveryTester::sendInitialPose()
 {
   geometry_msgs::msg::PoseWithCovarianceStamped pose;
   pose.header.frame_id = "map";
-  pose.header.stamp = rclcpp::Time();
+  pose.header.stamp = stamp_;
   pose.pose.pose.position.x = -2.0;
   pose.pose.pose.position.y = -0.5;
   pose.pose.pose.position.z = 0.0;
diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp
index 4c658860a0..43e9cb5b8d 100644
--- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp
+++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp
@@ -68,6 +68,7 @@ class BackupRecoveryTester
 
   bool is_active_;
   bool initial_pose_received_;
+  rclcpp::Time stamp_;
 
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp
index 73a66bd755..316b35b1fe 100644
--- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp
+++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp
@@ -66,6 +66,8 @@ SpinRecoveryTester::SpinRecoveryTester()
   subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1));
+
+  stamp_ = node_->now();
 }
 
 SpinRecoveryTester::~SpinRecoveryTester()
@@ -263,7 +265,7 @@ void SpinRecoveryTester::sendFakeCostmap(float angle)
   nav2_msgs::msg::Costmap fake_costmap;
 
   fake_costmap.header.frame_id = "odom";
-  fake_costmap.header.stamp = rclcpp::Time();
+  fake_costmap.header.stamp = stamp_;
   fake_costmap.metadata.layer = "master";
   fake_costmap.metadata.resolution = .1;
   fake_costmap.metadata.size_x = 100;
@@ -288,7 +290,7 @@ void SpinRecoveryTester::sendInitialPose()
 {
   geometry_msgs::msg::PoseWithCovarianceStamped pose;
   pose.header.frame_id = "map";
-  pose.header.stamp = rclcpp::Time();
+  pose.header.stamp = stamp_;
   pose.pose.pose.position.x = -2.0;
   pose.pose.pose.position.y = -0.5;
   pose.pose.pose.position.z = 0.0;
@@ -311,7 +313,7 @@ void SpinRecoveryTester::sendFakeOdom(float angle)
 {
   geometry_msgs::msg::TransformStamped transformStamped;
 
-  transformStamped.header.stamp = rclcpp::Time();
+  transformStamped.header.stamp = stamp_;
   transformStamped.header.frame_id = "odom";
   transformStamped.child_frame_id = "base_link";
   transformStamped.transform.translation.x = 0.0;
@@ -328,7 +330,7 @@ void SpinRecoveryTester::sendFakeOdom(float angle)
 
   geometry_msgs::msg::PolygonStamped footprint;
   footprint.header.frame_id = "odom";
-  footprint.header.stamp = node_->now();
+  footprint.header.stamp = stamp_;
   footprint.polygon.points.resize(4);
   footprint.polygon.points[0].x = 0.22;
   footprint.polygon.points[0].y = 0.22;
diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp
index 4ffd429001..a00ffa21e4 100644
--- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp
+++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp
@@ -79,6 +79,7 @@ class SpinRecoveryTester
   bool is_active_;
   bool initial_pose_received_;
   bool make_fake_costmap_;
+  rclcpp::Time stamp_;
 
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index 11368d7030..5281ff8813 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -42,7 +42,8 @@ namespace nav2_util
 bool getCurrentPose(
   geometry_msgs::msg::PoseStamped & global_pose,
   tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map",
-  const std::string robot_frame = "base_link", const double transform_timeout = 0.1);
+  const std::string robot_frame = "base_link", const double transform_timeout = 0.1,
+  const rclcpp::Time stamp = rclcpp::Time());
 
 /**
 * @brief get an arbitrary pose in a target frame
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index 615366de29..e99d9e3334 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -26,11 +26,12 @@ namespace nav2_util
 bool getCurrentPose(
   geometry_msgs::msg::PoseStamped & global_pose,
   tf2_ros::Buffer & tf_buffer, const std::string global_frame,
-  const std::string robot_frame, const double transform_timeout)
+  const std::string robot_frame, const double transform_timeout,
+  const rclcpp::Time stamp)
 {
   tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
   global_pose.header.frame_id = robot_frame;
-  global_pose.header.stamp = rclcpp::Time();
+  global_pose.header.stamp = stamp;
 
   return transformPoseInTargetFrame(
     global_pose, global_pose, tf_buffer, global_frame, transform_timeout);