diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp
index 1041d74d8e9..5d3fa6de52e 100644
--- a/nav2_core/include/nav2_core/recovery.hpp
+++ b/nav2_core/include/nav2_core/recovery.hpp
@@ -21,7 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "tf2_ros/buffer.h"
-#include "nav2_costmap_2d/collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 
 namespace nav2_core
 {
@@ -49,7 +49,7 @@ class Recovery
   virtual void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
     const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
-    std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker) = 0;
+    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker) = 0;
 
   /**
    * @brief Method to cleanup resources used on shutdown.
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index 8f3262b8a69..d7508415edf 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -47,6 +47,7 @@ add_library(nav2_costmap_2d_core SHARED
   src/costmap_layer.cpp
   src/observation_buffer.cpp
   src/clear_costmap_service.cpp
+  src/footprint_collision_checker.cpp
 )
 
 # prevent pluginlib from using boost
@@ -94,7 +95,7 @@ target_link_libraries(layers
 add_library(nav2_costmap_2d_client SHARED
   src/footprint_subscriber.cpp
   src/costmap_subscriber.cpp
-  src/collision_checker.cpp
+  src/costmap_topic_collision_checker.cpp
 )
 
 ament_target_dependencies(nav2_costmap_2d_client
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
similarity index 77%
rename from nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
index 13139e6cd7d..4a511092a66 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
@@ -11,9 +11,11 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+//
+// Modified by: Shivang Patel (shivaan14@gmail.com)
 
-#ifndef NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
-#define NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
+#ifndef NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
+#define NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
 
 #include <string>
 #include <vector>
@@ -24,6 +26,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
+#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"
@@ -34,33 +37,26 @@
 
 namespace nav2_costmap_2d
 {
-typedef std::vector<geometry_msgs::msg::Point> Footprint;
 
-class CollisionChecker
+class CostmapTopicCollisionChecker
 {
 public:
-  CollisionChecker(
+  CostmapTopicCollisionChecker(
     CostmapSubscriber & costmap_sub,
     FootprintSubscriber & footprint_sub,
     tf2_ros::Buffer & tf,
     std::string name = "collision_checker",
     std::string global_frame = "map");
 
-  ~CollisionChecker();
+  ~CostmapTopicCollisionChecker() = default;
 
   // Returns the obstacle footprint score for a particular pose
   double scorePose(const geometry_msgs::msg::Pose2D & pose);
   bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose);
 
 protected:
-  double lineCost(int x0, int x1, int y0, int y1) const;
-  double pointCost(int x, int y) const;
   void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
-  void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
   Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose);
-  double footprintCost(const Footprint footprint);
-
-  std::shared_ptr<Costmap2D> costmap_;
 
   // Name used for logging
   std::string name_;
@@ -68,7 +64,9 @@ class CollisionChecker
   tf2_ros::Buffer & tf_;
   CostmapSubscriber & costmap_sub_;
   FootprintSubscriber & footprint_sub_;
+  FootprintCollisionChecker collision_checker_;
 };
+
 }  // namespace nav2_costmap_2d
 
-#endif  // NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
+#endif  // NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp
new file mode 100644
index 00000000000..24f35aa1359
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp
@@ -0,0 +1,53 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Modified by: Shivang Patel (shivaang14@gmail.com)
+
+#ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
+#define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace nav2_costmap_2d
+{
+typedef std::vector<geometry_msgs::msg::Point> Footprint;
+
+class FootprintCollisionChecker
+{
+public:
+  FootprintCollisionChecker();
+  explicit FootprintCollisionChecker(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap);
+  double footprintCost(const Footprint footprint);
+  double footprintCostAtPose(double x, double y, double theta, const Footprint footprint);
+  double lineCost(int x0, int x1, int y0, int y1) const;
+  bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+  double pointCost(int x, int y) const;
+  void setCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap);
+
+private:
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
similarity index 51%
rename from nav2_costmap_2d/src/collision_checker.cpp
rename to nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
index 4e8d92c1278..610528cb4bf 100644
--- a/nav2_costmap_2d/src/collision_checker.cpp
+++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
@@ -11,13 +11,16 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+//
+// Modified by: Shivang Patel (shivaan14@gmail.com)
 
 #include <memory>
 #include <string>
 #include <vector>
 #include <algorithm>
+#include <iostream>
 
-#include "nav2_costmap_2d/collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_costmap_2d/exceptions.hpp"
@@ -29,7 +32,7 @@ using namespace std::chrono_literals;
 namespace nav2_costmap_2d
 {
 
-CollisionChecker::CollisionChecker(
+CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   CostmapSubscriber & costmap_sub,
   FootprintSubscriber & footprint_sub,
   tf2_ros::Buffer & tf,
@@ -39,15 +42,12 @@ CollisionChecker::CollisionChecker(
   global_frame_(global_frame),
   tf_(tf),
   costmap_sub_(costmap_sub),
-  footprint_sub_(footprint_sub)
+  footprint_sub_(footprint_sub),
+  collision_checker_(nullptr)
 {
 }
 
-CollisionChecker::~CollisionChecker()
-{
-}
-
-bool CollisionChecker::isCollisionFree(
+bool CostmapTopicCollisionChecker::isCollisionFree(
   const geometry_msgs::msg::Pose2D & pose)
 {
   try {
@@ -67,33 +67,25 @@ bool CollisionChecker::isCollisionFree(
   }
 }
 
-double CollisionChecker::scorePose(
+double CostmapTopicCollisionChecker::scorePose(
   const geometry_msgs::msg::Pose2D & pose)
 {
   try {
-    costmap_ = costmap_sub_.getCostmap();
+    collision_checker_.setCostmap(costmap_sub_.getCostmap());
   } catch (const std::runtime_error & e) {
     throw CollisionCheckerException(e.what());
   }
 
   unsigned int cell_x, cell_y;
-  if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
+  if (!collision_checker_.worldToMap(pose.x, pose.y, cell_x, cell_y)) {
     RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", cell_x, cell_y);
     throw IllegalPoseException(name_, "Pose Goes Off Grid.");
   }
 
-  return footprintCost(getFootprint(pose));
+  return collision_checker_.footprintCost(getFootprint(pose));
 }
 
-void CollisionChecker::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
-{
-  if (!costmap_->worldToMap(wx, wy, mx, my)) {
-    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", mx, my);
-    throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
-  }
-}
-
-Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
+Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
 {
   Footprint footprint;
   if (!footprint_sub_.getFootprint(footprint)) {
@@ -107,68 +99,7 @@ Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose
   return footprint;
 }
 
-double CollisionChecker::footprintCost(const Footprint footprint)
-{
-  // now we really have to lay down the footprint in the costmap_ grid
-  unsigned int x0, x1, y0, y1;
-  double footprint_cost = 0.0;
-
-  // we need to rasterize each line in the footprint
-  for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
-    // get the cell coord of the first point
-    worldToMap(footprint[i].x, footprint[i].y, x0, y0);
-
-    // get the cell coord of the second point
-    worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1);
-
-    footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
-  }
-
-  // we also need to connect the first point in the footprint to the last point
-  // get the cell coord of the last point
-  worldToMap(footprint.back().x, footprint.back().y, x0, y0);
-
-  // get the cell coord of the first point
-  worldToMap(footprint.front().x, footprint.front().y, x1, y1);
-
-  footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
-
-  // if all line costs are legal... then we can return that the footprint is legal
-  return footprint_cost;
-}
-
-double CollisionChecker::lineCost(int x0, int x1, int y0, int y1) const
-{
-  double line_cost = 0.0;
-  double point_cost = -1.0;
-
-  for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
-    point_cost = pointCost(line.getX(), line.getY());   // Score the current point
-
-    if (line_cost < point_cost) {
-      line_cost = point_cost;
-    }
-  }
-
-  return line_cost;
-}
-
-double CollisionChecker::pointCost(int x, int y) const
-{
-  unsigned char cost = costmap_->getCost(x, y);
-  // if the cell is in an obstacle the path is invalid or unknown
-  if (cost == LETHAL_OBSTACLE) {
-    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y);
-    throw IllegalPoseException(name_, "Footprint Hits Obstacle.");
-  } else if (cost == NO_INFORMATION) {
-    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y);
-    throw IllegalPoseException(name_, "Footprint Hits Unknown Region.");
-  }
-
-  return cost;
-}
-
-void CollisionChecker::unorientFootprint(
+void CostmapTopicCollisionChecker::unorientFootprint(
   const std::vector<geometry_msgs::msg::Point> & oriented_footprint,
   std::vector<geometry_msgs::msg::Point> & reset_footprint)
 {
@@ -186,4 +117,5 @@ void CollisionChecker::unorientFootprint(
   transformFootprint(0, 0, -theta, temp, reset_footprint);
 }
 
+
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp
new file mode 100644
index 00000000000..15adcd8665b
--- /dev/null
+++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp
@@ -0,0 +1,131 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Modified by: Shivang Patel (shivaang14@gmail.com)
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <algorithm>
+
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/exceptions.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/line_iterator.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+FootprintCollisionChecker::FootprintCollisionChecker()
+: costmap_(nullptr)
+{
+}
+
+FootprintCollisionChecker::FootprintCollisionChecker(
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap)
+: costmap_(costmap)
+{
+}
+
+double FootprintCollisionChecker::footprintCost(const Footprint footprint)
+{
+  // now we really have to lay down the footprint in the costmap_ grid
+  unsigned int x0, x1, y0, y1;
+  double footprint_cost = 0.0;
+
+  // we need to rasterize each line in the footprint
+  for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
+    // get the cell coord of the first point
+    if (!worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
+      return static_cast<double>(LETHAL_OBSTACLE);
+    }
+
+    // get the cell coord of the second point
+    if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
+      return static_cast<double>(LETHAL_OBSTACLE);
+    }
+
+    footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
+  }
+
+  // we also need to connect the first point in the footprint to the last point
+  // get the cell coord of the last point
+  if (!worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
+    return static_cast<double>(LETHAL_OBSTACLE);
+  }
+
+  // get the cell coord of the first point
+  if (!worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
+    return static_cast<double>(LETHAL_OBSTACLE);
+  }
+
+  footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
+
+  // if all line costs are legal... then we can return that the footprint is legal
+  return footprint_cost;
+}
+
+double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const
+{
+  double line_cost = 0.0;
+  double point_cost = -1.0;
+
+  for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
+    point_cost = pointCost(line.getX(), line.getY());   // Score the current point
+
+    if (line_cost < point_cost) {
+      line_cost = point_cost;
+    }
+  }
+
+  return line_cost;
+}
+
+bool FootprintCollisionChecker::worldToMap(
+  double wx, double wy, unsigned int & mx, unsigned int & my)
+{
+  return costmap_->worldToMap(wx, wy, mx, my);
+}
+
+double FootprintCollisionChecker::pointCost(int x, int y) const
+{
+  return costmap_->getCost(x, y);
+}
+
+void FootprintCollisionChecker::setCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap)
+{
+  costmap_ = costmap;
+}
+
+double FootprintCollisionChecker::footprintCostAtPose(
+  double x, double y, double theta, const Footprint footprint)
+{
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  Footprint oriented_footprint;
+  for (unsigned int i = 0; i < footprint.size(); ++i) {
+    geometry_msgs::msg::Point new_pt;
+    new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th);
+    new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th);
+    oriented_footprint.push_back(new_pt);
+  }
+
+  return footprintCost(oriented_footprint);
+}
+
+}  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt
index 57271a9b515..4170ad3875b 100644
--- a/nav2_costmap_2d/test/integration/CMakeLists.txt
+++ b/nav2_costmap_2d/test/integration/CMakeLists.txt
@@ -10,7 +10,7 @@ target_link_libraries(footprint_tests_exec
 )
 
 ament_add_gtest_executable(test_collision_checker_exec
-  test_collision_checker.cpp
+  test_costmap_topic_collision_checker.cpp
 )
 ament_target_dependencies(test_collision_checker_exec
   ${dependencies}
diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
similarity index 97%
rename from nav2_costmap_2d/test/integration/test_collision_checker.cpp
rename to nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
index ae73b914f64..56baa03a4b4 100644
--- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp
+++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
@@ -18,7 +18,7 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "nav2_costmap_2d/collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_costmap_2d/static_layer.hpp"
@@ -123,7 +123,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
       shared_from_this(),
       footprint_topic);
 
-    collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
+    collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
       *costmap_sub_, *footprint_sub_, *tf_buffer_, get_name(), "map");
 
     layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false);
@@ -274,7 +274,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
 
   std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
   std::shared_ptr<DummyFootprintSubscriber> footprint_sub_;
-  std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+  std::unique_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
 
   nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
   std::string global_frame_;
diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt
index 5a952eaf280..78085141b38 100644
--- a/nav2_costmap_2d/test/unit/CMakeLists.txt
+++ b/nav2_costmap_2d/test/unit/CMakeLists.txt
@@ -2,3 +2,8 @@ ament_add_gtest(array_parser_test array_parser_test.cpp)
 target_link_libraries(array_parser_test
   nav2_costmap_2d_core
 )
+
+ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp)
+target_link_libraries(collision_footprint_test
+  nav2_costmap_2d_core
+)
diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp
new file mode 100644
index 00000000000..f0e9c306233
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp
@@ -0,0 +1,153 @@
+// Copyright (c) 2020 Shivang Patel
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+
+TEST(collision_footprint, test_basic)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -0.5;
+  p1.y = 0.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 0.0;
+  p2.y = 0.5;
+  geometry_msgs::msg::Point p3;
+  p3.x = 0.5;
+  p3.y = 0.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = 0.0;
+  p4.y = -0.5;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
+
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+}
+
+TEST(collision_footprint, test_point_cost)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
+
+  auto value = collision_checker.pointCost(50, 50);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+}
+
+TEST(collision_footprint, test_world_to_map)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
+
+  unsigned int x, y;
+
+  collision_checker.worldToMap(1.0, 1.0, x, y);
+
+  auto value = collision_checker.pointCost(x, y);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  costmap_->setCost(50, 50, 200);
+  collision_checker.worldToMap(5.0, 5.0, x, y);
+
+  EXPECT_NEAR(collision_checker.pointCost(x, y), 200.0, 0.001);
+}
+
+TEST(collision_footprint, test_footprint_at_pose_with_movement)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 254);
+
+  for (unsigned int i = 40; i <= 60; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmap_->setCost(i, j, 0);
+    }
+  }
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;
+  p2.y = 1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;
+  p3.y = -1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;
+  p4.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
+
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  auto up_value = collision_checker.footprintCostAtPose(5.0, 4.9, 0.0, footprint);
+  EXPECT_NEAR(up_value, 254.0, 0.001);
+
+  auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint);
+  EXPECT_NEAR(down_value, 254.0, 0.001);
+}
+
+TEST(collision_footprint, test_point_and_line_cost)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.10000, 0, 0.0, 0.0);
+
+  costmap_->setCost(62, 50, 254);
+  costmap_->setCost(39, 60, 254);
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;
+  p2.y = 1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;
+  p3.y = -1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;
+  p4.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
+
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint);
+  EXPECT_NEAR(left_value, 254.0, 0.001);
+
+  auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint);
+  EXPECT_NEAR(right_value, 254.0, 0.001);
+}
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
index 80606e90ff1..002ebb28bfd 100644
--- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp
+++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
@@ -88,7 +88,7 @@ class Recovery : public nav2_core::Recovery
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
     const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
-    std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker) override
+    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker) override
   {
     RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str());
 
@@ -135,7 +135,7 @@ class Recovery : public nav2_core::Recovery
   std::string recovery_name_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
   std::shared_ptr<ActionServer> action_server_;
-  std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
 
   double cycle_frequency_;
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp
index 026c3ecab4f..0eff75fb77f 100644
--- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp
+++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp
@@ -59,7 +59,7 @@ class RecoveryServer : public nav2_util::LifecycleNode
   // Utilities
   std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
   std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
-  std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
 };
 
 }  // namespace recovery_server
diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp
index ecd23d4481f..58b2b240dec 100644
--- a/nav2_recoveries/src/recovery_server.cpp
+++ b/nav2_recoveries/src/recovery_server.cpp
@@ -71,7 +71,7 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     shared_from_this(), costmap_topic);
   footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
     shared_from_this(), footprint_topic, 1.0);
-  collision_checker_ = std::make_shared<nav2_costmap_2d::CollisionChecker>(
+  collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
     *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom");
 
   this->get_parameter("plugin_names", plugin_names_);
diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp
index f1600094a9b..137e3500e41 100644
--- a/nav2_recoveries/test/test_recoveries.cpp
+++ b/nav2_recoveries/test/test_recoveries.cpp
@@ -126,8 +126,8 @@ class RecoveryTest : public ::testing::Test
     std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_ =
       std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
       node_lifecycle_, footprint_topic, 1.0);
-    std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_ =
-      std::make_shared<nav2_costmap_2d::CollisionChecker>(
+    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");