diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp
index f0e9c306233117228d929aaa890a3ecf287cea33..f133a7eda693effedc306a4363e5ca3bc62dc5f4 100644
--- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp
+++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp
@@ -18,6 +18,7 @@
 
 #include "gtest/gtest.h"
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
 
 TEST(collision_footprint, test_basic)
 {
@@ -151,3 +152,96 @@ TEST(collision_footprint, test_point_and_line_cost)
   auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint);
   EXPECT_NEAR(right_value, 254.0, 0.001);
 }
+
+TEST(collision_footprint, not_enough_points)
+{
+  geometry_msgs::msg::Point p1;
+  p1.x = 2.0;
+  p1.y = 2.0;
+
+  geometry_msgs::msg::Point p2;
+  p2.x = -2.0;
+  p2.y = -2.0;
+
+  std::vector<geometry_msgs::msg::Point> footprint = {p1, p2};
+  double min_dist = 0.0;
+  double max_dist = 0.0;
+
+  nav2_costmap_2d::calculateMinAndMaxDistances(footprint, min_dist, max_dist);
+  EXPECT_EQ(min_dist, std::numeric_limits<double>::max());
+  EXPECT_EQ(max_dist, 0.0f);
+}
+
+TEST(collision_footprint, to_point_32) {
+  geometry_msgs::msg::Point p;
+  p.x = 123.0;
+  p.y = 456.0;
+  p.z = 789.0;
+
+  geometry_msgs::msg::Point32 p32;
+  p32 = nav2_costmap_2d::toPoint32(p);
+  EXPECT_NEAR(p.x, p32.x, 1e-5);
+  EXPECT_NEAR(p.y, p32.y, 1e-5);
+  EXPECT_NEAR(p.z, p32.z, 1e-5);
+}
+
+TEST(collision_footprint, to_polygon) {
+  geometry_msgs::msg::Point p1;
+  p1.x = 1.2;
+  p1.y = 3.4;
+  p1.z = 5.1;
+
+  geometry_msgs::msg::Point p2;
+  p2.x = -5.6;
+  p2.y = -7.8;
+  p2.z = -9.1;
+  std::vector<geometry_msgs::msg::Point> pts = {p1, p2};
+
+  geometry_msgs::msg::Polygon poly;
+  poly = nav2_costmap_2d::toPolygon(pts);
+
+  EXPECT_EQ(2u, sizeof(poly.points) / sizeof(poly.points[0]));
+  EXPECT_NEAR(poly.points[0].x, p1.x, 1e-5);
+  EXPECT_NEAR(poly.points[0].y, p1.y, 1e-5);
+  EXPECT_NEAR(poly.points[0].z, p1.z, 1e-5);
+  EXPECT_NEAR(poly.points[1].x, p2.x, 1e-5);
+  EXPECT_NEAR(poly.points[1].y, p2.y, 1e-5);
+  EXPECT_NEAR(poly.points[1].z, p2.z, 1e-5);
+}
+
+TEST(collision_footprint, make_footprint_from_string) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint);
+  EXPECT_EQ(result, true);
+  EXPECT_EQ(4u, footprint.size());
+  EXPECT_NEAR(footprint[0].x, 1.0, 1e-5);
+  EXPECT_NEAR(footprint[0].y, 2.2, 1e-5);
+  EXPECT_NEAR(footprint[1].x, 0.3, 1e-5);
+  EXPECT_NEAR(footprint[1].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[2].x, -0.3, 1e-5);
+  EXPECT_NEAR(footprint[2].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[3].x, -1.0, 1e-5);
+  EXPECT_NEAR(footprint[3].y, 2.2, 1e-5);
+}
+
+TEST(collision_footprint, make_footprint_from_string_parse_error) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[bad_string", footprint);
+  EXPECT_EQ(result, false);
+}
+
+TEST(collision_footprint, make_footprint_from_string_two_points_error) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4]", footprint);
+  EXPECT_EQ(result, false);
+}
+
+TEST(collision_footprint, make_footprint_from_string_not_pairs) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint);
+  EXPECT_EQ(result, false);
+}