diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp
index b8aa748c3b8bd..953a741da8137 100644
--- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp
+++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp
@@ -19,12 +19,16 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
 #include <tf2/utils.h>
+
+#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
 
 #include <vector>
 
-// TODO(wep21): Remove these apis
-//              after they are implemented in ros2 geometry2.
+#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
 namespace tf2
 {
 inline void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
@@ -48,6 +52,7 @@ inline void doTransform(
   toMsg(v_out, t_out);
 }
 }  // namespace tf2
+#endif
 
 namespace freespace_planning_algorithms
 {
diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp
index 858c383cc4cc8..99d89a1f86153 100644
--- a/planning/freespace_planning_algorithms/src/astar_search.cpp
+++ b/planning/freespace_planning_algorithms/src/astar_search.cpp
@@ -17,7 +17,12 @@
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <tf2/utils.h>
+
+#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
 
 #include <vector>