diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt
index 267b81513f4..aa6cf1a4ed7 100644
--- a/nav2_amcl/CMakeLists.txt
+++ b/nav2_amcl/CMakeLists.txt
@@ -64,12 +64,12 @@ add_library(motions_lib SHARED
 target_include_directories(motions_lib
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(motions_lib PUBLIC
   pf_lib
   pluginlib::pluginlib
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
 )
 
 add_library(sensors_lib SHARED
@@ -81,11 +81,11 @@ add_library(sensors_lib SHARED
 target_include_directories(sensors_lib
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(sensors_lib PUBLIC
   pf_lib
   map_lib
+  nav2_ros_common::nav2_ros_common
 )
 
 set(executable_name amcl)
@@ -101,12 +101,12 @@ endif()
 target_include_directories(${library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   message_filters::message_filters
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   ${sensor_msgs_TARGETS}
   ${std_srvs_TARGETS}
   pluginlib::pluginlib
@@ -129,10 +129,10 @@ add_executable(${executable_name}
 target_include_directories(${executable_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${executable_name} PRIVATE
   ${library_name}
+  nav2_ros_common::nav2_ros_common
 )
 
 rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode")
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 7b6d475d2f8..15c9c620fcb 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -29,8 +29,7 @@ add_library(${library_name} SHARED
 target_include_directories(${library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${library_name} PUBLIC
   ${action_msgs_TARGETS}
   behaviortree_cpp::behaviortree_cpp
@@ -38,6 +37,7 @@ target_link_libraries(${library_name} PUBLIC
   ${nav_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
   rclcpp_lifecycle::rclcpp_lifecycle
@@ -249,8 +249,7 @@ foreach(bt_plugin ${plugin_libs})
   target_include_directories(${bt_plugin}
     PRIVATE
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
   target_link_libraries(${bt_plugin} PUBLIC
     behaviortree_cpp::behaviortree_cpp
     ${geometry_msgs_TARGETS}
@@ -285,9 +284,10 @@ add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp
 target_link_libraries(generate_nav2_tree_nodes_xml PRIVATE
   behaviortree_cpp::behaviortree_cpp
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
 )
 # allow generate_nav2_tree_nodes_xml to find plugins_list.hpp
-target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR} "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR})
 install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY include/
diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp
index 39e12e57650..99fe1fdc736 100644
--- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp
@@ -25,6 +25,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/LinearMath/Matrix3x3.hpp"
 #include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 #include "behaviortree_cpp/bt_factory.h"
 
diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp
index 97d038d2aa8..c7f660a9ddf 100644
--- a/nav2_behavior_tree/test/test_bt_utils.cpp
+++ b/nav2_behavior_tree/test/test_bt_utils.cpp
@@ -58,7 +58,7 @@ TEST(PointPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>("PointPort");
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -68,7 +68,7 @@ TEST(PointPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(PointPortTest, test_correct_syntax)
@@ -105,7 +105,7 @@ TEST(QuaternionPortTest, test_wrong_syntax)
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>("QuaternionPort");
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -115,7 +115,7 @@ TEST(QuaternionPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(QuaternionPortTest, test_correct_syntax)
@@ -152,7 +152,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>("PoseStampedPort");
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -162,7 +162,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(PoseStampedPortTest, test_correct_syntax)
@@ -206,7 +206,7 @@ TEST(PoseStampedVectorPortTest, test_wrong_syntax)
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<std::vector<geometry_msgs::msg::PoseStamped>>>(
     "PoseStampedVectorPortTest");
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -216,7 +216,7 @@ TEST(PoseStampedVectorPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(PoseStampedVectorPortTest, test_correct_syntax)
@@ -270,7 +270,7 @@ TEST(GoalsArrayPortTest, test_wrong_syntax)
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<nav_msgs::msg::Goals>>(
     "GoalsArrayPortTest");
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -280,7 +280,7 @@ TEST(GoalsArrayPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(GoalsArrayPortTest, test_correct_syntax)
@@ -334,7 +334,7 @@ TEST(PathPortTest, test_wrong_syntax)
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<nav_msgs::msg::Path>>(
     "PathPortTest");
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -344,7 +344,7 @@ TEST(PathPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(PathPortTest, test_correct_syntax)
@@ -399,7 +399,7 @@ TEST(WaypointStatusPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<nav2_msgs::msg::WaypointStatus>>("WaypointStatusPort");
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -409,7 +409,7 @@ TEST(WaypointStatusPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(WaypointStatusPortTest, test_correct_syntax)
@@ -455,7 +455,7 @@ TEST(WaypointStatusVectorPortTest, test_wrong_syntax) {
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<std::vector<nav2_msgs::msg::WaypointStatus>>>(
     "WaypointStatusVectorPort");
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 
   xml_txt =
     R"(
@@ -465,7 +465,7 @@ TEST(WaypointStatusVectorPortTest, test_wrong_syntax) {
         </BehaviorTree>
       </root>)";
 
-  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+  EXPECT_THROW({auto unused = factory.createTreeFromText(xml_txt);}, std::exception);
 }
 
 TEST(WaypointStatusVectorPortTest, test_correct_syntax)
diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt
index 2e67a685f3a..c67f9af628b 100644
--- a/nav2_behaviors/CMakeLists.txt
+++ b/nav2_behaviors/CMakeLists.txt
@@ -31,7 +31,6 @@ target_include_directories(nav2_spin_behavior
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_spin_behavior PUBLIC
   ${geometry_msgs_TARGETS}
@@ -40,6 +39,7 @@ target_link_libraries(nav2_spin_behavior PUBLIC
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2::tf2
@@ -57,7 +57,6 @@ target_include_directories(nav2_wait_behavior
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_wait_behavior PUBLIC
   ${geometry_msgs_TARGETS}
@@ -65,6 +64,7 @@ target_link_libraries(nav2_wait_behavior PUBLIC
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2::tf2
@@ -78,7 +78,6 @@ target_include_directories(nav2_drive_on_heading_behavior
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_drive_on_heading_behavior PUBLIC
   ${geometry_msgs_TARGETS}
@@ -86,6 +85,7 @@ target_link_libraries(nav2_drive_on_heading_behavior PUBLIC
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2::tf2
@@ -102,7 +102,6 @@ target_include_directories(nav2_back_up_behavior
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_back_up_behavior PUBLIC
   ${geometry_msgs_TARGETS}
@@ -110,6 +109,7 @@ target_link_libraries(nav2_back_up_behavior PUBLIC
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2::tf2
@@ -126,7 +126,6 @@ target_include_directories(nav2_assisted_teleop_behavior
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_assisted_teleop_behavior PUBLIC
   ${geometry_msgs_TARGETS}
@@ -134,6 +133,7 @@ target_link_libraries(nav2_assisted_teleop_behavior PUBLIC
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${std_msgs_TARGETS}
@@ -154,13 +154,13 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   nav2_core::nav2_core
   nav2_costmap_2d::nav2_costmap_2d_client
   nav2_costmap_2d::nav2_costmap_2d_core
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   pluginlib::pluginlib
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2_ros::tf2_ros
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp
index 057d09c9bfe..a9e30ce2e0b 100644
--- a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp
@@ -78,8 +78,8 @@ class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
    * @param twist velocity to project pose by
    * @param projection_time time to project by
    */
-  geometry_msgs::msg::Pose2D projectPose(
-    const geometry_msgs::msg::Pose2D & pose,
+  geometry_msgs::msg::Pose projectPose(
+    const geometry_msgs::msg::Pose & pose,
     const geometry_msgs::msg::Twist & twist,
     double projection_time);
 
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
index b903c965784..936221d1d2b 100644
--- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
@@ -163,10 +163,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       cmd_vel->twist.linear.x = forward ? minimum_speed_ : -minimum_speed_;
     }
 
-    geometry_msgs::msg::Pose2D pose2d;
-    pose2d.x = current_pose.pose.position.x;
-    pose2d.y = current_pose.pose.position.y;
-    pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
+    geometry_msgs::msg::Pose pose2d = current_pose.pose;
 
     if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) {
       this->stopRobot();
@@ -200,13 +197,13 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    * @brief Check if pose is collision free
    * @param distance Distance to check forward
    * @param cmd_vel current commanded velocity
-   * @param pose2d Current pose
+   * @param pose Current pose
    * @return is collision free or not
    */
   bool isCollisionFree(
     const double & distance,
     const geometry_msgs::msg::Twist & cmd_vel,
-    geometry_msgs::msg::Pose2D & pose2d)
+    geometry_msgs::msg::Pose & pose)
   {
     if (command_disable_collision_checks_) {
       return true;
@@ -217,20 +214,21 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     double sim_position_change;
     const double diff_dist = abs(command_x_) - distance;
     const int max_cycle_count = static_cast<int>(this->cycle_frequency_ * simulate_ahead_time_);
-    geometry_msgs::msg::Pose2D init_pose = pose2d;
+    geometry_msgs::msg::Pose init_pose = pose;
+    double init_theta = tf2::getYaw(init_pose.orientation);
     bool fetch_data = true;
 
     while (cycle_count < max_cycle_count) {
       sim_position_change = cmd_vel.linear.x * (cycle_count / this->cycle_frequency_);
-      pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta);
-      pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta);
+      pose.position.x = init_pose.position.x + sim_position_change * cos(init_theta);
+      pose.position.y = init_pose.position.y + sim_position_change * sin(init_theta);
       cycle_count++;
 
       if (diff_dist - abs(sim_position_change) <= 0.) {
         break;
       }
 
-      if (!this->local_collision_checker_->isCollisionFree(pose2d, fetch_data)) {
+      if (!this->local_collision_checker_->isCollisionFree(pose, fetch_data)) {
         return false;
       }
       fetch_data = false;
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
index 7ef1e05489f..a334f4cbb73 100644
--- a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
@@ -81,7 +81,7 @@ class Spin : public TimedBehavior<SpinAction>
   bool isCollisionFree(
     const double & distance,
     const geometry_msgs::msg::Twist & cmd_vel,
-    geometry_msgs::msg::Pose2D & pose2d);
+    geometry_msgs::msg::Pose & pose);
 
   SpinAction::Feedback::SharedPtr feedback_;
 
diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp
index 95dbb3fd145..255f891e552 100644
--- a/nav2_behaviors/plugins/assisted_teleop.cpp
+++ b/nav2_behaviors/plugins/assisted_teleop.cpp
@@ -16,6 +16,7 @@
 
 #include "nav2_behaviors/plugins/assisted_teleop.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_behaviors
 {
@@ -110,10 +111,7 @@ ResultStatus AssistedTeleop::onCycleUpdate()
     return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TF_ERROR, error_msg};
   }
 
-  geometry_msgs::msg::Pose2D projected_pose;
-  projected_pose.x = current_pose.pose.position.x;
-  projected_pose.y = current_pose.pose.position.y;
-  projected_pose.theta = tf2::getYaw(current_pose.pose.orientation);
+  geometry_msgs::msg::Pose projected_pose = current_pose.pose;
 
   auto scaled_twist = std::make_unique<geometry_msgs::msg::TwistStamped>(teleop_twist_);
   for (double time = simulation_time_step_; time < projection_time_;
@@ -151,22 +149,25 @@ ResultStatus AssistedTeleop::onCycleUpdate()
   return ResultStatus{Status::RUNNING, AssistedTeleopActionResult::NONE, ""};
 }
 
-geometry_msgs::msg::Pose2D AssistedTeleop::projectPose(
-  const geometry_msgs::msg::Pose2D & pose,
+geometry_msgs::msg::Pose AssistedTeleop::projectPose(
+  const geometry_msgs::msg::Pose & pose,
   const geometry_msgs::msg::Twist & twist,
   double projection_time)
 {
-  geometry_msgs::msg::Pose2D projected_pose = pose;
+  geometry_msgs::msg::Pose projected_pose = pose;
 
-  projected_pose.x += projection_time * (
-    twist.linear.x * cos(pose.theta) +
-    twist.linear.y * sin(pose.theta));
+  double theta = tf2::getYaw(pose.orientation);
 
-  projected_pose.y += projection_time * (
-    twist.linear.x * sin(pose.theta) -
-    twist.linear.y * cos(pose.theta));
+  projected_pose.position.x += projection_time * (
+    twist.linear.x * cos(theta) +
+    twist.linear.y * sin(theta));
 
-  projected_pose.theta += projection_time * twist.angular.z;
+  projected_pose.position.y += projection_time * (
+    twist.linear.x * sin(theta) -
+    twist.linear.y * cos(theta));
+
+  double new_theta = theta + projection_time * twist.angular.z;
+  projected_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(new_theta);
 
   return projected_pose;
 }
diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp
index ece15c81359..bf400361b0c 100644
--- a/nav2_behaviors/plugins/spin.cpp
+++ b/nav2_behaviors/plugins/spin.cpp
@@ -19,9 +19,8 @@
 #include <utility>
 
 #include "nav2_behaviors/plugins/spin.hpp"
-#include "tf2/utils.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 using namespace std::chrono_literals;
 
@@ -145,12 +144,9 @@ ResultStatus Spin::onCycleUpdate()
   cmd_vel->header.stamp = clock_->now();
   cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_);
 
-  geometry_msgs::msg::Pose2D pose2d;
-  pose2d.x = current_pose.pose.position.x;
-  pose2d.y = current_pose.pose.position.y;
-  pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
+  geometry_msgs::msg::Pose pose = current_pose.pose;
 
-  if (!isCollisionFree(relative_yaw_, cmd_vel->twist, pose2d)) {
+  if (!isCollisionFree(relative_yaw_, cmd_vel->twist, pose)) {
     stopRobot();
     std::string error_msg = "Collision Ahead - Exiting Spin";
     RCLCPP_WARN(logger_, error_msg.c_str());
@@ -165,7 +161,7 @@ ResultStatus Spin::onCycleUpdate()
 bool Spin::isCollisionFree(
   const double & relative_yaw,
   const geometry_msgs::msg::Twist & cmd_vel,
-  geometry_msgs::msg::Pose2D & pose2d)
+  geometry_msgs::msg::Pose & pose)
 {
   if (cmd_disable_collision_checks_) {
     return true;
@@ -175,19 +171,21 @@ bool Spin::isCollisionFree(
   int cycle_count = 0;
   double sim_position_change;
   const int max_cycle_count = static_cast<int>(cycle_frequency_ * simulate_ahead_time_);
-  geometry_msgs::msg::Pose2D init_pose = pose2d;
+  geometry_msgs::msg::Pose init_pose = pose;
+  double init_theta = tf2::getYaw(init_pose.orientation);
   bool fetch_data = true;
 
   while (cycle_count < max_cycle_count) {
     sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_);
-    pose2d.theta = init_pose.theta + sim_position_change;
+    double new_theta = init_theta + sim_position_change;
+    pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(new_theta);
     cycle_count++;
 
-    if (abs(relative_yaw) - abs(sim_position_change) <= 0.) {
+    if (abs(relative_yaw) - abs(sim_position_change) <= 0.0) {
       break;
     }
 
-    if (!local_collision_checker_->isCollisionFree(pose2d, fetch_data)) {
+    if (!local_collision_checker_->isCollisionFree(pose, fetch_data)) {
       return false;
     }
     fetch_data = false;
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index c5cdd898d24..2fa895dd8b8 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/nav2_bt_navigator/CMakeLists.txt
@@ -31,11 +31,11 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   nav2_core::nav2_core
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
@@ -54,11 +54,11 @@ target_include_directories(${executable_name}
   PRIVATE
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${executable_name} PRIVATE
   ${library_name}
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
 )
 
 add_library(nav2_navigate_to_pose_navigator SHARED src/navigators/navigate_to_pose.cpp)
@@ -66,13 +66,13 @@ target_include_directories(nav2_navigate_to_pose_navigator
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_navigate_to_pose_navigator PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   ${nav_msgs_TARGETS}
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
@@ -89,7 +89,6 @@ target_include_directories(nav2_navigate_through_poses
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_navigate_through_poses PUBLIC
   ${geometry_msgs_TARGETS}
@@ -97,6 +96,7 @@ target_link_libraries(nav2_navigate_through_poses PUBLIC
   ${nav2_msgs_TARGETS}
   ${nav_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
 )
diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
index 21c3c1aabdb..a7c22e59a09 100644
--- a/nav2_collision_monitor/CMakeLists.txt
+++ b/nav2_collision_monitor/CMakeLists.txt
@@ -39,14 +39,14 @@ add_library(${monitor_library_name} SHARED
 target_include_directories(${monitor_library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${monitor_library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_costmap_2d::nav2_costmap_2d_client
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS}
@@ -73,14 +73,14 @@ add_library(${detector_library_name} SHARED
 target_include_directories(${detector_library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${detector_library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_costmap_2d::nav2_costmap_2d_client
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS}
@@ -98,10 +98,10 @@ add_executable(${monitor_executable_name}
 target_include_directories(${monitor_executable_name}
   PRIVATE
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${monitor_executable_name} PRIVATE
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
   ${monitor_library_name}
 )
 
@@ -111,10 +111,10 @@ add_executable(${detector_executable_name}
 target_include_directories(${detector_executable_name}
   PRIVATE
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${detector_executable_name} PRIVATE
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
   ${detector_library_name}
 )
 
diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt
index 92bcf17f79d..31e83e77fe5 100644
--- a/nav2_constrained_smoother/CMakeLists.txt
+++ b/nav2_constrained_smoother/CMakeLists.txt
@@ -30,7 +30,6 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   Ceres::ceres
@@ -39,6 +38,7 @@ target_link_libraries(${library_name} PUBLIC
   nav2_costmap_2d::nav2_costmap_2d_client
   nav2_costmap_2d::nav2_costmap_2d_core
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2_ros::tf2_ros
diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
index 5ed42e23c97..b9285fa9240 100644
--- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp
+++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
@@ -29,6 +29,8 @@
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
 #include "angles/angles.h"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 #include "nav2_constrained_smoother/constrained_smoother.hpp"
 
diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
index ecc147cc2d2..a100f96a214 100644
--- a/nav2_controller/CMakeLists.txt
+++ b/nav2_controller/CMakeLists.txt
@@ -33,7 +33,6 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
@@ -41,6 +40,7 @@ target_link_libraries(${library_name} PUBLIC
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   ${nav_2d_msgs_TARGETS}
   nav_2d_utils::conversions
   nav_2d_utils::tf_help
@@ -62,10 +62,10 @@ target_include_directories(${executable_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${executable_name} PRIVATE
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
   ${library_name}
 )
 
@@ -74,11 +74,11 @@ target_include_directories(simple_progress_checker
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(simple_progress_checker PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${rcl_interfaces_TARGETS}
@@ -94,13 +94,13 @@ target_include_directories(pose_progress_checker
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(pose_progress_checker PUBLIC
   ${geometry_msgs_TARGETS}
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${rcl_interfaces_TARGETS}
+  nav2_ros_common::nav2_ros_common
   simple_progress_checker
 )
 target_link_libraries(pose_progress_checker PRIVATE
@@ -115,11 +115,11 @@ target_include_directories(simple_goal_checker
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(simple_goal_checker PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
@@ -137,11 +137,11 @@ target_include_directories(stopped_goal_checker
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(stopped_goal_checker PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${rcl_interfaces_TARGETS}
@@ -157,11 +157,11 @@ target_include_directories(position_goal_checker
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(position_goal_checker PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp
index e91ce3306c9..4d790895f5e 100644
--- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp
+++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp
@@ -43,11 +43,11 @@ class PoseProgressChecker : public SimpleProgressChecker
    * @param pose Current pose of the robot
    * @return true, if movement is greater than radius_, or false
    */
-  bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
+  bool isRobotMovedEnough(const geometry_msgs::msg::Pose & pose);
 
   static double poseAngleDistance(
-    const geometry_msgs::msg::Pose2D &,
-    const geometry_msgs::msg::Pose2D &);
+    const geometry_msgs::msg::Pose &,
+    const geometry_msgs::msg::Pose &);
 
   double required_movement_angle_;
 
diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp
index fcff9c1215b..1b530bfa22b 100644
--- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp
+++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp
@@ -21,7 +21,7 @@
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_core/progress_checker.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 
 namespace nav2_controller
 {
@@ -46,23 +46,23 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker
    * @param pose Current pose of the robot
    * @return true, if movement is greater than radius_, or false
    */
-  bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
+  bool isRobotMovedEnough(const geometry_msgs::msg::Pose & pose);
   /**
    * @brief Resets baseline pose with the current pose of the robot
    * @param pose Current pose of the robot
    */
-  void resetBaselinePose(const geometry_msgs::msg::Pose2D & pose);
+  void resetBaselinePose(const geometry_msgs::msg::Pose & pose);
 
   static double pose_distance(
-    const geometry_msgs::msg::Pose2D &,
-    const geometry_msgs::msg::Pose2D &);
+    const geometry_msgs::msg::Pose &,
+    const geometry_msgs::msg::Pose &);
 
   rclcpp::Clock::SharedPtr clock_;
 
   double radius_;
   rclcpp::Duration time_allowance_{0, 0};
 
-  geometry_msgs::msg::Pose2D baseline_pose_;
+  geometry_msgs::msg::Pose baseline_pose_;
   rclcpp::Time baseline_time_;
 
   bool baseline_pose_set_{false};
diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp
index 60dce419476..453d20992e8 100644
--- a/nav2_controller/plugins/pose_progress_checker.cpp
+++ b/nav2_controller/plugins/pose_progress_checker.cpp
@@ -20,9 +20,11 @@
 #include "angles/angles.h"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "pluginlib/class_list_macros.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 using rcl_interfaces::msg::ParameterType;
 using std::placeholders::_1;
@@ -51,27 +53,26 @@ bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
 {
   // relies on short circuit evaluation to not call is_robot_moved_enough if
   // baseline_pose is not set.
-  geometry_msgs::msg::Pose2D current_pose2d;
-  current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
-
-  if (!baseline_pose_set_ || PoseProgressChecker::isRobotMovedEnough(current_pose2d)) {
-    resetBaselinePose(current_pose2d);
+  if (!baseline_pose_set_ || PoseProgressChecker::isRobotMovedEnough(current_pose.pose)) {
+    resetBaselinePose(current_pose.pose);
     return true;
   }
   return clock_->now() - baseline_time_ <= time_allowance_;
 }
 
-bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
+bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose & pose)
 {
   return pose_distance(pose, baseline_pose_) > radius_ ||
          poseAngleDistance(pose, baseline_pose_) > required_movement_angle_;
 }
 
 double PoseProgressChecker::poseAngleDistance(
-  const geometry_msgs::msg::Pose2D & pose1,
-  const geometry_msgs::msg::Pose2D & pose2)
+  const geometry_msgs::msg::Pose & pose1,
+  const geometry_msgs::msg::Pose & pose2)
 {
-  return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta));
+  double theta1 = tf2::getYaw(pose1.orientation);
+  double theta2 = tf2::getYaw(pose2.orientation);
+  return std::abs(angles::shortest_angular_distance(theta1, theta2));
 }
 
 rcl_interfaces::msg::SetParametersResult
diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp
index 6c00168b559..2e219cdeee8 100644
--- a/nav2_controller/plugins/simple_progress_checker.cpp
+++ b/nav2_controller/plugins/simple_progress_checker.cpp
@@ -19,7 +19,7 @@
 #include <vector>
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "pluginlib/class_list_macros.hpp"
 
@@ -56,11 +56,8 @@ bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose
 {
   // relies on short circuit evaluation to not call is_robot_moved_enough if
   // baseline_pose is not set.
-  geometry_msgs::msg::Pose2D current_pose2d;
-  current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
-
-  if ((!baseline_pose_set_) || (isRobotMovedEnough(current_pose2d))) {
-    resetBaselinePose(current_pose2d);
+  if ((!baseline_pose_set_) || (isRobotMovedEnough(current_pose.pose))) {
+    resetBaselinePose(current_pose.pose);
     return true;
   }
   return !((clock_->now() - baseline_time_) > time_allowance_);
@@ -71,24 +68,24 @@ void SimpleProgressChecker::reset()
   baseline_pose_set_ = false;
 }
 
-void SimpleProgressChecker::resetBaselinePose(const geometry_msgs::msg::Pose2D & pose)
+void SimpleProgressChecker::resetBaselinePose(const geometry_msgs::msg::Pose & pose)
 {
   baseline_pose_ = pose;
   baseline_time_ = clock_->now();
   baseline_pose_set_ = true;
 }
 
-bool SimpleProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
+bool SimpleProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose & pose)
 {
   return pose_distance(pose, baseline_pose_) > radius_;
 }
 
 double SimpleProgressChecker::pose_distance(
-  const geometry_msgs::msg::Pose2D & pose1,
-  const geometry_msgs::msg::Pose2D & pose2)
+  const geometry_msgs::msg::Pose & pose1,
+  const geometry_msgs::msg::Pose & pose2)
 {
-  double dx = pose1.x - pose2.x;
-  double dy = pose1.y - pose2.y;
+  double dx = pose1.position.x - pose2.position.x;
+  double dy = pose1.position.y - pose2.position.y;
 
   return std::hypot(dx, dy);
 }
diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp
index 4e819fe9946..4730aeb3a04 100644
--- a/nav2_controller/plugins/test/goal_checker.cpp
+++ b/nav2_controller/plugins/test/goal_checker.cpp
@@ -39,6 +39,7 @@
 #include "nav2_controller/plugins/simple_goal_checker.hpp"
 #include "nav2_controller/plugins/stopped_goal_checker.hpp"
 #include "nav_2d_utils/conversions.hpp"
+#include "nav2_util/geometry_utils.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "eigen3/Eigen/Geometry"
 
@@ -53,27 +54,27 @@ void checkMacro(
   bool expected_result)
 {
   gc.reset();
-  geometry_msgs::msg::Pose2D pose0, pose1;
-  pose0.x = x0;
-  pose0.y = y0;
-  pose0.theta = theta0;
-  pose1.x = x1;
-  pose1.y = y1;
-  pose1.theta = theta1;
-  nav_2d_msgs::msg::Twist2D v;
-  v.x = xv;
-  v.y = yv;
-  v.theta = thetav;
+
+  geometry_msgs::msg::Pose pose0, pose1;
+  pose0.position.x = x0;
+  pose0.position.y = y0;
+  pose0.position.z = 0.0;
+  pose0.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0);
+
+  pose1.position.x = x1;
+  pose1.position.y = y1;
+  pose1.position.z = 0.0;
+  pose1.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1);
+
+  geometry_msgs::msg::Twist v;
+  v.linear.x = xv;
+  v.linear.y = yv;
+  v.angular.z = thetav;
+
   if (expected_result) {
-    EXPECT_TRUE(
-      gc.isGoalReached(
-        nav_2d_utils::pose2DToPose(pose0),
-        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
+    EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v));
   } else {
-    EXPECT_FALSE(
-      gc.isGoalReached(
-        nav_2d_utils::pose2DToPose(pose0),
-        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
+    EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v));
   }
 }
 
diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp
index c95d601e11d..2ac3698ca58 100644
--- a/nav2_core/include/nav2_core/progress_checker.hpp
+++ b/nav2_core/include/nav2_core/progress_checker.hpp
@@ -21,7 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 
 namespace nav2_core
 {
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index ee9b7dabaa1..7a67433d6bb 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -46,13 +46,13 @@ target_include_directories(nav2_costmap_2d_core
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_costmap_2d_core PUBLIC
   ${geometry_msgs_TARGETS}
   ${map_msgs_TARGETS}
   ${nav_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
+  nav2_ros_common::nav2_ros_common
   nav2_util::nav2_util_core
   pluginlib::pluginlib
   ${std_srvs_TARGETS}
@@ -75,7 +75,6 @@ target_include_directories(layers
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(layers PUBLIC
   angles::angles
@@ -84,6 +83,7 @@ target_link_libraries(layers PUBLIC
   laser_geometry::laser_geometry
   nav2_voxel_grid::voxel_grid
   nav2_costmap_2d_core
+  nav2_ros_common::nav2_ros_common
   tf2::tf2
 )
 target_link_libraries(layers PRIVATE
@@ -99,12 +99,12 @@ target_include_directories(filters
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(filters PUBLIC
   nav2_costmap_2d_core
   ${std_msgs_TARGETS}
   tf2::tf2
+  nav2_ros_common::nav2_ros_common
 )
 
 add_library(nav2_costmap_2d_client SHARED
@@ -116,12 +116,12 @@ target_include_directories(nav2_costmap_2d_client
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(nav2_costmap_2d_client PUBLIC
   nav2_costmap_2d_core
   ${std_msgs_TARGETS}
   tf2::tf2
+  nav2_ros_common::nav2_ros_common
 )
 
 add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp)
@@ -137,7 +137,6 @@ target_include_directories(nav2_costmap_2d_markers
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp)
@@ -154,7 +153,6 @@ target_include_directories(nav2_costmap_2d_cloud
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 add_executable(nav2_costmap_2d src/costmap_2d_node.cpp)
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp
index 3cce5a112fc..e206725282c 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp
@@ -74,7 +74,7 @@ class BinaryFilter : public CostmapFilter
   void process(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j,
-    const geometry_msgs::msg::Pose2D & pose);
+    const geometry_msgs::msg::Pose & pose);
 
   /**
    * @brief Reset the costmap filter / topic / info
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index f55f3092276..e51809df696 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
@@ -45,7 +45,7 @@
 #include <mutex>
 #include <memory>
 
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "std_srvs/srv/set_bool.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
@@ -152,7 +152,7 @@ class CostmapFilter : public Layer
   virtual void process(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j,
-    const geometry_msgs::msg::Pose2D & pose) = 0;
+    const geometry_msgs::msg::Pose & pose) = 0;
 
   /**
    * @brief: Resets costmap filter. Stops all subscriptions
@@ -181,9 +181,9 @@ class CostmapFilter : public Layer
    */
   bool transformPose(
     const std::string global_frame,
-    const geometry_msgs::msg::Pose2D & global_pose,
+    const geometry_msgs::msg::Pose & global_pose,
     const std::string mask_frame,
-    geometry_msgs::msg::Pose2D & mask_pose) const;
+    geometry_msgs::msg::Pose & mask_pose) const;
 
   /**
    * @brief: Convert from world coordinates to mask coordinates.
@@ -248,7 +248,7 @@ class CostmapFilter : public Layer
   /**
    * @brief: Latest robot position
    */
-  geometry_msgs::msg::Pose2D latest_pose_;
+  geometry_msgs::msg::Pose latest_pose_;
 
   /**
    * @brief: Mutex for locking filter's resources
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
index c29c4c73bde..e615ba465a0 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
@@ -74,7 +74,7 @@ class KeepoutFilter : public CostmapFilter
   void process(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j,
-    const geometry_msgs::msg::Pose2D & pose);
+    const geometry_msgs::msg::Pose & pose);
 
   /**
    * @brief Reset the costmap filter / topic / info
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp
index 9f36728c0e5..a970b5f341b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp
@@ -74,7 +74,7 @@ class SpeedFilter : public CostmapFilter
   void process(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j,
-    const geometry_msgs::msg::Pose2D & pose);
+    const geometry_msgs::msg::Pose & pose);
 
   /**
    * @brief Reset the costmap filter / topic / info
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 a761415c5e4..18806fdf444 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
@@ -24,7 +24,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
@@ -70,7 +70,7 @@ class CostmapTopicCollisionChecker
    * 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,
+    const geometry_msgs::msg::Pose & pose,
     bool fetch_costmap_and_footprint = true);
 
   /**
@@ -81,7 +81,7 @@ class CostmapTopicCollisionChecker
    * 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,
+    const geometry_msgs::msg::Pose & pose,
     bool fetch_costmap_and_footprint = true);
 
 protected:
@@ -93,7 +93,7 @@ class CostmapTopicCollisionChecker
    * 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,
+    const geometry_msgs::msg::Pose & pose,
     bool fetch_latest_footprint = true);
 
   // Name used for logging
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
index 3a2f6a77d25..9d6b0935083 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
@@ -416,7 +416,8 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase
     }
 
     // Label 0 is reserved for the background pixels, i.e. labels[0] is always 0
-    labels_ = {0};
+    labels_.clear();
+    labels_.resize(1, 0);
     next_free_ = 1;
   }
 
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
index c4d4418c748..c7cf2b54c6b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp
@@ -24,7 +24,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_util/robot_utils.hpp"
 
diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp
index 68a7c7adea4..9c8bd890afb 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp
@@ -169,7 +169,7 @@ void BinaryFilter::maskCallback(
 void BinaryFilter::process(
   nav2_costmap_2d::Costmap2D & /*master_grid*/,
   int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/,
-  const geometry_msgs::msg::Pose2D & pose)
+  const geometry_msgs::msg::Pose & pose)
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
@@ -181,7 +181,7 @@ void BinaryFilter::process(
     return;
   }
 
-  geometry_msgs::msg::Pose2D mask_pose;  // robot coordinates in mask frame
+  geometry_msgs::msg::Pose mask_pose;  // robot coordinates in mask frame
 
   // Transforming robot pose from current layer frame to mask frame
   if (!transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
@@ -190,7 +190,9 @@ void BinaryFilter::process(
 
   // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
   unsigned int mask_robot_i, mask_robot_j;
-  if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
+  if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
+    mask_robot_j))
+  {
     // Robot went out of mask range. Set "false" state by-default
     RCLCPP_WARN(
       logger_,
@@ -210,6 +212,7 @@ void BinaryFilter::process(
       mask_robot_i, mask_robot_j);
     return;
   }
+
   // Check and flip binary state, if necessary
   if (base_ + mask_data * multiplier_ > flip_threshold_) {
     if (binary_state_ == default_state_) {
diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
index 1a089b69122..bc4e89f878e 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -42,7 +42,7 @@
 
 #include <exception>
 
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/geometry_utils.hpp"
 #include "geometry_msgs/msg/point_stamped.hpp"
 
 #include "nav2_costmap_2d/cost_values.hpp"
@@ -118,9 +118,10 @@ void CostmapFilter::updateBounds(
     return;
   }
 
-  latest_pose_.x = robot_x;
-  latest_pose_.y = robot_y;
-  latest_pose_.theta = robot_yaw;
+  latest_pose_.position.x = robot_x;
+  latest_pose_.position.y = robot_y;
+  latest_pose_.position.z = 0.0;
+  latest_pose_.orientation = nav2_util::geometry_utils::orientationAroundZAxis(robot_yaw);
 }
 
 void CostmapFilter::updateCosts(
@@ -151,21 +152,21 @@ void CostmapFilter::enableCallback(
 
 bool CostmapFilter::transformPose(
   const std::string global_frame,
-  const geometry_msgs::msg::Pose2D & global_pose,
+  const geometry_msgs::msg::Pose & global_pose,
   const std::string mask_frame,
-  geometry_msgs::msg::Pose2D & mask_pose) const
+  geometry_msgs::msg::Pose & mask_pose) const
 {
   if (mask_frame != global_frame) {
     // Filter mask and current layer are in different frames:
-    // Transform (global_pose.x, global_pose.y) point from current layer frame (global_frame)
-    // to mask_pose point in mask_frame
+    // Transform (global_pose.position.x, global_pose.position.y) point from current layer frame
+    // to mask_pose in mask_frame
     geometry_msgs::msg::TransformStamped transform;
     geometry_msgs::msg::PointStamped in, out;
     in.header.stamp = clock_->now();
     in.header.frame_id = global_frame;
-    in.point.x = global_pose.x;
-    in.point.y = global_pose.y;
-    in.point.z = 0;
+    in.point.x = global_pose.position.x;
+    in.point.y = global_pose.position.y;
+    in.point.z = 0.0;
 
     try {
       tf_->transform(in, out, mask_frame, transform_tolerance_);
@@ -177,11 +178,15 @@ bool CostmapFilter::transformPose(
         global_frame.c_str(), mask_frame.c_str(), ex.what());
       return false;
     }
-    mask_pose.x = out.point.x;
-    mask_pose.y = out.point.y;
+
+    mask_pose = global_pose;
+    mask_pose.position.x = out.point.x;
+    mask_pose.position.y = out.point.y;
+    // mask_pose.position.z is kept as-is (0 or original)
+    // orientation is preserved
   } else {
     // Filter mask and current layer are in the same frame:
-    // Just use global_pose coordinates
+    // Just use global_pose
     mask_pose = global_pose;
   }
 
diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
index af4c2fbb54c..e727d00634e 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
@@ -160,7 +160,7 @@ void KeepoutFilter::maskCallback(
 void KeepoutFilter::process(
   nav2_costmap_2d::Costmap2D & master_grid,
   int min_i, int min_j, int max_i, int max_j,
-  const geometry_msgs::msg::Pose2D & pose)
+  const geometry_msgs::msg::Pose & pose)
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
@@ -174,8 +174,8 @@ void KeepoutFilter::process(
 
   tf2::Transform tf2_transform;
   tf2_transform.setIdentity();  // initialize by identical transform
-  int mg_min_x, mg_min_y;  // masger_grid indexes of bottom-left window corner
-  int mg_max_x, mg_max_y;  // masger_grid indexes of top-right window corner
+  int mg_min_x, mg_min_y;  // master_grid indexes of bottom-left window corner
+  int mg_max_x, mg_max_y;  // master_grid indexes of top-right window corner
 
   const std::string mask_frame = filter_mask_->header.frame_id;
 
@@ -239,7 +239,7 @@ void KeepoutFilter::process(
     mg_min_y = std::max(min_j, mg_min_y);
 
     // Calculating bounds corresponding to top-right window (2) corner
-    // filter_mask_ -> master_grid intexes conversion
+    // filter_mask_ -> master_grid indexes conversion
     wx = filter_mask_->info.origin.position.x +
       filter_mask_->info.width * filter_mask_->info.resolution + half_cell_size;
     wy = filter_mask_->info.origin.position.y +
@@ -263,10 +263,12 @@ void KeepoutFilter::process(
   // Let's find the pose's cost if we are allowed to override the lethal cost
   bool is_pose_lethal = false;
   if (override_lethal_cost_) {
-    geometry_msgs::msg::Pose2D mask_pose;
+    geometry_msgs::msg::Pose mask_pose;
     if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
       unsigned int mask_robot_i, mask_robot_j;
-      if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
+      if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
+        mask_robot_j))
+      {
         auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
         is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
         if (is_pose_lethal) {
diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
index 7a0d062ee19..a66886bc6d0 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
@@ -177,7 +177,7 @@ void SpeedFilter::maskCallback(
 void SpeedFilter::process(
   nav2_costmap_2d::Costmap2D & /*master_grid*/,
   int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/,
-  const geometry_msgs::msg::Pose2D & pose)
+  const geometry_msgs::msg::Pose & pose)
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
@@ -189,7 +189,7 @@ void SpeedFilter::process(
     return;
   }
 
-  geometry_msgs::msg::Pose2D mask_pose;  // robot coordinates in mask frame
+  geometry_msgs::msg::Pose mask_pose;  // robot coordinates in mask frame
 
   // Transforming robot pose from current layer frame to mask frame
   if (!transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
@@ -198,7 +198,9 @@ void SpeedFilter::process(
 
   // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
   unsigned int mask_robot_i, mask_robot_j;
-  if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
+  if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
+    mask_robot_j))
+  {
     return;
   }
 
diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
index bd70e70442f..f701991c6ed 100644
--- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
+++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
@@ -20,6 +20,8 @@
 #include <algorithm>
 #include <iostream>
 
+#include "tf2/utils.hpp"
+
 #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 
 #include "nav2_costmap_2d/cost_values.hpp"
@@ -56,7 +58,7 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
 }
 
 bool CostmapTopicCollisionChecker::isCollisionFree(
-  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Pose & pose,
   bool fetch_costmap_and_footprint)
 {
   try {
@@ -77,7 +79,7 @@ bool CostmapTopicCollisionChecker::isCollisionFree(
 }
 
 double CostmapTopicCollisionChecker::scorePose(
-  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Pose & pose,
   bool fetch_costmap_and_footprint)
 {
   if (fetch_costmap_and_footprint) {
@@ -89,7 +91,7 @@ double CostmapTopicCollisionChecker::scorePose(
   }
 
   unsigned int cell_x, cell_y;
-  if (!collision_checker_.worldToMap(pose.x, pose.y, cell_x, cell_y)) {
+  if (!collision_checker_.worldToMap(pose.position.x, pose.position.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.");
   }
@@ -98,7 +100,7 @@ double CostmapTopicCollisionChecker::scorePose(
 }
 
 Footprint CostmapTopicCollisionChecker::getFootprint(
-  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Pose & pose,
   bool fetch_latest_footprint)
 {
   if (fetch_latest_footprint) {
@@ -110,8 +112,12 @@ Footprint CostmapTopicCollisionChecker::getFootprint(
       throw CollisionCheckerException("Current footprint not available.");
     }
   }
+
   Footprint footprint;
-  transformFootprint(pose.x, pose.y, pose.theta, footprint_, footprint);
+  double x = pose.position.x;
+  double y = pose.position.y;
+  double theta = tf2::getYaw(pose.orientation);
+  transformFootprint(x, y, theta, footprint_, footprint);
 
   return footprint;
 }
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 0bcb809c0dc..1543060ccca 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
@@ -183,10 +183,12 @@ class TestCollisionChecker : public nav2::LifecycleNode
   {
     rclcpp::Time stamp = now();
     publishPose(x, y, theta, stamp);
-    geometry_msgs::msg::Pose2D pose;
-    pose.x = x;
-    pose.y = y;
-    pose.theta = theta;
+
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = x;
+    pose.position.y = y;
+    pose.position.z = 0.0;
+    pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
 
     setPose(x, y, theta, stamp);
     publishFootprint();
diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp
index f56d18b8278..bd85faa903d 100644
--- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp
@@ -27,6 +27,7 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/transform_broadcaster.h"
+#include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/occ_grid_values.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "std_msgs/msg/bool.hpp"
@@ -457,7 +458,7 @@ void TestNode::testFullMask(
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   std_msgs::msg::Bool::SharedPtr binary_state;
 
   unsigned int x, y;
@@ -467,8 +468,10 @@ void TestNode::testFullMask(
   // data = 0
   x = 1;
   y = 0;
-  pose.x = x - tr_x;
-  pose.y = y - tr_y;
+  pose.position.x = x - tr_x;
+  pose.position.y = y - tr_y;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   publishTransform();
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   sign = getSign(x, y, base, multiplier, flip_threshold);
@@ -485,8 +488,10 @@ void TestNode::testFullMask(
   // data in range [1..100] (sparsed for testing speed)
   for (y = 1; y < height_; y += 2) {
     for (x = 0; x < width_; x += 2) {
-      pose.x = x - tr_x;
-      pose.y = y - tr_y;
+      pose.position.x = x - tr_x;
+      pose.position.y = y - tr_y;
+      pose.position.z = 0.0;
+      pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
       publishTransform();
       binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
 
@@ -505,8 +510,10 @@ void TestNode::testFullMask(
 
   // data = -1 (unknown)
   bool prev_state = binary_state->data;
-  pose.x = -tr_x;
-  pose.y = -tr_y;
+  pose.position.x = -tr_x;
+  pose.position.y = -tr_y;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   publishTransform();
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = getBinaryState();
@@ -522,7 +529,7 @@ void TestNode::testSimpleMask(
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   std_msgs::msg::Bool::SharedPtr binary_state;
 
   unsigned int x, y;
@@ -532,8 +539,10 @@ void TestNode::testSimpleMask(
   // data = 0
   x = 1;
   y = 0;
-  pose.x = x - tr_x;
-  pose.y = y - tr_y;
+  pose.position.x = x - tr_x;
+  pose.position.y = y - tr_y;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   publishTransform();
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   sign = getSign(x, y, base, multiplier, flip_threshold);
@@ -550,8 +559,10 @@ void TestNode::testSimpleMask(
   // data = <some_middle_value>
   x = width_ / 2 - 1;
   y = height_ / 2 - 1;
-  pose.x = x - tr_x;
-  pose.y = y - tr_y;
+  pose.position.x = x - tr_x;
+  pose.position.y = y - tr_y;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   publishTransform();
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
 
@@ -569,8 +580,10 @@ void TestNode::testSimpleMask(
   // data = 100
   x = width_ - 1;
   y = height_ - 1;
-  pose.x = x - tr_x;
-  pose.y = y - tr_y;
+  pose.position.x = x - tr_x;
+  pose.position.y = y - tr_y;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   publishTransform();
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
 
@@ -587,8 +600,10 @@ void TestNode::testSimpleMask(
 
   // data = -1 (unknown)
   bool prev_state = binary_state->data;
-  pose.x = -tr_x;
-  pose.y = -tr_y;
+  pose.position.x = -tr_x;
+  pose.position.y = -tr_y;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   publishTransform();
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = getBinaryState();
@@ -608,32 +623,40 @@ void TestNode::testOutOfMask()
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   std_msgs::msg::Bool::SharedPtr binary_state;
 
   // data = <some_middle_value>
-  pose.x = width_ / 2 - 1;
-  pose.y = height_ / 2 - 1;
+  pose.position.x = width_ / 2 - 1;
+  pose.position.y = height_ / 2 - 1;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = waitBinaryState();
-  verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state);
+  verifyBinaryState(getSign(pose.position.x, pose.position.y, base, multiplier, flip_threshold),
+    binary_state);
 
   // Then go to out of mask bounds and ensure that binary state is set back to default
-  pose.x = -2.0;
-  pose.y = -2.0;
+  pose.position.x = -2.0;
+  pose.position.y = -2.0;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = getBinaryState();
   ASSERT_TRUE(binary_state != nullptr);
   ASSERT_EQ(binary_state->data, default_state_);
 
-  pose.x = width_ + 1.0;
-  pose.y = height_ + 1.0;
+  pose.position.x = width_ + 1.0;
+  pose.position.y = height_ + 1.0;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = getBinaryState();
   ASSERT_TRUE(binary_state != nullptr);
   ASSERT_EQ(binary_state->data, default_state_);
 }
 
+
 void TestNode::testIncorrectTF()
 {
   const int min_i = 0;
@@ -641,12 +664,15 @@ void TestNode::testIncorrectTF()
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   std_msgs::msg::Bool::SharedPtr binary_state;
 
   // data = <some_middle_value>
-  pose.x = width_ / 2 - 1;
-  pose.y = height_ / 2 - 1;
+  pose.position.x = width_ / 2 - 1;
+  pose.position.y = height_ / 2 - 1;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
+
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = waitBinaryState();
   ASSERT_TRUE(binary_state == nullptr);
@@ -664,16 +690,19 @@ void TestNode::testResetFilter()
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   std_msgs::msg::Bool::SharedPtr binary_state;
 
   // Switch-on binary filter
-  pose.x = width_ / 2 - 1;
-  pose.y = height_ / 2 - 1;
+  pose.position.x = width_ / 2 - 1;
+  pose.position.y = height_ / 2 - 1;
+  pose.position.z = 0.0;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
   publishTransform();
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = waitBinaryState();
-  verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state);
+  verifyBinaryState(getSign(pose.position.x, pose.position.y, base,
+    multiplier, flip_threshold), binary_state);
 
   // Reset binary filter and check its state was reset to default
   binary_filter_->resetFilter();
@@ -682,6 +711,7 @@ void TestNode::testResetFilter()
   ASSERT_EQ(binary_state->data, default_state_);
 }
 
+
 void TestNode::resetMaps()
 {
   mask_.reset();
diff --git a/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp
index 55774f0a2d6..25731d8462c 100644
--- a/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp
+++ b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp
@@ -33,7 +33,7 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
   void process(
     nav2_costmap_2d::Costmap2D &,
     int, int, int, int,
-    const geometry_msgs::msg::Pose2D &) {}
+    const geometry_msgs::msg::Pose &) {}
 
   void resetFilter() {}
 
diff --git a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp
index e5416845cce..320eeaa1653 100644
--- a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp
@@ -20,7 +20,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/occ_grid_values.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
@@ -47,7 +47,7 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
   // API coverage
   void initializeFilter(const std::string &) {}
   void process(
-    nav2_costmap_2d::Costmap2D &, int, int, int, int, const geometry_msgs::msg::Pose2D &)
+    nav2_costmap_2d::Costmap2D &, int, int, int, int, const geometry_msgs::msg::Pose &)
   {}
   void resetFilter() {}
 };
diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
index 413a423ce72..c75acf2b7dc 100644
--- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
@@ -284,7 +284,7 @@ void TestNode::verifyMasterGrid(unsigned char free_value, unsigned char keepout_
 
 void TestNode::testStandardScenario(unsigned char free_value, unsigned char keepout_value)
 {
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   // Intersection window: added 4 points
   keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose);
   keepout_points_.push_back(Point{3, 3});
@@ -308,9 +308,10 @@ void TestNode::testStandardScenario(unsigned char free_value, unsigned char keep
   verifyMasterGrid(free_value, keepout_value);
 }
 
+
 void TestNode::testFramesScenario(unsigned char free_value, unsigned char keepout_value)
 {
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   // Intersection window: added all 9 points because of map->odom frame shift
   keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose);
   keepout_points_.push_back(Point{2, 2});
@@ -382,7 +383,7 @@ TEST_F(TestNode, testFreeKeepout)
   createKeepoutFilter("map");
 
   // Test KeepoutFilter
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   // Check whole area window
   keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose);
   // There should be no one point appeared on master_grid_ after process()
@@ -401,7 +402,7 @@ TEST_F(TestNode, testUnknownKeepout)
   createKeepoutFilter("map");
 
   // Test KeepoutFilter
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   // Check whole area window
   keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose);
   // There should be no one point appeared on master_grid_ after process()
diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp
index 0904ebd355c..76e96187924 100644
--- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp
@@ -448,12 +448,12 @@ void TestNode::testFullMask(
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit;
 
   // data = 0
-  pose.x = 1 - tr_x;
-  pose.y = -tr_y;
+  pose.position.x = 1 - tr_x;
+  pose.position.y = -tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = getSpeedLimit();
@@ -463,8 +463,8 @@ void TestNode::testFullMask(
   unsigned int x, y;
   for (y = 1; y < height_; y++) {
     for (x = 0; x < width_; x++) {
-      pose.x = x - tr_x;
-      pose.y = y - tr_y;
+      pose.position.x = x - tr_x;
+      pose.position.y = y - tr_y;
       publishTransform();
       speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
       speed_limit = waitSpeedLimit();
@@ -474,8 +474,8 @@ void TestNode::testFullMask(
   }
 
   // data = 0
-  pose.x = 1 - tr_x;
-  pose.y = -tr_y;
+  pose.position.x = 1 - tr_x;
+  pose.position.y = -tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = waitSpeedLimit();
@@ -483,8 +483,8 @@ void TestNode::testFullMask(
   EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT);
 
   // data = -1
-  pose.x = -tr_x;
-  pose.y = -tr_y;
+  pose.position.x = -tr_x;
+  pose.position.y = -tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = getSpeedLimit();
@@ -492,6 +492,7 @@ void TestNode::testFullMask(
   EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT);
 }
 
+
 void TestNode::testSimpleMask(
   uint8_t type, double base, double multiplier,
   double tr_x, double tr_y)
@@ -501,12 +502,12 @@ void TestNode::testSimpleMask(
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit;
 
   // data = 0
-  pose.x = 1 - tr_x;
-  pose.y = -tr_y;
+  pose.position.x = 1 - tr_x;
+  pose.position.y = -tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = getSpeedLimit();
@@ -515,8 +516,8 @@ void TestNode::testSimpleMask(
   // data = <some_middle_value>
   unsigned int x = width_ / 2 - 1;
   unsigned int y = height_ / 2 - 1;
-  pose.x = x - tr_x;
-  pose.y = y - tr_y;
+  pose.position.x = x - tr_x;
+  pose.position.y = y - tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = waitSpeedLimit();
@@ -526,8 +527,8 @@ void TestNode::testSimpleMask(
   // data = 100
   x = width_ - 1;
   y = height_ - 1;
-  pose.x = x - tr_x;
-  pose.y = y - tr_y;
+  pose.position.x = x - tr_x;
+  pose.position.y = y - tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = waitSpeedLimit();
@@ -535,8 +536,8 @@ void TestNode::testSimpleMask(
   verifySpeedLimit(type, base, multiplier, x, y, speed_limit);
 
   // data = 0
-  pose.x = 1 - tr_x;
-  pose.y = -tr_y;
+  pose.position.x = 1 - tr_x;
+  pose.position.y = -tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = waitSpeedLimit();
@@ -544,8 +545,8 @@ void TestNode::testSimpleMask(
   EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT);
 
   // data = -1
-  pose.x = -tr_x;
-  pose.y = -tr_y;
+  pose.position.x = -tr_x;
+  pose.position.y = -tr_y;
   publishTransform();
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = getSpeedLimit();
@@ -560,26 +561,26 @@ void TestNode::testOutOfMask(uint8_t type, double base, double multiplier)
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   nav2_msgs::msg::SpeedLimit::SharedPtr old_speed_limit, speed_limit;
 
   // data = <some_middle_value>
-  pose.x = width_ / 2 - 1;
-  pose.y = height_ / 2 - 1;
+  pose.position.x = width_ / 2 - 1;
+  pose.position.y = height_ / 2 - 1;
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   old_speed_limit = waitSpeedLimit();
   ASSERT_TRUE(old_speed_limit != nullptr);
-  verifySpeedLimit(type, base, multiplier, pose.x, pose.y, old_speed_limit);
+  verifySpeedLimit(type, base, multiplier, pose.position.x, pose.position.y, old_speed_limit);
 
   // Then go to out of mask bounds and ensure that speed limit was not updated
-  pose.x = -2.0;
-  pose.y = -2.0;
+  pose.position.x = -2.0;
+  pose.position.y = -2.0;
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = getSpeedLimit();
   ASSERT_TRUE(speed_limit == old_speed_limit);
 
-  pose.x = width_ + 1.0;
-  pose.y = height_ + 1.0;
+  pose.position.x = width_ + 1.0;
+  pose.position.y = height_ + 1.0;
   speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   speed_limit = getSpeedLimit();
   ASSERT_TRUE(speed_limit == old_speed_limit);
@@ -592,7 +593,7 @@ void TestNode::testIncorrectLimits(uint8_t type, double base, double multiplier)
   const int max_i = width_ + 4;
   const int max_j = height_ + 4;
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit;
 
   std::vector<std::tuple<unsigned int, unsigned int>> points;
@@ -608,12 +609,12 @@ void TestNode::testIncorrectLimits(uint8_t type, double base, double multiplier)
   points.push_back(std::make_tuple(width_ - 1, height_ - 1));
 
   for (auto it = points.begin(); it != points.end(); ++it) {
-    pose.x = static_cast<double>(std::get<0>(*it));
-    pose.y = static_cast<double>(std::get<1>(*it));
+    pose.position.x = static_cast<double>(std::get<0>(*it));
+    pose.position.y = static_cast<double>(std::get<1>(*it));
     speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
     speed_limit = waitSpeedLimit();
     ASSERT_TRUE(speed_limit != nullptr);
-    verifySpeedLimit(type, base, multiplier, pose.x, pose.y, speed_limit);
+    verifySpeedLimit(type, base, multiplier, pose.position.x, pose.position.y, speed_limit);
   }
 }
 
diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
index b1feb3e1d60..83680e3eeae 100644
--- a/nav2_docking/opennav_docking/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -37,11 +37,11 @@ target_include_directories(controller
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(controller PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_graceful_controller::nav2_graceful_controller
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${rcl_interfaces_TARGETS}
@@ -56,7 +56,6 @@ target_include_directories(${library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   angles::angles
@@ -64,6 +63,7 @@ target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   opennav_docking_core::opennav_docking_core
   pluginlib::pluginlib
   rclcpp::rclcpp
@@ -85,10 +85,10 @@ target_include_directories(pose_filter
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(pose_filter PUBLIC
   ${geometry_msgs_TARGETS}
+  nav2_ros_common::nav2_ros_common
 )
 target_link_libraries(pose_filter PRIVATE
   rclcpp::rclcpp
@@ -102,9 +102,11 @@ target_include_directories(${executable_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
-target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp)
+target_link_libraries(${executable_name} PRIVATE ${library_name}
+  rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
+)
 
 rclcpp_components_register_nodes(${library_name} "opennav_docking::DockingServer")
 
@@ -115,7 +117,6 @@ target_include_directories(simple_charging_dock
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(simple_charging_dock PUBLIC
   ${geometry_msgs_TARGETS}
@@ -127,6 +128,7 @@ target_link_libraries(simple_charging_dock PUBLIC
   ${sensor_msgs_TARGETS}
   tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
+  nav2_ros_common::nav2_ros_common
   bondcpp::bondcpp
 )
 target_link_libraries(simple_charging_dock PRIVATE
@@ -143,11 +145,11 @@ target_include_directories(simple_non_charging_dock
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(simple_non_charging_dock PUBLIC
   ${geometry_msgs_TARGETS}
   opennav_docking_core::opennav_docking_core
+  nav2_ros_common::nav2_ros_common
   pose_filter
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index da1bab7f8aa..9048c70a29a 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -204,7 +204,7 @@ bool Controller::isTrajectoryCollisionFree(
     // If this distance is greater than the dock_collision_threshold, check for collisions
     if (use_collision_detection_ &&
       dock_collision_distance > dock_collision_threshold_ &&
-      !collision_checker_->isCollisionFree(nav_2d_utils::poseToPose2D(local_pose.pose)))
+      !collision_checker_->isCollisionFree(local_pose.pose))
     {
       RCLCPP_WARN(
         logger_, "Collision detected at pose: (%.2f, %.2f, %.2f) in frame %s",
diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt
index 60a045e09db..4e26487361a 100644
--- a/nav2_docking/opennav_docking/test/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/test/CMakeLists.txt
@@ -7,6 +7,7 @@ target_link_libraries(test_utils
   ${geometry_msgs_TARGETS}
   ${library_name}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
@@ -14,7 +15,6 @@ target_include_directories(test_utils
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 # Test dock database
@@ -26,12 +26,12 @@ target_link_libraries(test_dock_database
   ${library_name}
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  nav2_ros_common::nav2_ros_common
 )
 target_include_directories(test_dock_database
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 # Test navigator
@@ -44,6 +44,7 @@ target_link_libraries(test_navigator
   ${library_name}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
@@ -51,7 +52,6 @@ target_include_directories(test_navigator
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 # Test Controller
@@ -63,12 +63,12 @@ target_link_libraries(test_controller
   ${library_name}
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  nav2_ros_common::nav2_ros_common
 )
 target_include_directories(test_controller
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 # Test Simple Dock
@@ -78,6 +78,7 @@ ament_add_gtest(test_simple_charging_dock
 target_link_libraries(test_simple_charging_dock
   ament_index_cpp::ament_index_cpp
   ${geometry_msgs_TARGETS}
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS}
@@ -88,7 +89,6 @@ target_include_directories(test_simple_charging_dock
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 ament_add_gtest(test_simple_non_charging_dock
   test_simple_non_charging_dock.cpp
@@ -98,6 +98,7 @@ target_link_libraries(test_simple_non_charging_dock
   ${library_name}
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  nav2_ros_common::nav2_ros_common
 )
 target_link_libraries(test_simple_non_charging_dock
   ${library_name} simple_non_charging_dock
@@ -106,7 +107,6 @@ target_include_directories(test_simple_non_charging_dock
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 # Test Pose Filter (unit)
@@ -115,6 +115,7 @@ ament_add_gtest(test_pose_filter
 )
 target_link_libraries(test_pose_filter
   ${geometry_msgs_TARGETS}
+  nav2_ros_common::nav2_ros_common
   pose_filter
   rclcpp::rclcpp
   tf2::tf2
@@ -124,7 +125,6 @@ target_include_directories(test_pose_filter
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 # Test dock pluing for server tests
@@ -133,6 +133,7 @@ target_link_libraries(test_dock PUBLIC
   ${geometry_msgs_TARGETS}
   ${bond_TARGETS}
   opennav_docking_core::opennav_docking_core
+  nav2_ros_common::nav2_ros_common
   pluginlib::pluginlib
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2_ros::tf2_ros
@@ -147,7 +148,6 @@ target_include_directories(test_dock
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 ament_export_libraries(test_dock)
 
@@ -159,6 +159,7 @@ target_link_libraries(test_docking_server_unit
   ${geometry_msgs_TARGETS}
   ${library_name}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
@@ -166,7 +167,6 @@ target_include_directories(test_docking_server_unit
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 # Test docking server (smoke)
diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt
index 68998664cb7..015265a3753 100644
--- a/nav2_docking/opennav_docking_bt/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt
@@ -21,13 +21,13 @@ target_include_directories(opennav_dock_action_bt_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_compile_definitions(opennav_dock_action_bt_node PRIVATE BT_PLUGIN_EXPORT)
 target_link_libraries(opennav_dock_action_bt_node PUBLIC
   behaviortree_cpp::behaviortree_cpp
   ${geometry_msgs_TARGETS}
   nav2_behavior_tree::nav2_behavior_tree
+  nav2_ros_common::nav2_ros_common
   ${nav2_msgs_TARGETS}
 )
 
@@ -36,13 +36,13 @@ target_include_directories(opennav_undock_action_bt_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_compile_definitions(opennav_undock_action_bt_node PRIVATE BT_PLUGIN_EXPORT)
 target_link_libraries(opennav_undock_action_bt_node PUBLIC
   behaviortree_cpp::behaviortree_cpp
   ${geometry_msgs_TARGETS}
   nav2_behavior_tree::nav2_behavior_tree
+  nav2_ros_common::nav2_ros_common
   ${nav2_msgs_TARGETS}
 )
 
diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt
index 7fb71cd761a..9575861645a 100644
--- a/nav2_docking/opennav_docking_core/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_core/CMakeLists.txt
@@ -15,12 +15,12 @@ target_include_directories(opennav_docking_core
   INTERFACE
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(opennav_docking_core INTERFACE
   ${geometry_msgs_TARGETS}
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2_ros::tf2_ros
+  nav2_ros_common::nav2_ros_common
 )
 
 install(TARGETS opennav_docking_core
diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt
index 30637ca2a35..ba787fb831c 100644
--- a/nav2_dwb_controller/dwb_core/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt
@@ -20,6 +20,7 @@ find_package(std_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 nav2_package()
 
@@ -32,13 +33,13 @@ add_library(dwb_core SHARED
 target_include_directories(dwb_core PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(dwb_core PUBLIC
   ${builtin_interfaces_TARGETS}
   ${dwb_msgs_TARGETS}
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   nav2_util::nav2_util_core
   ${nav_2d_msgs_TARGETS}
@@ -50,6 +51,7 @@ target_link_libraries(dwb_core PUBLIC
   rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS}
   tf2_ros::tf2_ros
+  tf2_geometry_msgs::tf2_geometry_msgs
   ${visualization_msgs_TARGETS}
 )
 
@@ -93,6 +95,7 @@ ament_export_dependencies(
   rclcpp_lifecycle
   sensor_msgs
   tf2_ros
+  tf2_geometry_msgs
   visualization_msgs
   nav2_ros_common
 )
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
index 59b987f9cde..41120a7c48a 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
@@ -44,7 +44,7 @@
 #include "dwb_core/publisher.hpp"
 #include "dwb_core/trajectory_critic.hpp"
 #include "dwb_core/trajectory_generator.hpp"
-#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -140,7 +140,7 @@ class DWBLocalPlanner : public nav2_core::Controller
    * @return          Best command
    */
   virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands(
-    const nav_2d_msgs::msg::Pose2DStamped & pose,
+    const geometry_msgs::msg::PoseStamped & pose,
     const nav_2d_msgs::msg::Twist2D & velocity,
     std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results);
 
@@ -167,14 +167,14 @@ class DWBLocalPlanner : public nav2_core::Controller
    * to match the local costmap's frame
    */
   void prepareGlobalPlan(
-    const nav_2d_msgs::msg::Pose2DStamped & pose, nav_2d_msgs::msg::Path2D & transformed_plan,
-    nav_2d_msgs::msg::Pose2DStamped & goal_pose, bool publish_plan = true);
+    const geometry_msgs::msg::PoseStamped & pose, nav_msgs::msg::Path & transformed_plan,
+    geometry_msgs::msg::PoseStamped & goal_pose, bool publish_plan = true);
 
   /**
    * @brief Iterate through all the twists and find the best one
    */
   virtual dwb_msgs::msg::TrajectoryScore coreScoringAlgorithm(
-    const geometry_msgs::msg::Pose2D & pose,
+    const geometry_msgs::msg::Pose & pose,
     const nav_2d_msgs::msg::Twist2D velocity,
     std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results);
 
@@ -193,9 +193,9 @@ class DWBLocalPlanner : public nav2_core::Controller
    * True means pass just a subset. This gives DWB less discretion to decide how it gets to the
    * nav goal. Instead it is encouraged to try to get on to the path generated by the global planner.
    */
-  virtual nav_2d_msgs::msg::Path2D transformGlobalPlan(
-    const nav_2d_msgs::msg::Pose2DStamped & pose);
-  nav_2d_msgs::msg::Path2D global_plan_;  ///< Saved Global Plan
+  virtual nav_msgs::msg::Path transformGlobalPlan(
+    const geometry_msgs::msg::PoseStamped & pose);
+  nav_msgs::msg::Path global_plan_;  ///< Saved Global Plan
   bool prune_plan_;
   double prune_distance_;
   bool debug_trajectory_details_;
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
index 1e0c1b681de..57cc7e97cd9 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
@@ -94,16 +94,16 @@ class DWBPublisher
   void publishCostGrid(
     const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
     const std::vector<TrajectoryCritic::Ptr> critics);
-  void publishGlobalPlan(const nav_2d_msgs::msg::Path2D plan);
-  void publishTransformedPlan(const nav_2d_msgs::msg::Path2D plan);
-  void publishLocalPlan(const nav_2d_msgs::msg::Path2D plan);
+  void publishGlobalPlan(const nav_msgs::msg::Path plan);
+  void publishTransformedPlan(const nav_msgs::msg::Path plan);
+  void publishLocalPlan(const nav_msgs::msg::Path plan);
 
 protected:
   void publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & results);
 
   // Helper function for publishing other plans
   void publishGenericPlan(
-    const nav_2d_msgs::msg::Path2D plan,
+    const nav_msgs::msg::Path plan,
     rclcpp::Publisher<nav_msgs::msg::Path> & pub, bool flag);
 
   // Flags for turning on/off publishing specific components
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
index 080c598ed7f..74b40be8aec 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
@@ -42,9 +42,9 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav_2d_msgs/msg/twist2_d.hpp"
-#include "nav_2d_msgs/msg/path2_d.hpp"
+#include "nav_msgs/msg/path.hpp"
 #include "dwb_msgs/msg/trajectory2_d.hpp"
 #include "sensor_msgs/msg/point_cloud.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
@@ -131,9 +131,9 @@ class TrajectoryCritic
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
   virtual bool prepare(
-    const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
-    const geometry_msgs::msg::Pose2D &,
-    const nav_2d_msgs::msg::Path2D &)
+    const geometry_msgs::msg::Pose &, const nav_2d_msgs::msg::Twist2D &,
+    const geometry_msgs::msg::Pose &,
+    const nav_msgs::msg::Path &)
   {
     return true;
   }
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp
index a0922ef2f87..3067c1c8d55 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp
@@ -120,7 +120,7 @@ class TrajectoryGenerator
    * @param cmd_vel The desired command velocity
    */
   virtual dwb_msgs::msg::Trajectory2D generateTrajectory(
-    const geometry_msgs::msg::Pose2D & start_pose,
+    const geometry_msgs::msg::Pose & start_pose,
     const nav_2d_msgs::msg::Twist2D & start_vel,
     const nav_2d_msgs::msg::Twist2D & cmd_vel) = 0;
 
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_utils.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_utils.hpp
index 43021901c5c..a714f9dc942 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_utils.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_utils.hpp
@@ -50,7 +50,7 @@ namespace dwb_core
  * Linearly searches through the poses. Once the poses time_offset is greater than the desired time_offset,
  * the search ends, since the poses have increasing time_offsets.
  */
-const geometry_msgs::msg::Pose2D & getClosestPose(
+const geometry_msgs::msg::Pose & getClosestPose(
   const dwb_msgs::msg::Trajectory2D & trajectory,
   const double time_offset);
 
@@ -58,10 +58,10 @@ const geometry_msgs::msg::Pose2D & getClosestPose(
  * @brief Helper function to create a pose with an exact time_offset by linearly interpolating between existing poses
  * @param trajectory The trajectory with pose and time offset information
  * @param time_offset The desired time_offset
- * @return New Pose2D with interpolated values
+ * @return New Pose with interpolated values
  * @note If the given time offset is outside the bounds of the trajectory, the return pose will be either the first or last pose.
  */
-geometry_msgs::msg::Pose2D projectPose(
+geometry_msgs::msg::Pose projectPose(
   const dwb_msgs::msg::Trajectory2D & trajectory,
   const double time_offset);
 
diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
index bc5b42f2823..ce879804bbc 100644
--- a/nav2_dwb_controller/dwb_core/package.xml
+++ b/nav2_dwb_controller/dwb_core/package.xml
@@ -24,6 +24,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
 
diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index 0af32912024..e539c98819a 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -237,15 +237,14 @@ DWBLocalPlanner::loadCritics()
 void
 DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path)
 {
-  auto path2d = nav_2d_utils::pathToPath2D(path);
   for (TrajectoryCritic::Ptr & critic : critics_) {
     critic->reset();
   }
 
   traj_generator_->reset();
 
-  pub_->publishGlobalPlan(path2d);
-  global_plan_ = path2d;
+  pub_->publishGlobalPlan(path);
+  global_plan_ = path;
 }
 
 geometry_msgs::msg::TwistStamped
@@ -261,7 +260,7 @@ DWBLocalPlanner::computeVelocityCommands(
 
   try {
     nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands(
-      nav_2d_utils::poseStampedToPose2D(pose),
+      pose,
       nav_2d_utils::twist3Dto2D(velocity), results);
     pub_->publishEvaluation(results);
     geometry_msgs::msg::TwistStamped cmd_vel;
@@ -284,8 +283,8 @@ DWBLocalPlanner::computeVelocityCommands(
 
 void
 DWBLocalPlanner::prepareGlobalPlan(
-  const nav_2d_msgs::msg::Pose2DStamped & pose, nav_2d_msgs::msg::Path2D & transformed_plan,
-  nav_2d_msgs::msg::Pose2DStamped & goal_pose, bool publish_plan)
+  const geometry_msgs::msg::PoseStamped & pose, nav_msgs::msg::Path & transformed_plan,
+  geometry_msgs::msg::PoseStamped & goal_pose, bool publish_plan)
 {
   transformed_plan = transformGlobalPlan(pose);
   if (publish_plan) {
@@ -293,7 +292,7 @@ DWBLocalPlanner::prepareGlobalPlan(
   }
 
   goal_pose.header.frame_id = global_plan_.header.frame_id;
-  goal_pose.pose = global_plan_.poses.back();
+  goal_pose.pose = global_plan_.poses.back().pose;
   nav_2d_utils::transformPose(
     tf_, costmap_ros_->getGlobalFrameID(), goal_pose,
     goal_pose, transform_tolerance_);
@@ -301,7 +300,7 @@ DWBLocalPlanner::prepareGlobalPlan(
 
 nav_2d_msgs::msg::Twist2DStamped
 DWBLocalPlanner::computeVelocityCommands(
-  const nav_2d_msgs::msg::Pose2DStamped & pose,
+  const geometry_msgs::msg::PoseStamped & pose,
   const nav_2d_msgs::msg::Twist2D & velocity,
   std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results)
 {
@@ -310,8 +309,8 @@ DWBLocalPlanner::computeVelocityCommands(
     results->header.stamp = clock_->now();
   }
 
-  nav_2d_msgs::msg::Path2D transformed_plan;
-  nav_2d_msgs::msg::Pose2DStamped goal_pose;
+  nav_msgs::msg::Path transformed_plan;
+  geometry_msgs::msg::PoseStamped goal_pose;
 
   prepareGlobalPlan(pose, transformed_plan, goal_pose);
 
@@ -364,7 +363,7 @@ DWBLocalPlanner::computeVelocityCommands(
 
 dwb_msgs::msg::TrajectoryScore
 DWBLocalPlanner::coreScoringAlgorithm(
-  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Pose & pose,
   const nav_2d_msgs::msg::Twist2D velocity,
   std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results)
 {
@@ -461,16 +460,16 @@ DWBLocalPlanner::scoreTrajectory(
   return score;
 }
 
-nav_2d_msgs::msg::Path2D
+nav_msgs::msg::Path
 DWBLocalPlanner::transformGlobalPlan(
-  const nav_2d_msgs::msg::Pose2DStamped & pose)
+  const geometry_msgs::msg::PoseStamped & pose)
 {
   if (global_plan_.poses.empty()) {
     throw nav2_core::InvalidPath("Received plan with zero length");
   }
 
   // let's get the pose of the robot in the frame of the plan
-  nav_2d_msgs::msg::Pose2DStamped robot_pose;
+  geometry_msgs::msg::PoseStamped robot_pose;
   if (!nav_2d_utils::transformPose(
       tf_, global_plan_.header.frame_id, pose,
       robot_pose, transform_tolerance_))
@@ -519,7 +518,7 @@ DWBLocalPlanner::transformGlobalPlan(
   auto transformation_begin = std::find_if(
     begin(global_plan_.poses), prune_point,
     [&](const auto & global_plan_pose) {
-      return euclidean_distance(robot_pose.pose, global_plan_pose) < transform_start_threshold;
+      return euclidean_distance(robot_pose, global_plan_pose) < transform_start_threshold;
     });
 
   // Find the first pose in the end of the plan that's further than transform_end_threshold
@@ -527,24 +526,22 @@ DWBLocalPlanner::transformGlobalPlan(
   auto transformation_end = std::find_if(
     transformation_begin, global_plan_.poses.end(),
     [&](const auto & global_plan_pose) {
-      return euclidean_distance(global_plan_pose, robot_pose.pose) > transform_end_threshold;
+      return euclidean_distance(global_plan_pose, robot_pose) > transform_end_threshold;
     });
 
   // Transform the near part of the global plan into the robot's frame of reference.
-  nav_2d_msgs::msg::Path2D transformed_plan;
+  nav_msgs::msg::Path transformed_plan;
   transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID();
   transformed_plan.header.stamp = pose.header.stamp;
 
   // Helper function for the transform below. Converts a pose2D from global
   // frame to local
   auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
-      nav_2d_msgs::msg::Pose2DStamped stamped_pose, transformed_pose;
-      stamped_pose.header.frame_id = global_plan_.header.frame_id;
-      stamped_pose.pose = global_plan_pose;
+      geometry_msgs::msg::PoseStamped transformed_pose;
       nav_2d_utils::transformPose(
         tf_, transformed_plan.header.frame_id,
-        stamped_pose, transformed_pose, transform_tolerance_);
-      return transformed_pose.pose;
+        global_plan_pose, transformed_pose, transform_tolerance_);
+      return transformed_pose;
     };
 
   std::transform(
diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp
index aad8e2e16f2..46651d586f6 100644
--- a/nav2_dwb_controller/dwb_core/src/publisher.cpp
+++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp
@@ -220,8 +220,8 @@ DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & res
     }
     m.points.clear();
     for (unsigned int j = 0; j < twist.traj.poses.size(); ++j) {
-      pt.x = twist.traj.poses[j].x;
-      pt.y = twist.traj.poses[j].y;
+      pt.x = twist.traj.poses[j].position.x;
+      pt.y = twist.traj.poses[j].position.y;
       pt.z = 0;
       m.points.push_back(pt);
     }
@@ -239,7 +239,7 @@ DWBPublisher::publishLocalPlan(
 
   auto path =
     std::make_unique<nav_msgs::msg::Path>(
-    nav_2d_utils::poses2DToPath(
+    nav_2d_utils::posesToPath(
       traj.poses, header.frame_id,
       header.stamp));
 
@@ -338,31 +338,31 @@ DWBPublisher::publishCostGrid(
 }
 
 void
-DWBPublisher::publishGlobalPlan(const nav_2d_msgs::msg::Path2D plan)
+DWBPublisher::publishGlobalPlan(const nav_msgs::msg::Path plan)
 {
   publishGenericPlan(plan, *global_pub_, publish_global_plan_);
 }
 
 void
-DWBPublisher::publishTransformedPlan(const nav_2d_msgs::msg::Path2D plan)
+DWBPublisher::publishTransformedPlan(const nav_msgs::msg::Path plan)
 {
   publishGenericPlan(plan, *transformed_pub_, publish_transformed_);
 }
 
 void
-DWBPublisher::publishLocalPlan(const nav_2d_msgs::msg::Path2D plan)
+DWBPublisher::publishLocalPlan(const nav_msgs::msg::Path plan)
 {
   publishGenericPlan(plan, *local_pub_, publish_local_plan_);
 }
 
 void
 DWBPublisher::publishGenericPlan(
-  const nav_2d_msgs::msg::Path2D plan,
+  const nav_msgs::msg::Path plan,
   rclcpp::Publisher<nav_msgs::msg::Path> & pub, bool flag)
 {
   if (pub.get_subscription_count() < 1) {return;}
   if (!flag) {return;}
-  auto path = std::make_unique<nav_msgs::msg::Path>(nav_2d_utils::pathToPath(plan));
+  auto path = std::make_unique<nav_msgs::msg::Path>(plan);
   pub.publish(std::move(path));
 }
 
diff --git a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp
index b89957f34ec..71e2c7cf316 100644
--- a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp
+++ b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp
@@ -36,13 +36,16 @@
 
 #include <cmath>
 
+#include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
 #include "rclcpp/duration.hpp"
 
 #include "dwb_core/exceptions.hpp"
 
 namespace dwb_core
 {
-const geometry_msgs::msg::Pose2D & getClosestPose(
+const geometry_msgs::msg::Pose & getClosestPose(
   const dwb_msgs::msg::Trajectory2D & trajectory,
   const double time_offset)
 {
@@ -66,7 +69,7 @@ const geometry_msgs::msg::Pose2D & getClosestPose(
   return trajectory.poses[closest_index];
 }
 
-geometry_msgs::msg::Pose2D projectPose(
+geometry_msgs::msg::Pose projectPose(
   const dwb_msgs::msg::Trajectory2D & trajectory,
   const double time_offset)
 {
@@ -91,12 +94,20 @@ geometry_msgs::msg::Pose2D projectPose(
       double ratio = (goal_time - rclcpp::Duration(trajectory.time_offsets[i])).seconds() /
         time_diff;
       double inv_ratio = 1.0 - ratio;
-      const geometry_msgs::msg::Pose2D & pose_a = trajectory.poses[i];
-      const geometry_msgs::msg::Pose2D & pose_b = trajectory.poses[i + 1];
-      geometry_msgs::msg::Pose2D projected;
-      projected.x = pose_a.x * inv_ratio + pose_b.x * ratio;
-      projected.y = pose_a.y * inv_ratio + pose_b.y * ratio;
-      projected.theta = pose_a.theta * inv_ratio + pose_b.theta * ratio;
+      const auto & pose_a = trajectory.poses[i];
+      const auto & pose_b = trajectory.poses[i + 1];
+
+      geometry_msgs::msg::Pose projected;
+      projected.position.x = pose_a.position.x * inv_ratio + pose_b.position.x * ratio;
+      projected.position.y = pose_a.position.y * inv_ratio + pose_b.position.y * ratio;
+      projected.position.z = pose_a.position.z * inv_ratio + pose_b.position.z * ratio;
+
+      // Interpolate orientation using slerp
+      tf2::Quaternion q1, q2;
+      tf2::fromMsg(pose_a.orientation, q1);
+      tf2::fromMsg(pose_b.orientation, q2);
+      projected.orientation = tf2::toMsg(q1.slerp(q2, ratio));
+
       return projected;
     }
   }
diff --git a/nav2_dwb_controller/dwb_core/test/utils_test.cpp b/nav2_dwb_controller/dwb_core/test/utils_test.cpp
index c8aff66bd09..f4389a5d4da 100644
--- a/nav2_dwb_controller/dwb_core/test/utils_test.cpp
+++ b/nav2_dwb_controller/dwb_core/test/utils_test.cpp
@@ -34,6 +34,8 @@
 
 #include "gtest/gtest.h"
 #include "dwb_core/trajectory_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "tf2/utils.hpp"
 
 using dwb_core::getClosestPose;
 using dwb_core::projectPose;
@@ -45,20 +47,20 @@ TEST(Utils, ClosestPose)
   traj.time_offsets.resize(4);
   for (unsigned int i = 0; i < traj.poses.size(); i++) {
     double d = static_cast<double>(i);
-    traj.poses[i].x = d;
+    traj.poses[i].position.x = d;
     traj.time_offsets[i] = rclcpp::Duration::from_seconds(d);
   }
 
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.0).x, traj.poses[0].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, -1.0).x, traj.poses[0].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.4).x, traj.poses[0].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.5).x, traj.poses[0].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.51).x, traj.poses[1].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.0).x, traj.poses[1].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.4999).x, traj.poses[1].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.0).x, traj.poses[2].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.51).x, traj.poses[3].x);
-  EXPECT_DOUBLE_EQ(getClosestPose(traj, 3.5).x, traj.poses[3].x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.0).position.x, traj.poses[0].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, -1.0).position.x, traj.poses[0].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.4).position.x, traj.poses[0].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.5).position.x, traj.poses[0].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.51).position.x, traj.poses[1].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.0).position.x, traj.poses[1].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.4999).position.x, traj.poses[1].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.0).position.x, traj.poses[2].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.51).position.x, traj.poses[3].position.x);
+  EXPECT_DOUBLE_EQ(getClosestPose(traj, 3.5).position.x, traj.poses[3].position.x);
 }
 
 TEST(Utils, ProjectPose)
@@ -68,44 +70,45 @@ TEST(Utils, ProjectPose)
   traj.time_offsets.resize(4);
   for (unsigned int i = 0; i < traj.poses.size(); i++) {
     double d = static_cast<double>(i);
-    traj.poses[i].x = d;
-    traj.poses[i].y = 30.0 - 2.0 * d;
-    traj.poses[i].theta = 0.42;
+    traj.poses[i].position.x = d;
+    traj.poses[i].position.y = 30.0 - 2.0 * d;
+    traj.poses[i].orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.42);
     traj.time_offsets[i] = rclcpp::Duration::from_seconds(d);
   }
 
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).x, 0.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).y, 30.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).x, 0.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).y, 30.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).x, 0.4);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).y, 29.2);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).x, 0.5);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).y, 29.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).x, 0.51);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).y, 28.98);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).x, 1.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).y, 28.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).x, 1.4999);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).y, 27.0002);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).x, 2.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).y, 26.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).theta, 0.42);
-  EXPECT_FLOAT_EQ(projectPose(traj, 2.51).x, 2.51);
-  EXPECT_FLOAT_EQ(projectPose(traj, 2.51).y, 24.98);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 2.51).theta, 0.42);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).x, 3.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).y, 24.0);
-  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).theta, 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).position.x, 0.0);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).position.y, 30.0);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 0.0).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).position.x, 0.0);
+  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).position.y, 30.0);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, -1.0).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).position.x, 0.4);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).position.y, 29.2);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 0.4).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).position.x, 0.5);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).position.y, 29.0);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 0.5).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).position.x, 0.51);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).position.y, 28.98);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 0.51).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).position.x, 1.0);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).position.y, 28.0);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 1.0).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).position.x, 1.4999);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).position.y, 27.0002);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 1.4999).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).position.x, 2.0);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).position.y, 26.0);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 2.0).orientation), 0.42);
+  EXPECT_FLOAT_EQ(projectPose(traj, 2.51).position.x, 2.51);
+  EXPECT_FLOAT_EQ(projectPose(traj, 2.51).position.y, 24.98);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 2.51).orientation), 0.42);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).position.x, 3.0);
+  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).position.y, 24.0);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(projectPose(traj, 3.5).orientation), 0.42);
 }
 
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
index 95f86354050..b1cb464cd5c 100644
--- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
@@ -15,6 +15,8 @@ find_package(nav_2d_utils REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(nav2_ros_common REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
 
 nav2_package()
 
@@ -35,7 +37,6 @@ add_library(${PROJECT_NAME} SHARED
 target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${PROJECT_NAME} PUBLIC
   costmap_queue::costmap_queue
@@ -43,8 +44,11 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
   ${dwb_msgs_TARGETS}
   ${geometry_msgs_TARGETS}
   nav2_costmap_2d::nav2_costmap_2d_core
+  nav2_ros_common::nav2_ros_common
   ${nav_2d_msgs_TARGETS}
   rclcpp::rclcpp
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
 )
 target_link_libraries(${PROJECT_NAME} PRIVATE
   angles::angles
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/alignment_util.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/alignment_util.hpp
index 1c6c179758f..d46158de4fd 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/alignment_util.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/alignment_util.hpp
@@ -35,7 +35,7 @@
 #ifndef DWB_CRITICS__ALIGNMENT_UTIL_HPP_
 #define DWB_CRITICS__ALIGNMENT_UTIL_HPP_
 
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 
 namespace dwb_critics
 {
@@ -47,7 +47,7 @@ namespace dwb_critics
  *
  * (used in both path_align and dist_align)
  */
-geometry_msgs::msg::Pose2D getForwardPose(const geometry_msgs::msg::Pose2D & pose, double distance);
+geometry_msgs::msg::Pose getForwardPose(const geometry_msgs::msg::Pose & pose, double distance);
 
 }  // namespace dwb_critics
 
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp
index 9bd4675ccb8..538ff13126e 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp
@@ -66,7 +66,7 @@ class BaseObstacleCritic : public dwb_core::TrajectoryCritic
    * @brief Return the obstacle score for a particular pose
    * @param pose Pose to check
    */
-  virtual double scorePose(const geometry_msgs::msg::Pose2D & pose);
+  virtual double scorePose(const geometry_msgs::msg::Pose & pose);
 
   /**
    * @brief Check to see whether a given cell cost is valid for driving through.
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_align.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_align.hpp
index 3345cf546ef..abecc194a93 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_align.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_align.hpp
@@ -56,9 +56,9 @@ class GoalAlignCritic : public GoalDistCritic
   : forward_point_distance_(0.0) {}
   void onInit() override;
   bool prepare(
-    const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-    const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
-  double scorePose(const geometry_msgs::msg::Pose2D & pose) override;
+    const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+    const geometry_msgs::msg::Pose & goal, const nav_msgs::msg::Path & global_plan) override;
+  double scorePose(const geometry_msgs::msg::Pose & pose) override;
 
 protected:
   double forward_point_distance_;
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_dist.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_dist.hpp
index 33959ea0eb9..a34682b3b6d 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_dist.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_dist.hpp
@@ -51,12 +51,12 @@ class GoalDistCritic : public MapGridCritic
 {
 public:
   bool prepare(
-    const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-    const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
+    const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+    const geometry_msgs::msg::Pose & goal, const nav_msgs::msg::Path & global_plan) override;
 
 protected:
   bool getLastPoseOnCostmap(
-    const nav_2d_msgs::msg::Path2D & global_plan, unsigned int & x,
+    const nav_msgs::msg::Path & global_plan, unsigned int & x,
     unsigned int & y);
 };
 
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp
index df6f77e7725..bbd7ea1643f 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp
@@ -74,7 +74,7 @@ class MapGridCritic : public dwb_core::TrajectoryCritic
    * @param pose The pose to score, assumed to be in the same frame as the costmap
    * @return The score associated with the cell of the costmap where the pose lies
    */
-  virtual double scorePose(const geometry_msgs::msg::Pose2D & pose);
+  virtual double scorePose(const geometry_msgs::msg::Pose & pose);
 
   /**
    * @brief Retrieve the score for a particular cell of the costmap
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp
index a859c392e50..231b654891e 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp
@@ -49,7 +49,7 @@ typedef std::vector<geometry_msgs::msg::Point> Footprint;
  * @return oriented footprint
  */
 Footprint getOrientedFootprint(
-  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Pose & pose,
   const Footprint & footprint_spec);
 
 /**
@@ -66,11 +66,11 @@ class ObstacleFootprintCritic : public BaseObstacleCritic
 {
 public:
   bool prepare(
-    const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-    const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
-  double scorePose(const geometry_msgs::msg::Pose2D & pose) override;
+    const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+    const geometry_msgs::msg::Pose & goal, const nav_msgs::msg::Path & global_plan) override;
+  double scorePose(const geometry_msgs::msg::Pose & pose) override;
   virtual double scorePose(
-    const geometry_msgs::msg::Pose2D & pose,
+    const geometry_msgs::msg::Pose & pose,
     const Footprint & oriented_footprint);
   double getScale() const override {return costmap_->getResolution() * scale_;}
 
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp
index bb2280801e2..6e7346ccecb 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp
@@ -86,8 +86,8 @@ class OscillationCritic : public dwb_core::TrajectoryCritic
   : oscillation_reset_time_(0s) {}
   void onInit() override;
   bool prepare(
-    const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-    const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
+    const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+    const geometry_msgs::msg::Pose & goal, const nav_msgs::msg::Path & global_plan) override;
   double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
   void reset() override;
   void debrief(const nav_2d_msgs::msg::Twist2D & cmd_vel) override;
@@ -152,7 +152,7 @@ class OscillationCritic : public dwb_core::TrajectoryCritic
   double oscillation_reset_dist_sq_;
 
   // Saved positions
-  geometry_msgs::msg::Pose2D pose_, prev_stationary_pose_;
+  geometry_msgs::msg::Pose pose_, prev_stationary_pose_;
   // Saved timestamp
   rclcpp::Time prev_reset_time_;
   rclcpp::Clock::SharedPtr clock_;
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_align.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_align.hpp
index 34f880f7b71..9e0d34c23ed 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_align.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_align.hpp
@@ -60,10 +60,10 @@ class PathAlignCritic : public PathDistCritic
   : zero_scale_(false), forward_point_distance_(0.0) {}
   void onInit() override;
   bool prepare(
-    const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-    const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
+    const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+    const geometry_msgs::msg::Pose & goal, const nav_msgs::msg::Path & global_plan) override;
   double getScale() const override;
-  double scorePose(const geometry_msgs::msg::Pose2D & pose) override;
+  double scorePose(const geometry_msgs::msg::Pose & pose) override;
 
 protected:
   bool zero_scale_;
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_dist.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_dist.hpp
index fdf36882c73..eec7eb842cd 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_dist.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_dist.hpp
@@ -46,8 +46,8 @@ class PathDistCritic : public MapGridCritic
 {
 public:
   bool prepare(
-    const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-    const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
+    const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+    const geometry_msgs::msg::Pose & goal, const nav_msgs::msg::Path & global_plan) override;
 };
 
 }  // namespace dwb_critics
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp
index 405dbc14723..00814689aee 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp
@@ -73,8 +73,8 @@ class RotateToGoalCritic : public dwb_core::TrajectoryCritic
   void onInit() override;
   void reset() override;
   bool prepare(
-    const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-    const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
+    const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+    const geometry_msgs::msg::Pose & goal, const nav_msgs::msg::Path & global_plan) override;
   double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
   /**
    * @brief Assuming that this is an actual rotation when near the goal, score the trajectory.
diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
index 5e943e3fde2..f6c22047aa6 100644
--- a/nav2_dwb_controller/dwb_critics/package.xml
+++ b/nav2_dwb_controller/dwb_critics/package.xml
@@ -21,6 +21,8 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>nav2_ros_common</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/nav2_dwb_controller/dwb_critics/src/alignment_util.cpp b/nav2_dwb_controller/dwb_critics/src/alignment_util.cpp
index 5c3f18f7623..9e37ec5941b 100644
--- a/nav2_dwb_controller/dwb_critics/src/alignment_util.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/alignment_util.cpp
@@ -34,18 +34,23 @@
 
 #include "dwb_critics/alignment_util.hpp"
 #include <cmath>
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 using std::cos;
 using std::sin;
 
 namespace dwb_critics
 {
-geometry_msgs::msg::Pose2D getForwardPose(const geometry_msgs::msg::Pose2D & pose, double distance)
+geometry_msgs::msg::Pose getForwardPose(const geometry_msgs::msg::Pose & pose, double distance)
 {
-  geometry_msgs::msg::Pose2D forward_pose;
-  forward_pose.x = pose.x + distance * cos(pose.theta);
-  forward_pose.y = pose.y + distance * sin(pose.theta);
-  forward_pose.theta = pose.theta;
+  geometry_msgs::msg::Pose forward_pose;
+  double theta = tf2::getYaw(pose.orientation);
+  forward_pose.position.x = pose.position.x + distance * cos(theta);
+  forward_pose.position.y = pose.position.y + distance * sin(theta);
+  forward_pose.position.z = pose.position.z;
+  forward_pose.orientation = pose.orientation;
   return forward_pose;
 }
+
 }  // namespace dwb_critics
diff --git a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
index 5b14322cad1..3d591b43e94 100644
--- a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
@@ -73,10 +73,10 @@ double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & t
   return score;
 }
 
-double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
+double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose & pose)
 {
   unsigned int cell_x, cell_y;
-  if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
+  if (!costmap_->worldToMap(pose.position.x, pose.position.y, cell_x, cell_y)) {
     throw dwb_core::
           IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
   }
diff --git a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp
index 6ff25bfa939..1ae85c2e1dd 100644
--- a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp
@@ -58,25 +58,26 @@ void GoalAlignCritic::onInit()
 }
 
 bool GoalAlignCritic::prepare(
-  const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-  const geometry_msgs::msg::Pose2D & goal,
-  const nav_2d_msgs::msg::Path2D & global_plan)
+  const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+  const geometry_msgs::msg::Pose & goal,
+  const nav_msgs::msg::Path & global_plan)
 {
   // we want the robot nose to be drawn to its final position
   // (before robot turns towards goal orientation), not the end of the
   // path for the robot center. Choosing the final position after
   // turning towards goal orientation causes instability when the
   // robot needs to make a 180 degree turn at the end
-  double angle_to_goal = atan2(goal.y - pose.y, goal.x - pose.x);
+  double angle_to_goal = atan2(goal.position.y - pose.position.y,
+    goal.position.x - pose.position.x);
 
-  nav_2d_msgs::msg::Path2D target_poses = global_plan;
-  target_poses.poses.back().x += forward_point_distance_ * cos(angle_to_goal);
-  target_poses.poses.back().y += forward_point_distance_ * sin(angle_to_goal);
+  nav_msgs::msg::Path target_poses = global_plan;
+  target_poses.poses.back().pose.position.x += forward_point_distance_ * cos(angle_to_goal);
+  target_poses.poses.back().pose.position.y += forward_point_distance_ * sin(angle_to_goal);
 
   return GoalDistCritic::prepare(pose, vel, goal, target_poses);
 }
 
-double GoalAlignCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
+double GoalAlignCritic::scorePose(const geometry_msgs::msg::Pose & pose)
 {
   return GoalDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
 }
diff --git a/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp b/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp
index b70f94353df..06b4f31d147 100644
--- a/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp
@@ -41,9 +41,9 @@
 namespace dwb_critics
 {
 bool GoalDistCritic::prepare(
-  const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
-  const geometry_msgs::msg::Pose2D &,
-  const nav_2d_msgs::msg::Path2D & global_plan)
+  const geometry_msgs::msg::Pose &, const nav_2d_msgs::msg::Twist2D &,
+  const geometry_msgs::msg::Pose &,
+  const nav_msgs::msg::Path & global_plan)
 {
   reset();
 
@@ -63,18 +63,18 @@ bool GoalDistCritic::prepare(
 }
 
 bool GoalDistCritic::getLastPoseOnCostmap(
-  const nav_2d_msgs::msg::Path2D & global_plan,
+  const nav_msgs::msg::Path & global_plan,
   unsigned int & x, unsigned int & y)
 {
-  nav_2d_msgs::msg::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(
+  nav_msgs::msg::Path adjusted_global_plan = nav_2d_utils::adjustPlanResolution(
     global_plan,
     costmap_->getResolution());
   bool started_path = false;
 
   // skip global path points until we reach the border of the local map
   for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); ++i) {
-    double g_x = adjusted_global_plan.poses[i].x;
-    double g_y = adjusted_global_plan.poses[i].y;
+    double g_x = adjusted_global_plan.poses[i].pose.position.x;
+    double g_y = adjusted_global_plan.poses[i].pose.position.y;
     unsigned int map_x, map_y;
     if (costmap_->worldToMap(
         g_x, g_y, map_x,
diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
index 782ce618267..fc8e217c23e 100644
--- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
@@ -155,11 +155,11 @@ double MapGridCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
   return score;
 }
 
-double MapGridCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
+double MapGridCritic::scorePose(const geometry_msgs::msg::Pose & pose)
 {
   unsigned int cell_x, cell_y;
   // we won't allow trajectories that go off the map... shouldn't happen that often anyways
-  if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
+  if (!costmap_->worldToMap(pose.position.x, pose.position.y, cell_x, cell_y)) {
     throw dwb_core::
           IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
   }
diff --git a/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp b/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp
index cec0129e57d..2b79069ab63 100644
--- a/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp
@@ -45,24 +45,25 @@ PLUGINLIB_EXPORT_CLASS(dwb_critics::ObstacleFootprintCritic, dwb_core::Trajector
 namespace dwb_critics
 {
 Footprint getOrientedFootprint(
-  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Pose & pose,
   const Footprint & footprint_spec)
 {
   std::vector<geometry_msgs::msg::Point> oriented_footprint;
   oriented_footprint.resize(footprint_spec.size());
-  double cos_th = cos(pose.theta);
-  double sin_th = sin(pose.theta);
+  double theta = tf2::getYaw(pose.orientation);
+  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 = oriented_footprint[i];
-    new_pt.x = pose.x + footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th;
-    new_pt.y = pose.y + footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th;
+    new_pt.x = pose.position.x + footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th;
+    new_pt.y = pose.position.y + footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th;
   }
   return oriented_footprint;
 }
 
 bool ObstacleFootprintCritic::prepare(
-  const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
-  const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Path2D &)
+  const geometry_msgs::msg::Pose &, const nav_2d_msgs::msg::Twist2D &,
+  const geometry_msgs::msg::Pose &, const nav_msgs::msg::Path &)
 {
   footprint_spec_ = costmap_ros_->getRobotFootprint();
   if (footprint_spec_.size() == 0) {
@@ -74,10 +75,10 @@ bool ObstacleFootprintCritic::prepare(
   return true;
 }
 
-double ObstacleFootprintCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
+double ObstacleFootprintCritic::scorePose(const geometry_msgs::msg::Pose & pose)
 {
   unsigned int cell_x, cell_y;
-  if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
+  if (!costmap_->worldToMap(pose.position.x, pose.position.y, cell_x, cell_y)) {
     throw dwb_core::
           IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
   }
@@ -85,7 +86,7 @@ double ObstacleFootprintCritic::scorePose(const geometry_msgs::msg::Pose2D & pos
 }
 
 double ObstacleFootprintCritic::scorePose(
-  const geometry_msgs::msg::Pose2D &,
+  const geometry_msgs::msg::Pose &,
   const Footprint & footprint)
 {
   // now we really have to lay down the footprint in the costmap grid
diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
index 6472f5c1a23..bad8e40e3ff 100644
--- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
@@ -41,6 +41,7 @@
 #include "nav2_ros_common/node_utils.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "pluginlib/class_list_macros.hpp"
+#include "angles/angles.h"
 
 PLUGINLIB_EXPORT_CLASS(dwb_critics::OscillationCritic, dwb_core::TrajectoryCritic)
 
@@ -148,10 +149,10 @@ void OscillationCritic::onInit()
 }
 
 bool OscillationCritic::prepare(
-  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Pose & pose,
   const nav_2d_msgs::msg::Twist2D &,
-  const geometry_msgs::msg::Pose2D &,
-  const nav_2d_msgs::msg::Path2D &)
+  const geometry_msgs::msg::Pose &,
+  const nav_msgs::msg::Path &)
 {
   pose_ = pose;
   return true;
@@ -172,19 +173,24 @@ void OscillationCritic::debrief(const nav_2d_msgs::msg::Twist2D & cmd_vel)
     }
   }
 }
-
 bool OscillationCritic::resetAvailable()
 {
   if (oscillation_reset_dist_ >= 0.0) {
-    double x_diff = pose_.x - prev_stationary_pose_.x;
-    double y_diff = pose_.y - prev_stationary_pose_.y;
+    double x_diff = pose_.position.x - prev_stationary_pose_.position.x;
+    double y_diff = pose_.position.y - prev_stationary_pose_.position.y;
     double sq_dist = x_diff * x_diff + y_diff * y_diff;
     if (sq_dist > oscillation_reset_dist_sq_) {
       return true;
     }
   }
   if (oscillation_reset_angle_ >= 0.0) {
-    double th_diff = pose_.theta - prev_stationary_pose_.theta;
+    tf2::Quaternion pose_q, prev_stationary_pose_q;
+    tf2::fromMsg(pose_.orientation, pose_q);
+    tf2::fromMsg(prev_stationary_pose_.orientation, prev_stationary_pose_q);
+
+    double th_diff = angles::shortest_angular_distance(
+       tf2::getYaw(pose_q), tf2::getYaw(prev_stationary_pose_q));
+
     if (fabs(th_diff) > oscillation_reset_angle_) {
       return true;
     }
diff --git a/nav2_dwb_controller/dwb_critics/src/path_align.cpp b/nav2_dwb_controller/dwb_critics/src/path_align.cpp
index 23c9dfb2350..6c4b28ea6f5 100644
--- a/nav2_dwb_controller/dwb_critics/src/path_align.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/path_align.cpp
@@ -58,12 +58,12 @@ void PathAlignCritic::onInit()
 }
 
 bool PathAlignCritic::prepare(
-  const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-  const geometry_msgs::msg::Pose2D & goal,
-  const nav_2d_msgs::msg::Path2D & global_plan)
+  const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+  const geometry_msgs::msg::Pose & goal,
+  const nav_msgs::msg::Path & global_plan)
 {
-  double dx = pose.x - goal.x;
-  double dy = pose.y - goal.y;
+  double dx = pose.position.x - goal.position.x;
+  double dy = pose.position.y - goal.position.y;
   double sq_dist = dx * dx + dy * dy;
   if (sq_dist > forward_point_distance_ * forward_point_distance_) {
     zero_scale_ = false;
@@ -85,7 +85,7 @@ double PathAlignCritic::getScale() const
   }
 }
 
-double PathAlignCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
+double PathAlignCritic::scorePose(const geometry_msgs::msg::Pose & pose)
 {
   return PathDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
 }
diff --git a/nav2_dwb_controller/dwb_critics/src/path_dist.cpp b/nav2_dwb_controller/dwb_critics/src/path_dist.cpp
index 3ba6a173436..034bd30bf5e 100644
--- a/nav2_dwb_controller/dwb_critics/src/path_dist.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/path_dist.cpp
@@ -41,14 +41,14 @@
 namespace dwb_critics
 {
 bool PathDistCritic::prepare(
-  const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
-  const geometry_msgs::msg::Pose2D &,
-  const nav_2d_msgs::msg::Path2D & global_plan)
+  const geometry_msgs::msg::Pose &, const nav_2d_msgs::msg::Twist2D &,
+  const geometry_msgs::msg::Pose &,
+  const nav_msgs::msg::Path & global_plan)
 {
   reset();
   bool started_path = false;
 
-  nav_2d_msgs::msg::Path2D adjusted_global_plan =
+  nav_msgs::msg::Path adjusted_global_plan =
     nav_2d_utils::adjustPlanResolution(global_plan, costmap_->getResolution());
 
   if (adjusted_global_plan.poses.size() != global_plan.poses.size()) {
@@ -61,8 +61,8 @@ bool PathDistCritic::prepare(
   unsigned int i;
   // put global path points into local map until we reach the border of the local map
   for (i = 0; i < adjusted_global_plan.poses.size(); ++i) {
-    double g_x = adjusted_global_plan.poses[i].x;
-    double g_y = adjusted_global_plan.poses[i].y;
+    double g_x = adjusted_global_plan.poses[i].pose.position.x;
+    double g_y = adjusted_global_plan.poses[i].pose.position.y;
     unsigned int map_x, map_y;
     if (costmap_->worldToMap(
         g_x, g_y, map_x,
diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
index 7a46a0340e9..f4323188b18 100644
--- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
@@ -82,15 +82,15 @@ void RotateToGoalCritic::reset()
 }
 
 bool RotateToGoalCritic::prepare(
-  const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
-  const geometry_msgs::msg::Pose2D & goal,
-  const nav_2d_msgs::msg::Path2D &)
+  const geometry_msgs::msg::Pose & pose, const nav_2d_msgs::msg::Twist2D & vel,
+  const geometry_msgs::msg::Pose & goal,
+  const nav_msgs::msg::Path &)
 {
-  double dxy_sq = hypot_sq(pose.x - goal.x, pose.y - goal.y);
+  double dxy_sq = hypot_sq(pose.position.x - goal.position.x, pose.position.y - goal.position.y);
   in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
   current_xy_speed_sq_ = hypot_sq(vel.x, vel.y);
   rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_);
-  goal_yaw_ = goal.theta;
+  goal_yaw_ = tf2::getYaw(goal.orientation);
   return true;
 }
 
@@ -124,10 +124,10 @@ double RotateToGoalCritic::scoreRotation(const dwb_msgs::msg::Trajectory2D & tra
 
   double end_yaw;
   if (lookahead_time_ >= 0.0) {
-    geometry_msgs::msg::Pose2D eval_pose = dwb_core::projectPose(traj, lookahead_time_);
-    end_yaw = eval_pose.theta;
+    geometry_msgs::msg::Pose eval_pose = dwb_core::projectPose(traj, lookahead_time_);
+    end_yaw = tf2::getYaw(eval_pose.orientation);
   } else {
-    end_yaw = traj.poses.back().theta;
+    end_yaw = tf2::getYaw(traj.poses.back().orientation);
   }
   return fabs(angles::shortest_angular_distance(end_yaw, goal_yaw_));
 }
diff --git a/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp b/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp
index 3f966474710..e85e51260bd 100644
--- a/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp
+++ b/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp
@@ -38,27 +38,37 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
+#include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "dwb_critics/alignment_util.hpp"
 #include "dwb_core/exceptions.hpp"
 
 TEST(AlignmentUtil, TestProjection)
 {
-  geometry_msgs::msg::Pose2D pose, pose_out;
-  pose.x = 1.0;
-  pose.y = -1.0;
+  geometry_msgs::msg::Pose pose, pose_out;
+  pose.position.x = 1.0;
+  pose.position.y = -1.0;
+  tf2::Quaternion q1;
+  q1.setRPY(0, 0, 0.0);  // theta = 0.0
+  pose.orientation = tf2::toMsg(q1);
+
   double distance = 1.0;
   pose_out = dwb_critics::getForwardPose(pose, distance);
-  EXPECT_EQ(pose_out.x, 2.0);
-  EXPECT_EQ(pose_out.y, -1.0);
-  EXPECT_EQ(pose_out.theta, pose.theta);
+  EXPECT_EQ(pose_out.position.x, 2.0);
+  EXPECT_EQ(pose_out.position.y, -1.0);
+  EXPECT_NEAR(tf2::getYaw(pose_out.orientation), 0.0, 1e-6);
+
+  pose.position.x = 2.0;
+  pose.position.y = -10.0;
+  tf2::Quaternion q2;
+  q2.setRPY(0, 0, 0.54);
+  pose.orientation = tf2::toMsg(q2);
 
-  pose.x = 2.0;
-  pose.y = -10.0;
-  pose.theta = 0.54;
   pose_out = dwb_critics::getForwardPose(pose, distance);
-  EXPECT_NEAR(pose_out.x, 2.8577, 0.01);
-  EXPECT_NEAR(pose_out.y, -9.4858, 0.01);
-  EXPECT_EQ(pose_out.theta, pose.theta);
+  EXPECT_NEAR(pose_out.position.x, 2.8577, 0.01);
+  EXPECT_NEAR(pose_out.position.y, -9.4858, 0.01);
+  EXPECT_NEAR(tf2::getYaw(pose_out.orientation), 0.54, 0.01);
 }
 
 int main(int argc, char ** argv)
diff --git a/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp
index 11eb9b5a7b3..b53687a269f 100644
--- a/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp
+++ b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp
@@ -41,6 +41,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "dwb_critics/obstacle_footprint.hpp"
 #include "dwb_core/exceptions.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 TEST(BaseObstacle, IsValidCost)
 {
@@ -81,41 +83,42 @@ TEST(BaseObstacle, ScorePose)
   costmap_ros->getCostmap()->setCost(0, 2, some_other_cost);
 
   // The pose is in "world" coordinates. The (default) resolution is 0.1 m.
-  geometry_msgs::msg::Pose2D pose;
-  pose.x = 0;
-  pose.y = 0;
+  geometry_msgs::msg::Pose pose;
 
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = 0;
-  pose.y = 0.15;
+  pose.position.x = 0;
+  pose.position.y = 0.15;
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.y = 0.25;
-  pose.x = 0.05;
+  pose.position.y = 0.25;
+  pose.position.x = 0.05;
   ASSERT_EQ(critic->scorePose(pose), some_other_cost);
 
   // The theta should not influence the cost
   for (int i = -50; i < 150; i++) {
-    pose.theta = (1.0 / 50) * i * M_PI;
+    double theta = (1.0 / 50) * i * M_PI;
+    tf2::Quaternion q;
+    q.setRPY(0, 0, theta);
+    pose.orientation = tf2::toMsg(q);
     ASSERT_EQ(critic->scorePose(pose), some_other_cost);
   }
 
   // Poses outside the map should throw an exception.
-  pose.x = 1.0;
-  pose.y = -0.1;
+  pose.position.x = 1.0;
+  pose.position.y = -0.1;
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = costmap_ros->getCostmap()->getSizeInMetersX() + 0.1;
-  pose.y = 1.0;
+  pose.position.x = costmap_ros->getCostmap()->getSizeInMetersX() + 0.1;
+  pose.position.y = 1.0;
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = 1.0;
-  pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + 0.1;
+  pose.position.x = 1.0;
+  pose.position.y = costmap_ros->getCostmap()->getSizeInMetersY() + 0.1;
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = -0.1;
-  pose.y = 1.0;
+  pose.position.x = -0.1;
+  pose.position.y = 1.0;
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 }
 
diff --git a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp
index d8cee366909..b8b75fbb88e 100644
--- a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp
+++ b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp
@@ -40,6 +40,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "dwb_critics/obstacle_footprint.hpp"
 #include "dwb_core/exceptions.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 class OpenObstacleFootprintCritic : public dwb_critics::ObstacleFootprintCritic
 {
@@ -101,16 +102,21 @@ TEST(ObstacleFootprint, GetOrientedFootprint)
   std::vector<geometry_msgs::msg::Point> footprint_before = getFootprint();
 
   std::vector<geometry_msgs::msg::Point> footprint_after;
-  geometry_msgs::msg::Pose2D pose;
-  pose.theta = theta;
+  geometry_msgs::msg::Pose pose;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
+
   footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before);
 
   for (uint i = 0; i < footprint_before.size(); i++) {
-    ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]);
-  }
+    geometry_msgs::msg::Point expected = rotate_origin(footprint_before[i], theta);
+    geometry_msgs::msg::Point actual = footprint_after[i];
 
+    ASSERT_NEAR(expected.x, actual.x, 0.000001);
+    ASSERT_NEAR(expected.y, actual.y, 0.000001);
+    ASSERT_NEAR(expected.z, actual.z, 0.000001);
+  }
   theta = 5.123;
-  pose.theta = theta;
+  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
   footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before);
 
   for (unsigned int i = 0; i < footprint_before.size(); i++) {
@@ -134,10 +140,10 @@ TEST(ObstacleFootprint, Prepare)
   std::string name = "name";
   critic->initialize(node, name, ns, costmap_ros);
 
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   nav_2d_msgs::msg::Twist2D vel;
-  geometry_msgs::msg::Pose2D goal;
-  nav_2d_msgs::msg::Path2D global_plan;
+  geometry_msgs::msg::Pose goal;
+  nav_msgs::msg::Path global_plan;
 
   // no footprint set in the costmap. Prepare should return false;
   std::vector<geometry_msgs::msg::Point> footprint;
@@ -151,24 +157,28 @@ TEST(ObstacleFootprint, Prepare)
   // If the robot footprint goes of the map, it should throw an exception
   // The following cases put the robot over the edge of the map on the left, bottom, right and top
 
-  pose.x = footprint_size_x_half;  // This gives an error
-  pose.y = footprint_size_y_half + epsilon;
+  pose.position.x = footprint_size_x_half;  // This gives an error
+  pose.position.y = footprint_size_y_half + epsilon;
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = footprint_size_x_half + epsilon;
-  pose.y = footprint_size_y_half;  // error
+  pose.position.x = footprint_size_x_half + epsilon;
+  pose.position.y = footprint_size_y_half;  // error
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half;  // error
-  pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half - epsilon;
+  pose.position.x = costmap_ros->getCostmap()->getSizeInMetersX() -
+    footprint_size_x_half;  // error
+  pose.position.y = costmap_ros->getCostmap()->getSizeInMetersY() +
+    footprint_size_y_half - epsilon;
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half - epsilon;
-  pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half;  // error
+  pose.position.x = costmap_ros->getCostmap()->getSizeInMetersX() -
+    footprint_size_x_half - epsilon;
+  pose.position.y = costmap_ros->getCostmap()->getSizeInMetersY() +
+    footprint_size_y_half;  // error
   ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
 
-  pose.x = footprint_size_x_half + epsilon;
-  pose.y = footprint_size_y_half + epsilon;
+  pose.position.x = footprint_size_x_half + epsilon;
+  pose.position.y = footprint_size_y_half + epsilon;
   ASSERT_EQ(critic->scorePose(pose), 0.0);
 
   for (unsigned int i = 1; i < costmap_ros->getCostmap()->getSizeInCellsX(); i++) {
diff --git a/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp b/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp
index 6ccf2c18a02..5b763a6b877 100644
--- a/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp
+++ b/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp
@@ -45,6 +45,7 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 static constexpr double default_penalty = 1.0;
 static constexpr double default_strafe_x = 0.1;
diff --git a/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp b/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp
index 45d147f8e60..d8b7f386e6c 100644
--- a/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp
+++ b/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp
@@ -40,6 +40,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "dwb_critics/twirling.hpp"
 #include "dwb_core/exceptions.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 TEST(TwirlingTests, Scoring)
 {
diff --git a/nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg b/nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg
index 5afd820ad6e..fb6c86eec5d 100644
--- a/nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg
+++ b/nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg
@@ -5,4 +5,4 @@ nav_2d_msgs/Twist2D velocity
 # Time difference between first and last poses
 builtin_interfaces/Duration[] time_offsets
 # Poses the robot will go to, given our kinematic model
-geometry_msgs/Pose2D[] poses
+geometry_msgs/Pose[] poses
diff --git a/nav2_dwb_controller/dwb_msgs/srv/DebugLocalPlan.srv b/nav2_dwb_controller/dwb_msgs/srv/DebugLocalPlan.srv
index ca0ed51f356..cc0b2644f04 100644
--- a/nav2_dwb_controller/dwb_msgs/srv/DebugLocalPlan.srv
+++ b/nav2_dwb_controller/dwb_msgs/srv/DebugLocalPlan.srv
@@ -1,6 +1,6 @@
 # For a given pose velocity and global_plan, run the local planner and return full results
-nav_2d_msgs/Pose2DStamped pose
+geometry_msgs/PoseStamped pose
 nav_2d_msgs/Twist2D velocity
-nav_2d_msgs/Path2D global_plan
+nav_msgs/Path global_plan
 ---
 LocalPlanEvaluation results
diff --git a/nav2_dwb_controller/dwb_msgs/srv/GenerateTrajectory.srv b/nav2_dwb_controller/dwb_msgs/srv/GenerateTrajectory.srv
index 330aec504da..5282876c9de 100644
--- a/nav2_dwb_controller/dwb_msgs/srv/GenerateTrajectory.srv
+++ b/nav2_dwb_controller/dwb_msgs/srv/GenerateTrajectory.srv
@@ -1,5 +1,5 @@
 # For a given start pose, velocity and desired velocity, generate which poses will be visited
-geometry_msgs/Pose2D start_pose
+geometry_msgs/Pose start_pose
 nav_2d_msgs/Twist2D start_vel
 nav_2d_msgs/Twist2D cmd_vel
 ---
diff --git a/nav2_dwb_controller/dwb_msgs/srv/GetCriticScore.srv b/nav2_dwb_controller/dwb_msgs/srv/GetCriticScore.srv
index eae55edd876..2ec42960d48 100644
--- a/nav2_dwb_controller/dwb_msgs/srv/GetCriticScore.srv
+++ b/nav2_dwb_controller/dwb_msgs/srv/GetCriticScore.srv
@@ -1,6 +1,6 @@
-nav_2d_msgs/Pose2DStamped pose
+geometry_msgs/PoseStamped pose
 nav_2d_msgs/Twist2D velocity
-nav_2d_msgs/Path2D global_plan
+nav_msgs/Path global_plan
 Trajectory2D traj
 string critic_name
 ---
diff --git a/nav2_dwb_controller/dwb_msgs/srv/ScoreTrajectory.srv b/nav2_dwb_controller/dwb_msgs/srv/ScoreTrajectory.srv
index 1a61359df8a..6736e31fd19 100644
--- a/nav2_dwb_controller/dwb_msgs/srv/ScoreTrajectory.srv
+++ b/nav2_dwb_controller/dwb_msgs/srv/ScoreTrajectory.srv
@@ -1,6 +1,6 @@
-nav_2d_msgs/Pose2DStamped pose
+geometry_msgs/PoseStamped pose
 nav_2d_msgs/Twist2D velocity
-nav_2d_msgs/Path2D global_plan
+nav_msgs/Path global_plan
 Trajectory2D traj
 ---
 TrajectoryScore score
diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
index 5624288e251..f725aa2cf94 100644
--- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
@@ -24,13 +24,13 @@ add_library(standard_traj_generator SHARED
 target_include_directories(standard_traj_generator PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(standard_traj_generator PUBLIC
   dwb_core::dwb_core
   ${dwb_msgs_TARGETS}
   ${geometry_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   ${nav_2d_msgs_TARGETS}
   rclcpp::rclcpp
   ${rcl_interfaces_TARGETS}
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
index 4b2ce5828cd..0a9d84c337a 100644
--- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
+++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
@@ -64,7 +64,7 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
   nav_2d_msgs::msg::Twist2D nextTwist() override;
 
   dwb_msgs::msg::Trajectory2D generateTrajectory(
-    const geometry_msgs::msg::Pose2D & start_pose,
+    const geometry_msgs::msg::Pose & start_pose,
     const nav_2d_msgs::msg::Twist2D & start_vel,
     const nav_2d_msgs::msg::Twist2D & cmd_vel) override;
 
@@ -108,8 +108,8 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
    * @param dt amount of time in seconds
    * @return New pose after dt seconds
    */
-  virtual geometry_msgs::msg::Pose2D computeNewPosition(
-    const geometry_msgs::msg::Pose2D start_pose, const nav_2d_msgs::msg::Twist2D & vel,
+  virtual geometry_msgs::msg::Pose computeNewPosition(
+    const geometry_msgs::msg::Pose start_pose, const nav_2d_msgs::msg::Twist2D & vel,
     const double dt);
 
 
diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
index 09024d3fbae..8e68a399f08 100644
--- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
@@ -42,6 +42,8 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 namespace dwb_plugins
 {
@@ -149,14 +151,14 @@ std::vector<double> StandardTrajectoryGenerator::getTimeSteps(
 }
 
 dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
-  const geometry_msgs::msg::Pose2D & start_pose,
+  const geometry_msgs::msg::Pose & start_pose,
   const nav_2d_msgs::msg::Twist2D & start_vel,
   const nav_2d_msgs::msg::Twist2D & cmd_vel)
 {
   dwb_msgs::msg::Trajectory2D traj;
   traj.velocity = cmd_vel;
   //  simulate the trajectory
-  geometry_msgs::msg::Pose2D pose = start_pose;
+  geometry_msgs::msg::Pose pose = start_pose;
   nav_2d_msgs::msg::Twist2D vel = start_vel;
   double running_time = 0.0;
   std::vector<double> steps = getTimeSteps(cmd_vel);
@@ -208,16 +210,23 @@ nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::computeNewVelocity(
   return new_vel;
 }
 
-geometry_msgs::msg::Pose2D StandardTrajectoryGenerator::computeNewPosition(
-  const geometry_msgs::msg::Pose2D start_pose,
+geometry_msgs::msg::Pose StandardTrajectoryGenerator::computeNewPosition(
+  const geometry_msgs::msg::Pose start_pose,
   const nav_2d_msgs::msg::Twist2D & vel, const double dt)
 {
-  geometry_msgs::msg::Pose2D new_pose;
-  new_pose.x = start_pose.x +
-    (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
-  new_pose.y = start_pose.y +
-    (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
-  new_pose.theta = start_pose.theta + vel.theta * dt;
+  geometry_msgs::msg::Pose new_pose;
+
+  double theta = tf2::getYaw(start_pose.orientation);
+  new_pose.position.x = start_pose.position.x +
+    (vel.x * cos(theta) + vel.y * cos(M_PI_2 + theta)) * dt;
+  new_pose.position.y = start_pose.position.y +
+    (vel.x * sin(theta) + vel.y * sin(M_PI_2 + theta)) * dt;
+
+  double new_theta = theta + vel.theta * dt;
+  tf2::Quaternion q;
+  q.setRPY(0.0, 0.0, new_theta);
+  new_pose.orientation = tf2::toMsg(q);
+
   return new_pose;
 }
 
diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
index 6ac3c14cffd..84263f53d06 100644
--- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
+++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
@@ -42,12 +42,14 @@
 #include "dwb_plugins/limited_accel_generator.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 using std::hypot;
 using std::fabs;
 using dwb_plugins::StandardTrajectoryGenerator;
 
-geometry_msgs::msg::Pose2D origin;
+geometry_msgs::msg::Pose origin;
 nav_2d_msgs::msg::Twist2D zero;
 nav_2d_msgs::msg::Twist2D forward;
 
@@ -293,20 +295,20 @@ TEST(VelocityIterator, nonzero)
     0.24622144504490268, 0.0, 0.1);
 }
 
-void matchPose(const geometry_msgs::msg::Pose2D & a, const geometry_msgs::msg::Pose2D & b)
+void matchPose(const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b)
 {
-  EXPECT_DOUBLE_EQ(a.x, b.x);
-  EXPECT_DOUBLE_EQ(a.y, b.y);
-  EXPECT_DOUBLE_EQ(a.theta, b.theta);
+  EXPECT_DOUBLE_EQ(a.position.x, b.position.x);
+  EXPECT_DOUBLE_EQ(a.position.y, b.position.y);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(a.orientation), tf2::getYaw(b.orientation));
 }
 
 void matchPose(
-  const geometry_msgs::msg::Pose2D & a, const double x, const double y,
+  const geometry_msgs::msg::Pose & a, const double x, const double y,
   const double theta)
 {
-  EXPECT_DOUBLE_EQ(a.x, x);
-  EXPECT_DOUBLE_EQ(a.y, y);
-  EXPECT_DOUBLE_EQ(a.theta, theta);
+  EXPECT_DOUBLE_EQ(a.position.x, x);
+  EXPECT_DOUBLE_EQ(a.position.y, y);
+  EXPECT_DOUBLE_EQ(tf2::getYaw(a.orientation), theta);
 }
 
 void matchTwist(const nav_2d_msgs::msg::Twist2D & a, const nav_2d_msgs::msg::Twist2D & b)
diff --git a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
index f1054e1a024..c2838963c9b 100644
--- a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
@@ -6,9 +6,7 @@ find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(nav_2d_msgs
-    "msg/Path2D.msg"
     "msg/Pose2D32.msg"
-    "msg/Pose2DStamped.msg"
     "msg/Twist2D.msg"
     "msg/Twist2D32.msg"
     "msg/Twist2DStamped.msg"
diff --git a/nav2_dwb_controller/nav_2d_msgs/msg/Path2D.msg b/nav2_dwb_controller/nav_2d_msgs/msg/Path2D.msg
deleted file mode 100644
index 9cfcace1a5c..00000000000
--- a/nav2_dwb_controller/nav_2d_msgs/msg/Path2D.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-std_msgs/Header header
-geometry_msgs/Pose2D[] poses
diff --git a/nav2_dwb_controller/nav_2d_msgs/msg/Pose2DStamped.msg b/nav2_dwb_controller/nav_2d_msgs/msg/Pose2DStamped.msg
deleted file mode 100644
index 904e504d6ec..00000000000
--- a/nav2_dwb_controller/nav_2d_msgs/msg/Pose2DStamped.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-std_msgs/Header header
-geometry_msgs/Pose2D pose
diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml
index 5f5ed5f97bc..2f422cc95da 100644
--- a/nav2_dwb_controller/nav_2d_msgs/package.xml
+++ b/nav2_dwb_controller/nav_2d_msgs/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
   <name>nav_2d_msgs</name>
   <version>1.4.0</version>
-  <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
+  <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose.</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>
 
diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
index a6aefd38e77..8a729af46a9 100644
--- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
@@ -24,7 +24,6 @@ target_include_directories(conversions
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(conversions PUBLIC
   ${geometry_msgs_TARGETS}
@@ -45,10 +44,12 @@ target_include_directories(path_ops
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(path_ops PUBLIC
   ${nav_2d_msgs_TARGETS}
+  rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
+  nav2_util::nav2_util_core
 )
 target_link_libraries(path_ops PRIVATE
   ${geometry_msgs_TARGETS}
@@ -61,13 +62,13 @@ target_include_directories(tf_help
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(tf_help PUBLIC
   conversions
   ${geometry_msgs_TARGETS}
   ${nav_2d_msgs_TARGETS}
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
   tf2::tf2
   tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp
index 901e9f0a1e5..35f3ec2018f 100644
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp
+++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp
@@ -40,8 +40,7 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_2d_msgs/msg/twist2_d.hpp"
-#include "nav_2d_msgs/msg/path2_d.hpp"
-#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/convert.hpp"
@@ -50,21 +49,13 @@ namespace nav_2d_utils
 {
 geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d);
 nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel);
-// nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D(const tf2::Stamped<tf2::Pose>& pose);
-nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D(const geometry_msgs::msg::PoseStamped & pose);
-geometry_msgs::msg::Pose2D poseToPose2D(const geometry_msgs::msg::Pose & pose);
-geometry_msgs::msg::Pose pose2DToPose(const geometry_msgs::msg::Pose2D & pose2d);
-geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
-  const nav_2d_msgs::msg::Pose2DStamped & pose2d);
-geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
-  const geometry_msgs::msg::Pose2D & pose2d,
+geometry_msgs::msg::PoseStamped poseToPoseStamped(
+  const geometry_msgs::msg::Pose & pose,
   const std::string & frame, const rclcpp::Time & stamp);
 nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStamped> & poses);
-nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path);
-nav_msgs::msg::Path poses2DToPath(
-  const std::vector<geometry_msgs::msg::Pose2D> & poses,
+nav_msgs::msg::Path posesToPath(
+  const std::vector<geometry_msgs::msg::Pose> & poses,
   const std::string & frame, const rclcpp::Time & stamp);
-nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d);
 
 }  // namespace nav_2d_utils
 
diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/path_ops.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/path_ops.hpp
index 86f5ccfd12e..7acbf5018d7 100644
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/path_ops.hpp
+++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/path_ops.hpp
@@ -35,7 +35,7 @@
 #ifndef NAV_2D_UTILS__PATH_OPS_HPP_
 #define NAV_2D_UTILS__PATH_OPS_HPP_
 
-#include "nav_2d_msgs/msg/path2_d.hpp"
+#include "nav_msgs/msg/path.hpp"
 
 namespace nav_2d_utils
 {
@@ -46,8 +46,8 @@ namespace nav_2d_utils
  * @param resolution desired distance between waypoints
  * @return Higher resolution plan
  */
-nav_2d_msgs::msg::Path2D adjustPlanResolution(
-  const nav_2d_msgs::msg::Path2D & global_plan_in,
+nav_msgs::msg::Path adjustPlanResolution(
+  const nav_msgs::msg::Path & global_plan_in,
   double resolution);
 }  // namespace nav_2d_utils
 
diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp
index 442eb36797e..9bed6cd6810 100644
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp
+++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp
@@ -40,7 +40,6 @@
 #include "tf2_ros/buffer.h"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 namespace nav_2d_utils
@@ -64,7 +63,7 @@ bool transformPose(
 );
 
 /**
- * @brief Transform a Pose2DStamped from one frame to another while catching exceptions
+ * @brief Transform a PoseStamped from one frame to another while catching exceptions
  *
  * Also returns immediately if the frames are equal.
  * @param tf Smart pointer to TFListener
@@ -76,8 +75,8 @@ bool transformPose(
 bool transformPose(
   const std::shared_ptr<tf2_ros::Buffer> tf,
   const std::string frame,
-  const nav_2d_msgs::msg::Pose2DStamped & in_pose,
-  nav_2d_msgs::msg::Pose2DStamped & out_pose,
+  const geometry_msgs::msg::PoseStamped & in_pose,
+  geometry_msgs::msg::PoseStamped & out_pose,
   rclcpp::Duration & transform_tolerance
 );
 
diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
index 5abed18bdcb..4ec9a73fb99 100644
--- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
@@ -38,13 +38,10 @@
 #include <string>
 
 #include "geometry_msgs/msg/pose.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav_2d_msgs/msg/twist2_d.hpp"
-#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
-#include "nav_2d_msgs/msg/path2_d.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -75,64 +72,15 @@ nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel)
   return cmd_vel_2d;
 }
 
-// nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D(const tf2::Stamped<tf2::Pose>& pose)
-// {
-//   nav_2d_msgs::msg::Pose2DStamped pose2d;
-//   pose2d.header.stamp = pose.stamp_;
-//   pose2d.header.frame_id = pose.frame_id_;
-//   pose2d.pose.x = pose.getOrigin().getX();
-//   pose2d.pose.y = pose.getOrigin().getY();
-//   pose2d.pose.theta = tf::getYaw(pose.getRotation());
-//   return pose2d;
-// }
 
-nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D(const geometry_msgs::msg::PoseStamped & pose)
-{
-  nav_2d_msgs::msg::Pose2DStamped pose2d;
-  pose2d.header = pose.header;
-  pose2d.pose.x = pose.pose.position.x;
-  pose2d.pose.y = pose.pose.position.y;
-  pose2d.pose.theta = tf2::getYaw(pose.pose.orientation);
-  return pose2d;
-}
-
-geometry_msgs::msg::Pose2D poseToPose2D(const geometry_msgs::msg::Pose & pose)
-{
-  geometry_msgs::msg::Pose2D pose2d;
-  pose2d.x = pose.position.x;
-  pose2d.y = pose.position.y;
-  pose2d.theta = tf2::getYaw(pose.orientation);
-  return pose2d;
-}
-
-geometry_msgs::msg::Pose pose2DToPose(const geometry_msgs::msg::Pose2D & pose2d)
-{
-  geometry_msgs::msg::Pose pose;
-  pose.position.x = pose2d.x;
-  pose.position.y = pose2d.y;
-  pose.orientation = orientationAroundZAxis(pose2d.theta);
-  return pose;
-}
-
-geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
-  const nav_2d_msgs::msg::Pose2DStamped & pose2d)
-{
-  geometry_msgs::msg::PoseStamped pose;
-  pose.header = pose2d.header;
-  pose.pose = pose2DToPose(pose2d.pose);
-  return pose;
-}
-
-geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
-  const geometry_msgs::msg::Pose2D & pose2d,
+geometry_msgs::msg::PoseStamped poseToPoseStamped(
+  const geometry_msgs::msg::Pose & pose_in,
   const std::string & frame, const rclcpp::Time & stamp)
 {
   geometry_msgs::msg::PoseStamped pose;
   pose.header.frame_id = frame;
   pose.header.stamp = stamp;
-  pose.pose.position.x = pose2d.x;
-  pose.pose.position.y = pose2d.y;
-  pose.pose.orientation = orientationAroundZAxis(pose2d.theta);
+  pose.pose = pose_in;
   return pose;
 }
 
@@ -151,19 +99,9 @@ nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStampe
   return path;
 }
 
-nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path)
-{
-  nav_2d_msgs::msg::Path2D path2d;
-  path2d.header = path.header;
-  for (auto & pose : path.poses) {
-    path2d.poses.push_back(poseToPose2D(pose.pose));
-  }
-  return path2d;
-}
-
 
-nav_msgs::msg::Path poses2DToPath(
-  const std::vector<geometry_msgs::msg::Pose2D> & poses,
+nav_msgs::msg::Path posesToPath(
+  const std::vector<geometry_msgs::msg::Pose> & poses,
   const std::string & frame, const rclcpp::Time & stamp)
 {
   nav_msgs::msg::Path path;
@@ -171,19 +109,9 @@ nav_msgs::msg::Path poses2DToPath(
   path.header.frame_id = frame;
   path.header.stamp = stamp;
   for (unsigned int i = 0; i < poses.size(); i++) {
-    path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
-  }
-  return path;
-}
-
-nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d)
-{
-  nav_msgs::msg::Path path;
-  path.header = path2d.header;
-  path.poses.resize(path2d.poses.size());
-  for (unsigned int i = 0; i < path.poses.size(); i++) {
-    path.poses[i].header = path2d.header;
-    path.poses[i].pose = pose2DToPose(path2d.poses[i]);
+    path.poses[i].header.frame_id = frame;
+    path.poses[i].header.stamp = stamp;
+    path.poses[i].pose = poses[i];
   }
   return path;
 }
diff --git a/nav2_dwb_controller/nav_2d_utils/src/path_ops.cpp b/nav2_dwb_controller/nav_2d_utils/src/path_ops.cpp
index 5cf808b1793..b6146915495 100644
--- a/nav2_dwb_controller/nav_2d_utils/src/path_ops.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/src/path_ops.cpp
@@ -32,52 +32,62 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "nav_2d_utils/path_ops.hpp"
 #include <cmath>
 
+#include "nav_2d_utils/path_ops.hpp"
+#include "tf2/convert.hpp"
+#include "tf2/utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
 using std::sqrt;
 
 namespace nav_2d_utils
 {
-nav_2d_msgs::msg::Path2D adjustPlanResolution(
-  const nav_2d_msgs::msg::Path2D & global_plan_in,
+nav_msgs::msg::Path adjustPlanResolution(
+  const nav_msgs::msg::Path & global_plan_in,
   double resolution)
 {
-  nav_2d_msgs::msg::Path2D global_plan_out;
+  nav_msgs::msg::Path global_plan_out;
   if (global_plan_in.poses.size() == 0) {
     return global_plan_out;
   }
 
-  geometry_msgs::msg::Pose2D last = global_plan_in.poses[0];
+  geometry_msgs::msg::PoseStamped last = global_plan_in.poses[0];
   global_plan_out.poses.push_back(last);
 
   // we can take "holes" in the plan smaller than 2 grid cells (squared = 4)
   double min_sq_resolution = resolution * resolution * 4.0;
 
   for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i) {
-    geometry_msgs::msg::Pose2D loop = global_plan_in.poses[i];
-    double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y);
+    geometry_msgs::msg::PoseStamped loop = global_plan_in.poses[i];
+    double sq_dist = (loop.pose.position.x - last.pose.position.x) *
+      (loop.pose.position.x - last.pose.position.x) +
+      (loop.pose.position.y - last.pose.position.y) *
+      (loop.pose.position.y - last.pose.position.y);
     if (sq_dist > min_sq_resolution) {
       // add points in-between
       double diff = sqrt(sq_dist) - sqrt(min_sq_resolution);
       int steps = static_cast<int>(diff / resolution) - 1;
       double steps_double = static_cast<double>(steps);
 
-      double delta_x = (loop.x - last.x) / steps_double;
-      double delta_y = (loop.y - last.y) / steps_double;
-      double delta_t = (loop.theta - last.theta) / steps_double;
+      double theta_last = tf2::getYaw(last.pose.orientation);
+      double theta_loop = tf2::getYaw(loop.pose.orientation);
+      double delta_x = (loop.pose.position.x - last.pose.position.x) / steps_double;
+      double delta_y = (loop.pose.position.y - last.pose.position.y) / steps_double;
+      double delta_t = (theta_loop - theta_last) / steps_double;
 
       for (int j = 1; j < steps; ++j) {
-        geometry_msgs::msg::Pose2D pose;
-        pose.x = last.x + j * delta_x;
-        pose.y = last.y + j * delta_y;
-        pose.theta = last.theta + j * delta_t;
+        geometry_msgs::msg::PoseStamped pose;
+        pose.pose.position.x = last.pose.position.x + j * delta_x;
+        pose.pose.position.y = last.pose.position.y + j * delta_y;
+        pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+          theta_last + j * delta_t);
         global_plan_out.poses.push_back(pose);
       }
     }
     global_plan_out.poses.push_back(global_plan_in.poses[i]);
-    last.x = loop.x;
-    last.y = loop.y;
+    last.pose.position.x = loop.pose.position.x;
+    last.pose.position.y = loop.pose.position.y;
   }
   return global_plan_out;
 }
diff --git a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp
index ac4b7a6a4b0..0916cc6a402 100644
--- a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp
@@ -95,21 +95,4 @@ bool transformPose(
   return false;
 }
 
-bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> tf,
-  const std::string frame,
-  const nav_2d_msgs::msg::Pose2DStamped & in_pose,
-  nav_2d_msgs::msg::Pose2DStamped & out_pose,
-  rclcpp::Duration & transform_tolerance
-)
-{
-  geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
-  geometry_msgs::msg::PoseStamped out_3d_pose;
-
-  bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance);
-  if (ret) {
-    out_pose = poseStampedToPose2D(out_3d_pose);
-  }
-  return ret;
-}
 }  // namespace nav_2d_utils
diff --git a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp
index 2a0ae9c7689..c7f5c502f76 100644
--- a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp
@@ -36,6 +36,8 @@
 #include <vector>
 #include <tf2/LinearMath/Quaternion.hpp>
 
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
 #include "gtest/gtest.h"
 #include "nav_2d_utils/conversions.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
@@ -43,7 +45,6 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 using nav_2d_utils::posesToPath;
-using nav_2d_utils::pathToPath;
 
 TEST(nav_2d_utils, PosesToPathEmpty)
 {
@@ -102,50 +103,6 @@ TEST(nav_2d_utils, PosesToPathNonEmpty)
   EXPECT_EQ(path.header.stamp, time1);
 }
 
-TEST(nav_2d_utils, PathToPathEmpty)
-{
-  nav_2d_msgs::msg::Path2D path2d;
-  nav_msgs::msg::Path path = pathToPath(path2d);
-  EXPECT_EQ(path.poses.size(), 0ul);
-}
-
-TEST(nav_2d_utils, PathToPathNoNEmpty)
-{
-  nav_2d_msgs::msg::Path2D path2d;
-
-  geometry_msgs::msg::Pose2D pose1;
-  pose1.x = 1.0;
-  pose1.y = 2.0;
-  pose1.theta = M_PI / 2.0;
-
-  geometry_msgs::msg::Pose2D pose2;
-  pose2.x = 4.0;
-  pose2.y = 5.0;
-  pose2.theta = M_PI;
-
-  path2d.poses.push_back(pose1);
-  path2d.poses.push_back(pose2);
-
-  nav_msgs::msg::Path path = pathToPath(path2d);
-  EXPECT_EQ(path.poses.size(), 2ul);
-  EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
-  EXPECT_EQ(path.poses[0].pose.position.y, 2.0);
-
-  tf2::Quaternion quat;
-  quat.setRPY(0, 0, M_PI / 2.0);
-  EXPECT_EQ(path.poses[0].pose.orientation.w, quat.w());
-  EXPECT_EQ(path.poses[0].pose.orientation.x, quat.x());
-  EXPECT_EQ(path.poses[0].pose.orientation.y, quat.x());
-  EXPECT_EQ(path.poses[0].pose.orientation.z, quat.z());
-
-  EXPECT_EQ(path.poses[1].pose.position.x, 4.0);
-  EXPECT_EQ(path.poses[1].pose.position.y, 5.0);
-  quat.setRPY(0, 0, M_PI);
-  EXPECT_EQ(path.poses[1].pose.orientation.w, quat.w());
-  EXPECT_EQ(path.poses[1].pose.orientation.x, quat.x());
-  EXPECT_EQ(path.poses[1].pose.orientation.y, quat.x());
-  EXPECT_EQ(path.poses[1].pose.orientation.z, quat.z());
-}
 
 int main(int argc, char ** argv)
 {
diff --git a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt
index 5faa7c8daec..f0cf339a322 100644
--- a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt
@@ -1,10 +1,12 @@
 ament_add_gtest(2d_utils_tests 2d_utils_test.cpp)
-target_link_libraries(2d_utils_tests conversions)
+target_link_libraries(2d_utils_tests
+  conversions
+  nav2_ros_common::nav2_ros_common
+)
 target_include_directories(2d_utils_tests
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 ament_add_gtest(path_ops_tests path_ops_test.cpp)
diff --git a/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp
index e04abd7d400..bef452e1a73 100644
--- a/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp
@@ -41,27 +41,27 @@ using nav_2d_utils::adjustPlanResolution;
 
 TEST(path_ops_test, AdjustResolutionEmpty)
 {
-  nav_2d_msgs::msg::Path2D in;
-  nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, 2.0);
+  nav_msgs::msg::Path in;
+  nav_msgs::msg::Path out = adjustPlanResolution(in, 2.0);
   EXPECT_EQ(out.poses.size(), 0ul);
 }
 
 TEST(path_ops_test, AdjustResolutionSimple)
 {
-  nav_2d_msgs::msg::Path2D in;
+  nav_msgs::msg::Path in;
   const float RESOLUTION = 20.0;
 
-  geometry_msgs::msg::Pose2D pose1;
-  pose1.x = 0.0;
-  pose1.y = 0.0;
-  geometry_msgs::msg::Pose2D pose2;
-  pose2.x = 100.0;
-  pose2.y = 0.0;
+  geometry_msgs::msg::PoseStamped pose1;
+  pose1.pose.position.x = 0.0;
+  pose1.pose.position.y = 0.0;
+  geometry_msgs::msg::PoseStamped pose2;
+  pose2.pose.position.x = 100.0;
+  pose2.pose.position.y = 0.0;
 
   in.poses.push_back(pose1);
   in.poses.push_back(pose2);
 
-  nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, RESOLUTION);
+  nav_msgs::msg::Path out = adjustPlanResolution(in, RESOLUTION);
   float length = 100;
   uint32_t number_of_points = ceil(length / (2 * RESOLUTION));
   EXPECT_EQ(out.poses.size(), number_of_points);
@@ -71,8 +71,10 @@ TEST(path_ops_test, AdjustResolutionSimple)
     pose1 = out.poses[i - 1];
     pose2 = out.poses[i];
 
-    double sq_dist = (pose1.x - pose2.x) * (pose1.x - pose2.x) +
-      (pose1.y - pose2.y) * (pose1.y - pose2.y);
+    double sq_dist = (pose1.pose.position.x - pose2.pose.position.x) *
+      (pose1.pose.position.x - pose2.pose.position.x) +
+      (pose1.pose.position.y - pose2.pose.position.y) *
+      (pose1.pose.position.y - pose2.pose.position.y);
 
     EXPECT_TRUE(sqrt(sq_dist) <= max_length);
   }
diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt
index 3a18bfa62ac..1a72a698085 100644
--- a/nav2_graceful_controller/CMakeLists.txt
+++ b/nav2_graceful_controller/CMakeLists.txt
@@ -28,12 +28,12 @@ target_include_directories(smooth_control_law
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(smooth_control_law PUBLIC
   angles::angles
   ${geometry_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   tf2::tf2
 )
 
@@ -50,11 +50,11 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_util::nav2_util_core
   nav2_costmap_2d::nav2_costmap_2d_core
   nav_2d_utils::tf_help
diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt
index 48ebccbb84f..654b7153d06 100644
--- a/nav2_lifecycle_manager/CMakeLists.txt
+++ b/nav2_lifecycle_manager/CMakeLists.txt
@@ -23,17 +23,16 @@ add_library(${library_name} SHARED
   src/lifecycle_manager.cpp
   src/lifecycle_manager_client.cpp
 )
-message(STATUS "nav2_ros_common_INCLUDE_DIRS: ${nav2_ros_common_INCLUDE_DIRS}")
 target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   bondcpp::bondcpp
   diagnostic_updater::diagnostic_updater
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
   ${std_srvs_TARGETS}
@@ -51,11 +50,11 @@ target_include_directories(lifecycle_manager
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(lifecycle_manager PRIVATE
   ${library_name}
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
 )
 
 rclcpp_components_register_nodes(${library_name} "nav2_lifecycle_manager::LifecycleManager")
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
index d742a54a475..438e1aa12fb 100644
--- a/nav2_map_server/CMakeLists.txt
+++ b/nav2_map_server/CMakeLists.txt
@@ -39,14 +39,13 @@ add_library(${map_io_library_name} SHARED
 target_include_directories(${map_io_library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_include_directories(${map_io_library_name}
   PRIVATE
-  ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  ${GRAPHICSMAGICKCPP_INCLUDE_DIRS})
 target_link_libraries(${map_io_library_name} PUBLIC
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   ${nav_msgs_TARGETS}
 )
 target_link_libraries(${map_io_library_name} PRIVATE
@@ -62,13 +61,13 @@ add_library(${library_name} SHARED
 target_include_directories(${library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${library_name} PUBLIC
   ${map_io_library_name}
   ${nav_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
@@ -83,12 +82,12 @@ add_executable(${map_server_executable}
 target_include_directories(${map_server_executable}
   PRIVATE
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${map_server_executable} PRIVATE
   ${library_name}
   ${map_io_library_name}
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
 )
 
 add_executable(${map_saver_cli_executable}
@@ -96,14 +95,14 @@ add_executable(${map_saver_cli_executable}
 target_include_directories(${map_saver_cli_executable}
   PRIVATE
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${map_saver_cli_executable} PRIVATE
   ${library_name}
   ${map_io_library_name}
   ${nav_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
@@ -113,14 +112,14 @@ add_executable(${map_saver_server_executable}
 target_include_directories(${map_saver_server_executable}
   PRIVATE
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${map_saver_server_executable} PRIVATE
   ${library_name}
   ${map_io_library_name}
   ${nav_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
@@ -130,14 +129,14 @@ add_executable(${costmap_filter_info_server_executable}
 target_include_directories(${costmap_filter_info_server_executable}
   PRIVATE
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${costmap_filter_info_server_executable} PRIVATE
   ${library_name}
   ${map_io_library_name}
   ${nav_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index d38678171eb..c11fa166c99 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -50,12 +50,12 @@ target_compile_options(mppi_controller PUBLIC -O3)
 target_include_directories(mppi_controller
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(mppi_controller PUBLIC
   angles::angles
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::layers
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav_msgs_TARGETS}
@@ -86,12 +86,12 @@ target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
 target_include_directories(mppi_critics
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(mppi_critics PUBLIC
   angles::angles
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::layers
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav_msgs_TARGETS}
diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
index 07af0e2b19b..df0a924a40a 100644
--- a/nav2_navfn_planner/CMakeLists.txt
+++ b/nav2_navfn_planner/CMakeLists.txt
@@ -27,11 +27,11 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   nav2_util::nav2_util_core
   ${nav_msgs_TARGETS}
diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt
index 05e78bc0258..9c2e44e69b2 100644
--- a/nav2_planner/CMakeLists.txt
+++ b/nav2_planner/CMakeLists.txt
@@ -30,11 +30,11 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
index fd8f5fe8f69..2cd43c87154 100644
--- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
+++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
@@ -31,11 +31,11 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   nav2_util::nav2_util_core
   ${nav_msgs_TARGETS}
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp
index c002ee6cb96..4b835ecf345 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp
@@ -25,7 +25,7 @@
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_util/odometry_utils.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp"
 
 #include "nav2_core/controller_exceptions.hpp"
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
index fbab24cc8c7..cc8b7915330 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
@@ -27,7 +27,7 @@
 #include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_core/controller_exceptions.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 
 namespace nav2_regulated_pure_pursuit_controller
 {
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
index 42ed663d9b3..2eb901dc11f 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
@@ -26,7 +26,7 @@
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "std_msgs/msg/bool.hpp"
 #include "nav2_regulated_pure_pursuit_controller/path_handler.hpp"
 #include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp"
diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp
index 83558bcf4b4..c2cd9552dfa 100644
--- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp
@@ -86,10 +86,8 @@ bool CollisionChecker::isCollisionImminent(
   }
 
   const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position;
-  geometry_msgs::msg::Pose2D curr_pose;
-  curr_pose.x = robot_pose.pose.position.x;
-  curr_pose.y = robot_pose.pose.position.y;
-  curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation);
+  geometry_msgs::msg::Pose curr_pose;
+  curr_pose = robot_pose.pose;
 
   // only forward simulate within time requested
   double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot;
@@ -104,24 +102,27 @@ bool CollisionChecker::isCollisionImminent(
   while (i * projection_time < max_allowed_time_to_collision_check) {
     i++;
 
+    double theta = tf2::getYaw(curr_pose.orientation);
+
     // apply velocity at curr_pose over distance
-    curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta));
-    curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta));
-    curr_pose.theta += projection_time * angular_vel;
+    curr_pose.position.x += projection_time * (linear_vel * cos(theta));
+    curr_pose.position.y += projection_time * (linear_vel * sin(theta));
+    theta += projection_time * angular_vel;
+    curr_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
 
     // check if past carrot pose, where no longer a thoughtfully valid command
-    if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) {
+    if (hypot(curr_pose.position.x - robot_xy.x, curr_pose.position.y - robot_xy.y) > carrot_dist) {
       break;
     }
 
     // store it for visualization
-    pose_msg.pose.position.x = curr_pose.x;
-    pose_msg.pose.position.y = curr_pose.y;
+    pose_msg.pose.position.x = curr_pose.position.x;
+    pose_msg.pose.position.y = curr_pose.position.y;
     pose_msg.pose.position.z = 0.01;
     arc_pts_msg.poses.push_back(pose_msg);
 
     // check for collision at the projected pose
-    if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) {
+    if (inCollision(curr_pose.position.x, curr_pose.position.y, theta)) {
       carrot_arc_pub_->publish(arc_pts_msg);
       return true;
     }
diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt
index e7f40238000..c8140e661a6 100644
--- a/nav2_rotation_shim_controller/CMakeLists.txt
+++ b/nav2_rotation_shim_controller/CMakeLists.txt
@@ -28,12 +28,12 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_controller::position_goal_checker
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   pluginlib::pluginlib
   rclcpp::rclcpp
diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt
index 47d29fbf87e..3ceb96f6753 100644
--- a/nav2_route/CMakeLists.txt
+++ b/nav2_route/CMakeLists.txt
@@ -48,10 +48,10 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
@@ -97,10 +97,10 @@ target_include_directories(edge_scorers
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(edge_scorers PUBLIC
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
@@ -135,10 +135,10 @@ target_include_directories(route_operations
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(route_operations PUBLIC
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
@@ -168,10 +168,10 @@ target_include_directories(graph_file_loaders
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(graph_file_loaders PUBLIC
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
@@ -200,10 +200,10 @@ target_include_directories(graph_file_savers
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(graph_file_savers PUBLIC
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson
index 2e6a43af148..4c4fb3208ff 100644
--- a/nav2_route/graphs/aws_graph.geojson
+++ b/nav2_route/graphs/aws_graph.geojson
@@ -1,132 +1,1664 @@
 {
-"type": "FeatureCollection",
-"name": "graph",
-"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } },
-"date_generated": "Wed Feb 22 05:41:45 PM EST 2025",
-"features": [
-{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } },
-{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } },
-{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } },
-{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } },
-{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } },
-{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } },
-{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } },
-{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } },
-{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } },
-{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } },
-{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } },
-{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } },
-{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } },
-{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } },
-{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } },
-{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } },
-{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } },
-{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } },
-{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } },
-{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } },
-{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } },
-{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } },
-{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } },
-{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } },
-{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } },
-{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } },
-{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } },
-{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } },
-{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } },
-{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } },
-{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } },
-{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } },
-{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } },
-{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } },
-{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } },
-{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } },
-{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } },
-{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } },
-{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } },
-{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } },
-{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } },
-{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } }
-]
+    "crs": {
+        "properties": {
+            "name": "urn:ogc:def:crs:EPSG::3857"
+        },
+        "type": "name"
+    },
+    "features": [
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    -9.350000381469727
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 0
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    -3.3499999046325684
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 1
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    0.6499999761581421
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 2
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    5.650000095367432
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 3
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    3.6500000953674316
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 4
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    6.650000095367432
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 5
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    8.649999618530273
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 6
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    -7.349999904632568
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 7
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    1.649999976158142
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 8
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    -0.3499999940395355
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 9
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    -2.3499999046325684
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 10
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    -4.349999904632568
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 11
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    -5.872222900390625
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 12
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    4.0,
+                    -5.872222900390625
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 13
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    6.0,
+                    -5.872222900390625
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 14
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    -7.763441562652588
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 15
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    4.0,
+                    -7.874690055847168
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 16
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    6.0,
+                    -7.874690055847168
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 17
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    0.0,
+                    -8.690509796142578
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 18
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -2.0,
+                    -9.395081520080566
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 19
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    3.6500000953674316
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 20
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    1.0,
+                    6.650000095367432
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 21
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    6.650000095367432
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 22
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    4.650000095367432
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 23
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    2.6500000953674316
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 24
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    -0.3499999940395355
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 25
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    -2.3499999046325684
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 26
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    -4.349999904632568
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 27
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    -6.349999904632568
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 28
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -6.0,
+                    -9.350000381469727
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 29
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -2.0,
+                    -0.3499999940395355
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 30
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -2.0,
+                    -2.3499999046325684
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 31
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -2.0,
+                    -4.349999904632568
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 32
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    -4.0,
+                    -5.349999904632568
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 33
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    4.0,
+                    -2.3499999046325684
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 34
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    6.0,
+                    -2.3499999046325684
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 35
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    6.0,
+                    -0.3499999940395355
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 36
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    4.0,
+                    -0.3499999940395355
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 37
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    4.0,
+                    1.649999976158142
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 38
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "coordinates": [
+                    6.0,
+                    1.649999976158142
+                ],
+                "type": "Point"
+            },
+            "properties": {
+                "frame": "map",
+                "id": 39
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 7,
+                "id": 75,
+                "overridable": true,
+                "startid": 0
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 19,
+                "id": 92,
+                "overridable": true,
+                "startid": 0
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 2,
+                "id": 78,
+                "overridable": true,
+                "startid": 1
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 33,
+                "id": 89,
+                "overridable": true,
+                "startid": 1
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 26,
+                "id": 102,
+                "overridable": true,
+                "startid": 1
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 31,
+                "id": 119,
+                "overridable": true,
+                "startid": 1
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 32,
+                "id": 120,
+                "overridable": true,
+                "startid": 1
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 4,
+                "id": 79,
+                "overridable": true,
+                "startid": 2
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 1,
+                "id": 88,
+                "overridable": true,
+                "startid": 2
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 24,
+                "id": 99,
+                "overridable": true,
+                "startid": 2
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 30,
+                "id": 117,
+                "overridable": true,
+                "startid": 2
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 31,
+                "id": 118,
+                "overridable": true,
+                "startid": 2
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 5,
+                "id": 81,
+                "overridable": true,
+                "startid": 3
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 4,
+                "id": 86,
+                "overridable": true,
+                "startid": 3
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 3,
+                "id": 80,
+                "overridable": true,
+                "startid": 4
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 2,
+                "id": 87,
+                "overridable": true,
+                "startid": 4
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 20,
+                "id": 122,
+                "overridable": true,
+                "startid": 4
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 6,
+                "id": 82,
+                "overridable": true,
+                "startid": 5
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 21,
+                "id": 83,
+                "overridable": true,
+                "startid": 5
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 3,
+                "id": 85,
+                "overridable": true,
+                "startid": 5
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 22,
+                "id": 96,
+                "overridable": true,
+                "startid": 5
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 5,
+                "id": 84,
+                "overridable": true,
+                "startid": 6
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 33,
+                "id": 76,
+                "overridable": true,
+                "startid": 7
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 0,
+                "id": 91,
+                "overridable": true,
+                "startid": 7
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 28,
+                "id": 105,
+                "overridable": true,
+                "startid": 7
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 9,
+                "id": 42,
+                "overridable": true,
+                "startid": 8
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 20,
+                "id": 52,
+                "overridable": true,
+                "startid": 8
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 38,
+                "id": 54,
+                "overridable": true,
+                "startid": 8
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 10,
+                "id": 43,
+                "overridable": true,
+                "startid": 9
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 8,
+                "id": 51,
+                "overridable": true,
+                "startid": 9
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 30,
+                "id": 115,
+                "overridable": true,
+                "startid": 9
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 11,
+                "id": 44,
+                "overridable": true,
+                "startid": 10
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 9,
+                "id": 50,
+                "overridable": true,
+                "startid": 10
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 34,
+                "id": 59,
+                "overridable": true,
+                "startid": 10
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 31,
+                "id": 111,
+                "overridable": true,
+                "startid": 10
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 12,
+                "id": 45,
+                "overridable": true,
+                "startid": 11
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 10,
+                "id": 49,
+                "overridable": true,
+                "startid": 11
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 32,
+                "id": 108,
+                "overridable": true,
+                "startid": 11
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 15,
+                "id": 46,
+                "overridable": true,
+                "startid": 12
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 11,
+                "id": 48,
+                "overridable": true,
+                "startid": 12
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 12,
+                "id": 63,
+                "overridable": true,
+                "startid": 13
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 16,
+                "id": 71,
+                "overridable": true,
+                "startid": 13
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 13,
+                "id": 62,
+                "overridable": true,
+                "startid": 14
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 17,
+                "id": 64,
+                "overridable": true,
+                "startid": 14
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 12,
+                "id": 47,
+                "overridable": true,
+                "startid": 15
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 18,
+                "id": 72,
+                "overridable": true,
+                "startid": 15
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 15,
+                "id": 66,
+                "overridable": true,
+                "startid": 16
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 16,
+                "id": 65,
+                "overridable": true,
+                "startid": 17
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 19,
+                "id": 73,
+                "overridable": true,
+                "startid": 18
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 15,
+                "id": 94,
+                "overridable": true,
+                "startid": 18
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 0,
+                "id": 74,
+                "overridable": true,
+                "startid": 19
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 18,
+                "id": 93,
+                "overridable": true,
+                "startid": 19
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 8,
+                "id": 41,
+                "overridable": true,
+                "startid": 20
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 21,
+                "id": 53,
+                "overridable": true,
+                "startid": 20
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 4,
+                "id": 123,
+                "overridable": true,
+                "startid": 20
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 20,
+                "id": 40,
+                "overridable": true,
+                "startid": 21
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 5,
+                "id": 95,
+                "overridable": true,
+                "startid": 21
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 23,
+                "id": 97,
+                "overridable": true,
+                "startid": 22
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 4,
+                "id": 98,
+                "overridable": true,
+                "startid": 23
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 25,
+                "id": 100,
+                "overridable": true,
+                "startid": 24
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 2,
+                "id": 101,
+                "overridable": true,
+                "startid": 25
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 27,
+                "id": 103,
+                "overridable": true,
+                "startid": 26
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 33,
+                "id": 104,
+                "overridable": true,
+                "startid": 27
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 29,
+                "id": 106,
+                "overridable": true,
+                "startid": 28
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 7,
+                "id": 107,
+                "overridable": true,
+                "startid": 29
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 2,
+                "id": 116,
+                "overridable": true,
+                "startid": 30
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 31,
+                "id": 112,
+                "overridable": true,
+                "startid": 31
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 2,
+                "id": 113,
+                "overridable": true,
+                "startid": 31
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 1,
+                "id": 114,
+                "overridable": true,
+                "startid": 31
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 1,
+                "id": 109,
+                "overridable": true,
+                "startid": 32
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 33,
+                "id": 110,
+                "overridable": true,
+                "startid": 32
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 1,
+                "id": 77,
+                "overridable": true,
+                "startid": 33
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 7,
+                "id": 90,
+                "overridable": true,
+                "startid": 33
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 32,
+                "id": 121,
+                "overridable": true,
+                "startid": 33
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 35,
+                "id": 60,
+                "overridable": true,
+                "startid": 34
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 13,
+                "id": 70,
+                "overridable": true,
+                "startid": 34
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 14,
+                "id": 61,
+                "overridable": true,
+                "startid": 35
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 37,
+                "id": 57,
+                "overridable": true,
+                "startid": 36
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 35,
+                "id": 67,
+                "overridable": true,
+                "startid": 36
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 9,
+                "id": 58,
+                "overridable": true,
+                "startid": 37
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 34,
+                "id": 69,
+                "overridable": true,
+                "startid": 37
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 39,
+                "id": 55,
+                "overridable": true,
+                "startid": 38
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 37,
+                "id": 68,
+                "overridable": true,
+                "startid": 38
+            },
+            "type": "Feature"
+        },
+        {
+            "geometry": {
+                "type": "MultiLineString"
+            },
+            "properties": {
+                "cost": 0.0,
+                "endid": 36,
+                "id": 56,
+                "overridable": true,
+                "startid": 39
+            },
+            "type": "Feature"
+        }
+    ],
+    "name": "graph",
+    "type": "FeatureCollection"
 }
diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index 61b852a9708..958047a22c2 100644
--- a/nav2_rviz_plugins/CMakeLists.txt
+++ b/nav2_rviz_plugins/CMakeLists.txt
@@ -73,13 +73,13 @@ target_include_directories(${library_name} PUBLIC
   ${OGRE_INCLUDE_DIRS}
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_lifecycle_manager::nav2_lifecycle_manager_core
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   nav2_route::route_server_core
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
index d7dd3fc5459..e6780574604 100644
--- a/nav2_rviz_plugins/src/nav2_panel.cpp
+++ b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -34,7 +34,7 @@
 #include "rviz_common/load_resource.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
 #include "yaml-cpp/yaml.h"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 
 using namespace std::chrono_literals;
 
diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
index 906e37b19a0..a344a41aa0f 100644
--- a/nav2_smac_planner/CMakeLists.txt
+++ b/nav2_smac_planner/CMakeLists.txt
@@ -56,11 +56,11 @@ target_include_directories(${library_name}_common
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
     ${OMPL_INCLUDE_DIRS}
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name}_common PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::layers
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav_msgs_TARGETS}
@@ -84,12 +84,12 @@ target_include_directories(${library_name}
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
     ${OMPL_INCLUDE_DIRS}
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${library_name}_common
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav_msgs_TARGETS}
   nlohmann_json::nlohmann_json
@@ -114,12 +114,12 @@ target_include_directories(${library_name}_2d
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
     ${OMPL_INCLUDE_DIRS}
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name}_2d PUBLIC
   ${library_name}_common
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav_msgs_TARGETS}
   nlohmann_json::nlohmann_json
@@ -144,12 +144,12 @@ target_include_directories(${library_name}_lattice
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
     ${OMPL_INCLUDE_DIRS}
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name}_lattice PUBLIC
   ${library_name}_common
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   ${nav_msgs_TARGETS}
   nlohmann_json::nlohmann_json
diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
index 5ce209349fa..b2e3555ab2a 100644
--- a/nav2_smoother/CMakeLists.txt
+++ b/nav2_smoother/CMakeLists.txt
@@ -29,8 +29,7 @@ add_library(${library_name} SHARED
 target_include_directories(${library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${library_name} PUBLIC
   nav2_core::nav2_core
   nav2_costmap_2d::nav2_costmap_2d_client
@@ -58,8 +57,7 @@ add_executable(${executable_name}
 target_include_directories(${executable_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${executable_name} PRIVATE rclcpp::rclcpp ${library_name})
 
 # Simple Smoother plugin
@@ -69,8 +67,7 @@ add_library(simple_smoother SHARED
 target_include_directories(simple_smoother
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(simple_smoother PUBLIC
   angles::angles
   nav2_core::nav2_core
@@ -95,8 +92,7 @@ add_library(savitzky_golay_smoother SHARED
 target_include_directories(savitzky_golay_smoother
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(savitzky_golay_smoother PUBLIC
   angles::angles
   nav2_core::nav2_core
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
index 2c916d97e2c..4408b5a43e0 100644
--- a/nav2_smoother/src/nav2_smoother.cpp
+++ b/nav2_smoother/src/nav2_smoother.cpp
@@ -296,23 +296,21 @@ void SmootherServer::smoothPlan()
 
     // Check for collisions
     if (goal->check_for_collisions) {
-      geometry_msgs::msg::Pose2D pose2d;
+      geometry_msgs::msg::Pose pose;
       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);
+      for (const auto & p : result->path.poses) {
+        pose = p.pose;
 
-        if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) {
+        if (!collision_checker_->isCollisionFree(pose, fetch_data)) {
           RCLCPP_ERROR(
             get_logger(),
             "Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf",
-            pose2d.x, pose2d.y, pose2d.theta);
+            pose.position.x, pose.position.y, tf2::getYaw(pose.orientation));
           throw nav2_core::SmoothedPathInCollision(
                   "Smoothed Path collided at"
-                  "X: " + std::to_string(pose2d.x) +
-                  "Y: " + std::to_string(pose2d.y) +
-                  "Theta: " + std::to_string(pose2d.theta));
+                  "X: " + std::to_string(pose.position.x) +
+                  "Y: " + std::to_string(pose.position.y) +
+                  "Theta: " + std::to_string(tf2::getYaw(pose.orientation)));
         }
         fetch_data = false;
       }
diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt
index 99bb0388538..3d07e66e11b 100644
--- a/nav2_smoother/test/CMakeLists.txt
+++ b/nav2_smoother/test/CMakeLists.txt
@@ -17,8 +17,7 @@ target_link_libraries(test_smoother_server
 target_include_directories(test_smoother_server
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 
 ament_add_gtest(test_simple_smoother
   test_simple_smoother.cpp
@@ -39,8 +38,7 @@ target_link_libraries(test_simple_smoother
 target_include_directories(test_simple_smoother
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 
 ament_add_gtest(test_savitzky_golay_smoother
   test_savitzky_golay_smoother.cpp
@@ -62,5 +60,4 @@ target_link_libraries(test_savitzky_golay_smoother
 target_include_directories(test_savitzky_golay_smoother
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index e2c2e03495e..88922fc900f 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -45,6 +45,7 @@ if(BUILD_TESTING)
   target_link_libraries(${local_controller_plugin_lib} PRIVATE
     ${geometry_msgs_TARGETS}
     nav2_core::nav2_core
+    nav2_ros_common::nav2_ros_common
     nav2_costmap_2d::nav2_costmap_2d_core
     ${nav_msgs_TARGETS}
     pluginlib::pluginlib
@@ -54,7 +55,6 @@ if(BUILD_TESTING)
     PUBLIC
       "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
       "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-      "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
   )
 
   install(TARGETS ${local_controller_plugin_lib}
@@ -73,6 +73,7 @@ if(BUILD_TESTING)
   target_link_libraries(${global_planner_plugin_lib} PRIVATE
     ${geometry_msgs_TARGETS}
     nav2_core::nav2_core ${nav_msgs_TARGETS}
+    nav2_ros_common::nav2_ros_common
     nav2_costmap_2d::nav2_costmap_2d_core
     rclcpp::rclcpp
     rclcpp_lifecycle::rclcpp_lifecycle
@@ -82,7 +83,6 @@ if(BUILD_TESTING)
     PUBLIC
       "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
       "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-      "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
   )
   pluginlib_export_plugin_description_file(nav2_core src/error_codes/planner_plugins.xml)
 
@@ -102,6 +102,7 @@ if(BUILD_TESTING)
   target_link_libraries(${smoother_plugin_lib} PRIVATE
     nav2_core::nav2_core
     nav2_costmap_2d::nav2_costmap_2d_core
+    nav2_ros_common::nav2_ros_common
     ${nav_msgs_TARGETS}
     rclcpp::rclcpp
     rclcpp_lifecycle::rclcpp_lifecycle
@@ -111,7 +112,6 @@ if(BUILD_TESTING)
     PUBLIC
       "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
       "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-      "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
   )
   pluginlib_export_plugin_description_file(nav2_core src/error_codes/smoother_plugins.xml)
 
diff --git a/nav2_system_tests/src/behavior_tree/CMakeLists.txt b/nav2_system_tests/src/behavior_tree/CMakeLists.txt
index f4054500013..e2e595e4ecc 100644
--- a/nav2_system_tests/src/behavior_tree/CMakeLists.txt
+++ b/nav2_system_tests/src/behavior_tree/CMakeLists.txt
@@ -9,6 +9,7 @@ target_link_libraries(test_behavior_tree_node
   nav2_behavior_tree::nav2_behavior_tree
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   tf2_ros::tf2_ros
   nav2_ros_common::nav2_ros_common
@@ -17,5 +18,4 @@ target_include_directories(test_behavior_tree_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt
index 2f5a0c58af7..204c7a0ea30 100644
--- a/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt
+++ b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt
@@ -9,6 +9,7 @@ target_link_libraries(${test_assisted_teleop_behavior}
   nav2_costmap_2d::nav2_costmap_2d_core
   nav2_costmap_2d::nav2_costmap_2d_client
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   ${nav2_msgs_TARGETS}
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
@@ -21,7 +22,6 @@ target_include_directories(${test_assisted_teleop_behavior}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 ament_add_test(test_assisted_teleop_behavior
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp
index f846201cac1..dfedf68072d 100644
--- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp
+++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp
@@ -222,12 +222,7 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest(
     return false;
   }
 
-  geometry_msgs::msg::Pose2D pose_2d;
-  pose_2d.x = current_pose.pose.position.x;
-  pose_2d.y = current_pose.pose.position.y;
-  pose_2d.theta = tf2::getYaw(current_pose.pose.orientation);
-
-  if (!collision_checker_->isCollisionFree(pose_2d)) {
+  if (!collision_checker_->isCollisionFree(current_pose.pose)) {
     RCLCPP_ERROR(node_->get_logger(), "Ended in collision");
     return false;
   }
diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp
index 3ffd679e930..bb3f3b18159 100644
--- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp
+++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp
@@ -25,7 +25,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 #include "nav2_msgs/action/assisted_teleop.hpp"
 #include "nav2_ros_common/node_thread.hpp"
diff --git a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt
index 13e99462b46..b71a3114c56 100644
--- a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt
+++ b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt
@@ -9,6 +9,7 @@ target_link_libraries(${test_wait_behavior_exec}
   ${geometry_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
   tf2::tf2
@@ -19,7 +20,6 @@ target_include_directories(${test_wait_behavior_exec}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 ament_add_test(test_wait_behavior
diff --git a/nav2_system_tests/src/dummy_controller/CMakeLists.txt b/nav2_system_tests/src/dummy_controller/CMakeLists.txt
index c6b03837191..322118ed8a5 100644
--- a/nav2_system_tests/src/dummy_controller/CMakeLists.txt
+++ b/nav2_system_tests/src/dummy_controller/CMakeLists.txt
@@ -6,6 +6,7 @@ target_link_libraries(dummy_controller_node PRIVATE
   ${geometry_msgs_TARGETS}
   nav2_behavior_tree::nav2_behavior_tree
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   nav2_ros_common::nav2_ros_common
 )
@@ -13,5 +14,4 @@ target_include_directories(dummy_controller_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
diff --git a/nav2_system_tests/src/dummy_planner/CMakeLists.txt b/nav2_system_tests/src/dummy_planner/CMakeLists.txt
index 9be653e0a4d..2cf4e3670da 100644
--- a/nav2_system_tests/src/dummy_planner/CMakeLists.txt
+++ b/nav2_system_tests/src/dummy_planner/CMakeLists.txt
@@ -11,5 +11,4 @@ target_include_directories(dummy_planner_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
diff --git a/nav2_system_tests/src/localization/CMakeLists.txt b/nav2_system_tests/src/localization/CMakeLists.txt
index a3c609894c5..8a08e440910 100644
--- a/nav2_system_tests/src/localization/CMakeLists.txt
+++ b/nav2_system_tests/src/localization/CMakeLists.txt
@@ -10,7 +10,6 @@ target_include_directories(test_localization_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 ament_add_test(test_localization
diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt
index 0df3c439f9f..4ba346420d0 100644
--- a/nav2_system_tests/src/planning/CMakeLists.txt
+++ b/nav2_system_tests/src/planning/CMakeLists.txt
@@ -22,7 +22,6 @@ target_include_directories(${test_planner_costmaps_exec}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 set(test_planner_random_exec test_planner_random_node)
@@ -49,7 +48,6 @@ target_include_directories(${test_planner_random_exec}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 ament_add_test(test_planner_costmaps
@@ -95,7 +93,6 @@ target_include_directories(test_planner_plugins
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 ament_add_gtest(test_planner_is_path_valid
@@ -118,5 +115,4 @@ target_include_directories(test_planner_is_path_valid
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
diff --git a/nav2_system_tests/src/updown/CMakeLists.txt b/nav2_system_tests/src/updown/CMakeLists.txt
index 5fecb7d28b6..2818ab92f31 100644
--- a/nav2_system_tests/src/updown/CMakeLists.txt
+++ b/nav2_system_tests/src/updown/CMakeLists.txt
@@ -4,13 +4,13 @@ add_executable(test_updown
 target_link_libraries(test_updown PRIVATE
   ${geometry_msgs_TARGETS}
   nav2_lifecycle_manager::nav2_lifecycle_manager_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
 )
 target_include_directories(test_updown
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 
 
diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt
index b5ec6623dee..87da1fa9cbc 100644
--- a/nav2_theta_star_planner/CMakeLists.txt
+++ b/nav2_theta_star_planner/CMakeLists.txt
@@ -26,11 +26,11 @@ add_library(${library_name} SHARED
 target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::nav2_costmap_2d_core
   nav2_util::nav2_util_core
   ${nav_msgs_TARGETS}
diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp
index dc6a7a83913..3a56cb5feea 100644
--- a/nav2_util/include/nav2_util/geometry_utils.hpp
+++ b/nav2_util/include/nav2_util/geometry_utils.hpp
@@ -20,7 +20,6 @@
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose2_d.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -105,22 +104,6 @@ inline double euclidean_distance(
   return euclidean_distance(pos1.pose, pos2.pose, is_3d);
 }
 
-/**
- * @brief Get the L2 distance between 2 geometry_msgs::Pose2D
- * @param pos1 First pose
- * @param pos1 Second pose
- * @return double L2 distance
- */
-inline double euclidean_distance(
-  const geometry_msgs::msg::Pose2D & pos1,
-  const geometry_msgs::msg::Pose2D & pos2)
-{
-  double dx = pos1.x - pos2.x;
-  double dy = pos1.y - pos2.y;
-
-  return std::hypot(dx, dy);
-}
-
 /**
  * Find element in iterator with the minimum calculated value
  */
diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt
index 66ef54f0322..563742e8b72 100644
--- a/nav2_util/src/CMakeLists.txt
+++ b/nav2_util/src/CMakeLists.txt
@@ -24,12 +24,12 @@ target_link_libraries(${library_name} PUBLIC
   tf2::tf2
   ${tf2_geometry_msgs_TARGETS}
   pluginlib::pluginlib
+  nav2_ros_common::nav2_ros_common
 )
 target_link_libraries(${library_name} PRIVATE
   ${bond_TARGETS}
 )
 target_include_directories(${library_name} PUBLIC
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index e9e26a9fdb5..912d73d0859 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -19,6 +19,9 @@
 #include <memory>
 
 #include "tf2/convert.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
 #include "nav2_util/robot_utils.hpp"
 #include "rclcpp/logger.hpp"
 
diff --git a/nav2_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt
index 20c02e9194a..75c59b8a89c 100644
--- a/nav2_velocity_smoother/CMakeLists.txt
+++ b/nav2_velocity_smoother/CMakeLists.txt
@@ -23,11 +23,11 @@ target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_util::nav2_util_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
 )
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index da8c358b2f8..f41c6e5f730 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -35,11 +35,11 @@ add_library(${library_name} SHARED
 target_include_directories(${library_name} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${library_name} PUBLIC
   ${geographic_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
   ${nav_msgs_TARGETS}
@@ -70,24 +70,24 @@ add_executable(${executable_name}
 target_include_directories(${executable_name} PRIVATE
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(${executable_name} PRIVATE
   ${library_name}
   rclcpp::rclcpp
+  nav2_ros_common::nav2_ros_common
 )
 
 add_library(wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp)
 target_include_directories(wait_at_waypoint PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(wait_at_waypoint PUBLIC
   ${geometry_msgs_TARGETS}
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
 )
 target_link_libraries(wait_at_waypoint PRIVATE
   pluginlib::pluginlib
@@ -98,13 +98,13 @@ add_library(photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp)
 target_include_directories(photo_at_waypoint PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(photo_at_waypoint PUBLIC
   cv_bridge::cv_bridge
   ${geometry_msgs_TARGETS}
   image_transport::image_transport
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   opencv_core
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
@@ -119,11 +119,11 @@ add_library(input_at_waypoint SHARED plugins/input_at_waypoint.cpp)
 target_include_directories(input_at_waypoint PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>"
 )
 target_link_libraries(input_at_waypoint PUBLIC
   ${geometry_msgs_TARGETS}
   nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   ${std_msgs_TARGETS}