diff --git a/.circleci/config.yml b/.circleci/config.yml
index edb996eb1f7..841d60042d1 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v38\
+              - "<< parameters.key >>-v39\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v38\
+              - "<< parameters.key >>-v39\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v38\
+            key: "<< parameters.key >>-v39\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml
index 5b4aa08f1e1..95941856d3e 100644
--- a/.github/workflows/build_main_against_distros.yml
+++ b/.github/workflows/build_main_against_distros.yml
@@ -17,7 +17,7 @@ jobs:
 
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v5
 
     - name: Set up Docker build context
       run: |
diff --git a/.github/workflows/claude.yml b/.github/workflows/claude.yml
index 3ffbfa392fd..73467b184cc 100644
--- a/.github/workflows/claude.yml
+++ b/.github/workflows/claude.yml
@@ -34,7 +34,7 @@ jobs:
       id-token: write
     steps:
       - name: Checkout repository
-        uses: actions/checkout@v4
+        uses: actions/checkout@v5
         with:
           fetch-depth: 1
 
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
index 62645e662aa..337b0966f0d 100644
--- a/.github/workflows/lint.yml
+++ b/.github/workflows/lint.yml
@@ -13,7 +13,7 @@ jobs:
       matrix:
           linter: [xmllint, cpplint, uncrustify, pep257, flake8, mypy]
     steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v5
 
     - name: Install typeshed for mypy
       if: matrix.linter == 'mypy'
@@ -30,7 +30,7 @@ jobs:
     name: pre-commit
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - uses: actions/setup-python@v5
       - uses: pre-commit/action@v3.0.1
         env:
diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml
index ef39facf09c..3c7743f1ad3 100644
--- a/.github/workflows/update_ci_image.yaml
+++ b/.github/workflows/update_ci_image.yaml
@@ -66,7 +66,7 @@ jobs:
       - check_ci_image
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3
       - name: Login to Docker Hub
diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt
index 4813b3cdd9b..e29f641cb84 100644
--- a/nav2_amcl/CMakeLists.txt
+++ b/nav2_amcl/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_amcl)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index de7d0f6237f..de9cde5f2ab 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_behavior_tree CXX)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt
index bd37a097c2f..292191ea013 100644
--- a/nav2_behaviors/CMakeLists.txt
+++ b/nav2_behaviors/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_behaviors)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt
index b0999ee4eb8..af08ad0265b 100644
--- a/nav2_bringup/CMakeLists.txt
+++ b/nav2_bringup/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_bringup)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index 83fa44fab7f..156ec4afff6 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/nav2_bt_navigator/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_bt_navigator)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
index e67c8ea7719..eb91f295d47 100644
--- a/nav2_collision_monitor/CMakeLists.txt
+++ b/nav2_collision_monitor/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_collision_monitor)
 
 find_package(ament_cmake REQUIRED)
@@ -17,6 +17,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
+find_package(point_cloud_transport REQUIRED)
 
 nav2_package()
 
@@ -54,6 +55,7 @@ target_link_libraries(${monitor_library_name} PUBLIC
   tf2::tf2
   tf2_ros::tf2_ros
   ${visualization_msgs_TARGETS}
+  point_cloud_transport::point_cloud_transport
 )
 target_link_libraries(${monitor_library_name} PRIVATE
   rclcpp_components::component
@@ -88,6 +90,7 @@ target_link_libraries(${detector_library_name} PUBLIC
   tf2_ros::tf2_ros
   tf2::tf2
   ${visualization_msgs_TARGETS}
+  point_cloud_transport::point_cloud_transport
 )
 target_link_libraries(${detector_library_name} PRIVATE
   rclcpp_components::component
@@ -169,6 +172,7 @@ ament_export_dependencies(
   tf2_ros
   nav2_ros_common
   visualization_msgs
+  point_cloud_transport
 )
 ament_export_targets(export_${PROJECT_NAME})
 
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
index 8a46d2b65c5..c94180f94cf 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
@@ -20,6 +20,7 @@
 #include <string>
 
 #include "sensor_msgs/msg/point_cloud2.hpp"
+#include "point_cloud_transport/point_cloud_transport.hpp"
 
 #include "nav2_collision_monitor/source.hpp"
 
@@ -98,7 +99,15 @@ class PointCloud : public Source
   // ----- Variables -----
 
   /// @brief PointCloud data subscriber
+  #if RCLCPP_VERSION_GTE(30, 0, 0)
+  std::shared_ptr<point_cloud_transport::PointCloudTransport> pct_;
+  point_cloud_transport::Subscriber data_sub_;
+  #else
   nav2::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr data_sub_;
+  #endif
+
+  // Transport type used for PointCloud messages (e.g., raw or compressed)
+  std::string transport_type_;
 
   // Minimum and maximum height of PointCloud projected to 2D space
   double min_height_, max_height_;
diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
index 6a8e5ace75b..6b009f1634b 100644
--- a/nav2_collision_monitor/package.xml
+++ b/nav2_collision_monitor/package.xml
@@ -25,6 +25,8 @@
   <depend>tf2_ros</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>point_cloud_transport_plugins</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml
index eb3ee0a8739..bbeaadc5154 100644
--- a/nav2_collision_monitor/params/collision_detector_params.yaml
+++ b/nav2_collision_monitor/params/collision_detector_params.yaml
@@ -23,6 +23,7 @@ collision_detector:
     pointcloud:
       type: "pointcloud"
       topic: "/intel_realsense_r200_depth/points"
+      transport_type: "raw" # Options: raw, zlib, draco, zstd
       min_height: 0.1
       max_height: 0.5
       enabled: True
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
index eac7486913c..28224362ea9 100644
--- a/nav2_collision_monitor/params/collision_monitor_params.yaml
+++ b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -96,6 +96,7 @@ collision_monitor:
     pointcloud:
       type: "pointcloud"
       topic: "/intel_realsense_r200_depth/points"
+      transport_type: "raw" # Options: raw, zlib, draco, zstd
       min_height: 0.1
       max_height: 0.5
       enabled: True
diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp
index 590eacb3cb8..72ef467b9a5 100644
--- a/nav2_collision_monitor/src/pointcloud.cpp
+++ b/nav2_collision_monitor/src/pointcloud.cpp
@@ -45,7 +45,11 @@ PointCloud::PointCloud(
 PointCloud::~PointCloud()
 {
   RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str());
+  #if RCLCPP_VERSION_GTE(30, 0, 0)
+  data_sub_.shutdown();
+  #else
   data_sub_.reset();
+  #endif
 }
 
 void PointCloud::configure()
@@ -60,10 +64,20 @@ void PointCloud::configure()
 
   getParameters(source_topic);
 
+  #if RCLCPP_VERSION_GTE(30, 0, 0)
+  const point_cloud_transport::TransportHints hint(transport_type_);
+  pct_ = std::make_shared<point_cloud_transport::PointCloudTransport>(*node);
+  data_sub_ = pct_->subscribe(
+    source_topic, nav2::qos::SensorDataQoS(),
+    std::bind(&PointCloud::dataCallback, this, std::placeholders::_1),
+    {}, &hint
+  );
+  #else
   data_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
     source_topic,
     std::bind(&PointCloud::dataCallback, this, std::placeholders::_1),
     nav2::qos::SensorDataQoS());
+  #endif
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -131,6 +145,9 @@ void PointCloud::getParameters(std::string & source_topic)
   nav2::declare_parameter_if_not_declared(
     node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0));
   min_range_ = node->get_parameter(source_name_ + ".min_range").as_double();
+  nav2::declare_parameter_if_not_declared(
+    node, source_name_ + ".transport_type", rclcpp::ParameterValue(std::string("raw")));
+  transport_type_ = node->get_parameter(source_name_ + ".transport_type").as_string();
 }
 
 void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt
index f0f1e22c32c..71a771b29c4 100644
--- a/nav2_common/CMakeLists.txt
+++ b/nav2_common/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 
 project(nav2_common NONE)
 
diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt
index c4613f52496..6e179687f94 100644
--- a/nav2_constrained_smoother/CMakeLists.txt
+++ b/nav2_constrained_smoother/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_constrained_smoother)
 
 set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup
diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
index 93885ef7efc..c8a655f4340 100644
--- a/nav2_controller/CMakeLists.txt
+++ b/nav2_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt
index 586410c3488..1f7ae52aa05 100644
--- a/nav2_core/CMakeLists.txt
+++ b/nav2_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_core)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index 851f2428b42..00cb720aa15 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_costmap_2d)
 
 find_package(ament_cmake REQUIRED)
@@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(tf2_sensor_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(point_cloud_transport REQUIRED)
 
 nav2_package()
 
@@ -88,6 +89,7 @@ target_link_libraries(layers PUBLIC
   nav2_ros_common::nav2_ros_common
   tf2::tf2
   nav2_ros_common::nav2_ros_common
+  point_cloud_transport::point_cloud_transport
 )
 target_link_libraries(layers PRIVATE
   pluginlib::pluginlib
@@ -222,6 +224,7 @@ ament_export_dependencies(
   tf2_ros
   tf2_sensor_msgs
   nav2_ros_common
+  point_cloud_transport
 )
 pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
 ament_package()
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 e51809df696..25238cf8a35 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
@@ -99,7 +99,7 @@ class CostmapFilter : public Layer
    */
   void updateBounds(
     double robot_x, double robot_y, double robot_yaw,
-    double * min_x, double * min_y, double * max_x, double * max_y) final;
+    double * min_x, double * min_y, double * max_x, double * max_y) override;
 
   /**
    * @brief Update the costs in the master costmap in the window
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 e615ba465a0..118fc3ef295 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
@@ -68,6 +68,20 @@ class KeepoutFilter : public CostmapFilter
   void initializeFilter(
     const std::string & filter_info_topic);
 
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  void updateBounds(
+    double robot_x, double robot_y, double robot_yaw,
+    double * min_x, double * min_y, double * max_x, double * max_y) override;
+
   /**
    * @brief Process the keepout layer at the current pose / bounds / grid
    */
@@ -106,8 +120,15 @@ class KeepoutFilter : public CostmapFilter
   bool override_lethal_cost_{false};  // If true, lethal cost will be overridden
   unsigned char lethal_override_cost_{252};  // Value to override lethal cost with
   bool last_pose_lethal_{false};  // If true, last pose was lethal
-  unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u};
-  unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u};
+  bool is_pose_lethal_{false};
+  double lethal_state_update_min_x_, lethal_state_update_min_y_;
+  double lethal_state_update_max_x_, lethal_state_update_max_y_;
+
+  unsigned int x_{0};
+  unsigned int y_{0};
+  unsigned int width_{0};
+  unsigned int height_{0};
+  bool has_updated_data_{false};
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 5e66e24795a..32b17ca5d5b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -51,6 +51,7 @@
 #pragma GCC diagnostic pop
 
 #include "message_filters/subscriber.hpp"
+#include "point_cloud_transport/subscriber_filter.hpp"
 
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
index c69d467b908..4e4940a59af 100644
--- a/nav2_costmap_2d/package.xml
+++ b/nav2_costmap_2d/package.xml
@@ -42,6 +42,8 @@
   <depend>tf2_sensor_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>point_cloud_transport_plugins</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
index bc4e89f878e..06d7ed2d025 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -222,7 +222,7 @@ unsigned char CostmapFilter::getMaskCost(
 {
   const unsigned int index = my * filter_mask->info.width + mx;
 
-  const char data = filter_mask->data[index];
+  const signed char data = filter_mask->data[index];
   if (data == nav2_util::OCC_GRID_UNKNOWN) {
     return NO_INFORMATION;
   } else {
diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
index e727d00634e..48cd6c74e42 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
@@ -43,6 +43,7 @@
 
 #include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -84,6 +85,8 @@ void KeepoutFilter::initializeFilter(
   // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given
   lethal_override_cost_ = \
     std::clamp<unsigned int>(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE);
+  lethal_state_update_max_x_ = lethal_state_update_max_y_ = std::numeric_limits<double>::lowest();
+  lethal_state_update_min_x_ = lethal_state_update_min_y_ = std::numeric_limits<double>::max();
 }
 
 void KeepoutFilter::filterInfoCallback(
@@ -155,12 +158,86 @@ void KeepoutFilter::maskCallback(
 
   // Store filter_mask_
   filter_mask_ = msg;
+  has_updated_data_ = true;
+  x_ = y_ = 0;
+  width_ = msg->info.width;
+  height_ = msg->info.height;
+}
+
+void KeepoutFilter::updateBounds(
+  double robot_x, double robot_y, double robot_yaw,
+  double * min_x, double * min_y, double * max_x, double * max_y)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  CostmapFilter::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+
+  // If new keepout zone received
+  if(has_updated_data_) {
+    double wx, wy;
+    layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy);
+    *min_x = std::min(wx, *min_x);
+    *min_y = std::min(wy, *min_y);
+
+    layered_costmap_->getCostmap()->mapToWorld(x_ + width_, y_ + height_, wx, wy);
+    *max_x = std::max(wx, *max_x);
+    *max_y = std::max(wy, *max_y);
+
+    has_updated_data_ = false;
+    return;
+  }
+
+  // Let's find the pose's cost if we are allowed to override the lethal cost
+  is_pose_lethal_ = false;
+  if (override_lethal_cost_) {
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = robot_x;
+    pose.position.y = robot_y;
+    pose.position.z = 0.0;
+    pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(robot_yaw);
+    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.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_) {
+          RCLCPP_WARN_THROTTLE(
+            logger_, *(clock_), 2000,
+            "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
+        }
+      }
+    }
+
+    // If in lethal space or just exited lethal space,
+    // we need to update all possible spaces touched during this state
+    if (is_pose_lethal_ || (last_pose_lethal_ && !is_pose_lethal_)) {
+      lethal_state_update_min_x_ = std::min(*min_x, lethal_state_update_min_x_);
+      *min_x = lethal_state_update_min_x_;
+      lethal_state_update_min_y_ = std::min(*min_y, lethal_state_update_min_y_);
+      *min_y = lethal_state_update_min_y_;
+      lethal_state_update_max_x_ = std::max(*max_x, lethal_state_update_max_x_);
+      *max_x = lethal_state_update_max_x_;
+      lethal_state_update_max_y_ = std::max(*max_y, lethal_state_update_max_y_);
+      *max_y = lethal_state_update_max_y_;
+    } else {
+      // If out of lethal space, reset managed lethal state sizes
+      lethal_state_update_min_x_ = std::numeric_limits<double>::max();
+      lethal_state_update_min_y_ = std::numeric_limits<double>::max();
+      lethal_state_update_max_x_ = std::numeric_limits<double>::lowest();
+      lethal_state_update_max_y_ = std::numeric_limits<double>::lowest();
+    }
+  }
 }
 
 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::Pose & pose)
+  const geometry_msgs::msg::Pose & /*pose*/)
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
@@ -255,49 +332,10 @@ void KeepoutFilter::process(
   }
 
   // unsigned<-signed conversions.
-  unsigned int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
-  unsigned int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
-  unsigned int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
-  unsigned int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
-
-  // 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::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.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) {
-          RCLCPP_WARN_THROTTLE(
-            logger_, *(clock_), 2000,
-            "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
-        }
-      }
-    }
-
-    // If in lethal space or just exited lethal space,
-    // we need to update all possible spaces touched during this state
-    if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) {
-      lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_);
-      mg_min_x_u = lethal_state_update_min_x_;
-      lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_);
-      mg_min_y_u = lethal_state_update_min_y_;
-      lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_);
-      mg_max_x_u = lethal_state_update_max_x_;
-      lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_);
-      mg_max_y_u = lethal_state_update_max_y_;
-    } else {
-      // If out of lethal space, reset managed lethal state sizes
-      lethal_state_update_min_x_ = master_grid.getSizeInCellsX();
-      lethal_state_update_min_y_ = master_grid.getSizeInCellsY();
-      lethal_state_update_max_x_ = 0u;
-      lethal_state_update_max_y_ = 0u;
-    }
-  }
+  const unsigned int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
+  const unsigned int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
+  const unsigned int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
+  const unsigned int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
 
   unsigned int i, j;  // master_grid iterators
   unsigned int index;  // corresponding index of master_grid
@@ -336,7 +374,7 @@ void KeepoutFilter::process(
         }
 
         if (data > old_data || old_data == NO_INFORMATION) {
-          if (override_lethal_cost_ && is_pose_lethal) {
+          if (override_lethal_cost_ && is_pose_lethal_) {
             master_array[index] = lethal_override_cost_;
           } else {
             master_array[index] = data;
@@ -346,7 +384,7 @@ void KeepoutFilter::process(
     }
   }
 
-  last_pose_lethal_ = is_pose_lethal;
+  last_pose_lethal_ = is_pose_lethal_;
 }
 
 void KeepoutFilter::resetFilter()
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index bac51b92f92..b897ac515be 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -141,7 +141,7 @@ void ObstacleLayer::onInitialize()
   while (ss >> source) {
     // get the parameters for the specific topic
     double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
-    std::string topic, sensor_frame, data_type;
+    std::string topic, sensor_frame, data_type, transport_type;
     bool inf_is_valid, clearing, marking;
 
     declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));
@@ -158,6 +158,7 @@ void ObstacleLayer::onInitialize()
     declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0));
     declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0));
     declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "transport_type", rclcpp::ParameterValue(std::string("raw")));
 
     node->get_parameter(name_ + "." + source + "." + "topic", topic);
     node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
@@ -173,6 +174,7 @@ void ObstacleLayer::onInitialize()
     node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
     node->get_parameter(name_ + "." + source + "." + "marking", marking);
     node->get_parameter(name_ + "." + source + "." + "clearing", clearing);
+    node->get_parameter(name_ + "." + source + "." + "transport_type", transport_type);
 
     if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
       RCLCPP_FATAL(
@@ -287,16 +289,23 @@ void ObstacleLayer::onInitialize()
           tf_filter_tolerance));
 
     } else {
+      // For Rolling and Newer Support from PointCloudTransport API change
+      #if RCLCPP_VERSION_GTE(30, 0, 0)
+      std::shared_ptr<point_cloud_transport::SubscriberFilter> sub;
       // For Kilted and Older Support from Message Filters API change
-      #if RCLCPP_VERSION_GTE(29, 6, 0)
+      #elif RCLCPP_VERSION_GTE(29, 6, 0)
       std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> sub;
       #else
       std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
         rclcpp_lifecycle::LifecycleNode>> sub;
       #endif
 
+      // For Rolling compatibility in PointCloudTransport API change
+      #if RCLCPP_VERSION_GTE(30, 0, 0)
+      sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
+        *node, topic, transport_type, custom_qos_profile, sub_opt);
       // For Kilted compatibility in Message Filters API change
-      #if RCLCPP_VERSION_GTE(29, 6, 0)
+      #elif RCLCPP_VERSION_GTE(29, 6, 0)
       sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(
         node, topic, custom_qos_profile, sub_opt);
       // For Jazzy compatibility in Message Filters API change
diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
index a68bd4677ab..8801b18d5a8 100644
--- a/nav2_docking/opennav_docking/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(opennav_docking)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt
index 05e67fb713f..2d0306397aa 100644
--- a/nav2_docking/opennav_docking_bt/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(opennav_docking_bt)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt
index 057bebb0b6c..e8a4f821884 100644
--- a/nav2_docking/opennav_docking_core/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(opennav_docking_core)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt
index 68dbfa64ef9..c330e96a9f0 100644
--- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt
+++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(costmap_queue)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt
index 00789a46e3d..f69d546f551 100644
--- a/nav2_dwb_controller/dwb_core/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_core)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
index ddda0a9087b..be02bcc98f8 100644
--- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_critics)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
index 574a48c4c6a..9ff83591d2e 100644
--- a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_msgs)
 
 find_package(backward_ros REQUIRED)
diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
index 7d4228ae427..b462e7b3aab 100644
--- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_plugins)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
index 5c1c8fce07a..3fd5af5cc2a 100644
--- a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
+++ b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_dwb_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
index 155c13cdd65..f597712deda 100644
--- a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav_2d_msgs)
 
 find_package(backward_ros REQUIRED)
diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
index 7aad657a142..5378f2e4b83 100644
--- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav_2d_utils)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt
index d9720ca6401..95d682a8cad 100644
--- a/nav2_graceful_controller/CMakeLists.txt
+++ b/nav2_graceful_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_graceful_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
index 96358ae9607..28bec7eddd8 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
@@ -107,6 +107,24 @@ class GracefulController : public nav2_core::Controller
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
 protected:
+  /**
+   * @brief Validate a given target pose for calculating command velocity
+   * @param target_pose Target pose to validate
+   * @param dist_to_target Distance to target pose
+   * @param dist_to_goal Distance to navigation goal
+   * @param trajectory Trajectory to validate in simulation
+   * @param costmap_transform Transform between global and local costmap
+   * @param cmd_vel Initial command velocity to validate in simulation
+   * @return true if target pose is valid, false otherwise
+   */
+  bool validateTargetPose(
+    geometry_msgs::msg::PoseStamped & target_pose,
+    double dist_to_target,
+    double dist_to_goal,
+    nav_msgs::msg::Path & trajectory,
+    geometry_msgs::msg::TransformStamped & costmap_transform,
+    geometry_msgs::msg::TwistStamped & cmd_vel);
+
   /**
    * @brief Simulate trajectory calculating in every step the new velocity command based on
    * a new curvature value and checking for collisions.
diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp
index 0706c86378d..b8cce0b0aed 100644
--- a/nav2_graceful_controller/src/graceful_controller.cpp
+++ b/nav2_graceful_controller/src/graceful_controller.cpp
@@ -18,6 +18,7 @@
 #include "angles/angles.h"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/controller_utils.hpp"
 #include "nav2_graceful_controller/graceful_controller.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
@@ -195,46 +196,38 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     // Else, fall through and see if we should follow control law longer
   }
 
-  // Precompute distance to candidate poses
+  // Find a valid target pose and its trajectory
+  nav_msgs::msg::Path local_plan;
+  geometry_msgs::msg::PoseStamped target_pose;
+
+  double dist_to_target;
   std::vector<double> target_distances;
   computeDistanceAlongPath(transformed_plan.poses, target_distances);
 
-  // Work back from the end of plan to find valid target pose
+  bool is_first_iteration = true;
   for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) {
-    // Underlying control law needs a single target pose, which should:
-    //  * Be as far away as possible from the robot (for smoothness)
-    //  * But no further than the max_lookahed_ distance
-    //  * Be feasible to reach in a collision free manner
-    geometry_msgs::msg::PoseStamped target_pose = transformed_plan.poses[i];
-    double dist_to_target = target_distances[i];
-
-    // Continue if target_pose is too far away from robot
-    if (dist_to_target > params_->max_lookahead) {continue;}
-
-    if (dist_to_goal < params_->max_lookahead) {
-      if (params_->prefer_final_rotation) {
-        // Avoid instability and big sweeping turns at the end of paths by
-        // ignoring final heading
-        double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
-        target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
-      }
-    } else if (dist_to_target < params_->min_lookahead) {
-      // Make sure target is far enough away to avoid instability
-      break;
-    }
-
-    // Flip the orientation of the motion target if the robot is moving backwards
-    bool reversing = false;
-    if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
-      reversing = true;
-      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
-        tf2::getYaw(target_pose.pose.orientation) + M_PI);
+    if (is_first_iteration) {
+      // Calculate target pose through lookahead interpolation to get most accurate
+      // lookahead point, if possible
+      dist_to_target = params_->max_lookahead;
+      // Interpolate after goal false for graceful controller
+      // Requires interpolating the orientation which is not yet implemented
+      // Updates dist_to_target for target_pose returned if using the point on the path
+      target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_plan, false);
+      is_first_iteration = false;
+    } else {
+      // Underlying control law needs a single target pose, which should:
+      //  * Be as far away as possible from the robot (for smoothness)
+      //  * But no further than the max_lookahed_ distance
+      //  * Be feasible to reach in a collision free manner
+      dist_to_target = target_distances[i];
+      target_pose = transformed_plan.poses[i];
     }
 
-    // Actually simulate our path
-    nav_msgs::msg::Path local_plan;
-    if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) {
-      // Successfully simulated to target_pose - compute velocity at this moment
+    // Compute velocity at this moment if valid target pose is found
+    if (validateTargetPose(
+        target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel))
+    {
       // Publish the selected target_pose
       motion_target_pub_->publish(target_pose);
       // Publish marker for slowdown radius around motion target for debugging / visualization
@@ -283,6 +276,49 @@ void GracefulController::setSpeedLimit(
   }
 }
 
+bool GracefulController::validateTargetPose(
+  geometry_msgs::msg::PoseStamped & target_pose,
+  double dist_to_target,
+  double dist_to_goal,
+  nav_msgs::msg::Path & trajectory,
+  geometry_msgs::msg::TransformStamped & costmap_transform,
+  geometry_msgs::msg::TwistStamped & cmd_vel)
+{
+  // Continue if target_pose is too far away from robot
+  if (dist_to_target > params_->max_lookahead) {
+    return false;
+  }
+
+  if (dist_to_goal < params_->max_lookahead) {
+    if (params_->prefer_final_rotation) {
+      // Avoid instability and big sweeping turns at the end of paths by
+      // ignoring final heading
+      double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
+      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+    }
+  } else if (dist_to_target < params_->min_lookahead) {
+    // Make sure target is far enough away to avoid instability
+    return false;
+  }
+
+  // Flip the orientation of the motion target if the robot is moving backwards
+  bool reversing = false;
+  if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
+    reversing = true;
+    target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+      tf2::getYaw(target_pose.pose.orientation) + M_PI);
+  }
+
+  // Actually simulate the path
+  if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
+    // Successfully simulated to target_pose
+    return true;
+  }
+
+  // Validation not successful
+  return false;
+}
+
 bool GracefulController::simulateTrajectory(
   const geometry_msgs::msg::PoseStamped & motion_target,
   const geometry_msgs::msg::TransformStamped & costmap_transform,
diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt
index 64b1f4ad88d..50874482167 100644
--- a/nav2_lifecycle_manager/CMakeLists.txt
+++ b/nav2_lifecycle_manager/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_lifecycle_manager)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
index 55160b2f7de..a003a8fb087 100644
--- a/nav2_map_server/CMakeLists.txt
+++ b/nav2_map_server/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_map_server)
 
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules)
diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index 9dabc27507f..931af234887 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_mppi_controller)
 
 find_package(ament_cmake REQUIRED)
@@ -83,7 +83,13 @@ add_library(mppi_critics SHARED
   src/critics/twirling_critic.cpp
   src/critics/velocity_deadband_critic.cpp
 )
-target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE)
+    # Apple Clang: use C++20 and optimization, omit -fconcepts
+    target_compile_features(mppi_critics PUBLIC cxx_std_20)
+    target_compile_options(mppi_critics PUBLIC -O3)
+else()
+    target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
+endif()
 target_include_directories(mppi_critics
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index f01fbdba6a5..93f7998c56a 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_msgs)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
index 0da443e5e35..1b50472013f 100644
--- a/nav2_navfn_planner/CMakeLists.txt
+++ b/nav2_navfn_planner/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_navfn_planner)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt
index d3037ddf839..0285fbe9c1a 100644
--- a/nav2_planner/CMakeLists.txt
+++ b/nav2_planner/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_planner)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
index 493418fda67..bec081d3987 100644
--- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
+++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_regulated_pure_pursuit_controller)
 
 find_package(ament_cmake REQUIRED)
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 2eb901dc11f..fd79208d291 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
@@ -174,32 +174,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller
     const double & pose_cost, const nav_msgs::msg::Path & path,
     double & linear_vel, double & sign);
 
-  /**
-   * @brief Find the intersection a circle and a line segment.
-   * This assumes the circle is centered at the origin.
-   * If no intersection is found, a floating point error will occur.
-   * @param p1 first endpoint of line segment
-   * @param p2 second endpoint of line segment
-   * @param r radius of circle
-   * @return point of intersection
-   */
-  static geometry_msgs::msg::Point circleSegmentIntersection(
-    const geometry_msgs::msg::Point & p1,
-    const geometry_msgs::msg::Point & p2,
-    double r);
-
-  /**
-   * @brief Get lookahead point
-   * @param lookahead_dist Optimal lookahead distance
-   * @param path Current global path
-   * @param interpolate_after_goal If true, interpolate the lookahead point after the goal based
-   * on the orientation given by the position of the last two pose of the path
-   * @return Lookahead point
-   */
-  geometry_msgs::msg::PoseStamped getLookAheadPoint(
-    const double &, const nav_msgs::msg::Path &,
-    bool interpolate_after_goal = false);
-
   /**
    * @brief checks for the cusp position
    * @param pose Pose input to determine the cusp position
diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
index 1bd12ae3a06..fced1dff734 100644
--- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
@@ -25,6 +25,7 @@
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/controller_utils.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
 using std::hypot;
@@ -204,7 +205,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   }
 
   // Get the particular point on the path at the lookahead distance
-  auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);
+  auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan);
   auto rotate_to_path_carrot_pose = carrot_pose;
   carrot_pub_->publish(createCarrotMsg(carrot_pose));
 
@@ -214,7 +215,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   double regulation_curvature = lookahead_curvature;
   if (params_->use_fixed_curvature_lookahead) {
-    auto curvature_lookahead_pose = getLookAheadPoint(
+    auto curvature_lookahead_pose = nav2_util::getLookAheadPoint(
       curv_lookahead_dist,
       transformed_plan, params_->interpolate_curvature_after_goal);
     rotate_to_path_carrot_pose = curvature_lookahead_pose;
@@ -358,100 +359,6 @@ void RegulatedPurePursuitController::rotateToHeading(
   }
 }
 
-geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection(
-  const geometry_msgs::msg::Point & p1,
-  const geometry_msgs::msg::Point & p2,
-  double r)
-{
-  // Formula for intersection of a line with a circle centered at the origin,
-  // modified to always return the point that is on the segment between the two points.
-  // https://mathworld.wolfram.com/Circle-LineIntersection.html
-  // This works because the poses are transformed into the robot frame.
-  // This can be derived from solving the system of equations of a line and a circle
-  // which results in something that is just a reformulation of the quadratic formula.
-  // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
-  // https://www.desmos.com/calculator/td5cwbuocd
-  double x1 = p1.x;
-  double x2 = p2.x;
-  double y1 = p1.y;
-  double y2 = p2.y;
-
-  double dx = x2 - x1;
-  double dy = y2 - y1;
-  double dr2 = dx * dx + dy * dy;
-  double D = x1 * y2 - x2 * y1;
-
-  // Augmentation to only return point within segment
-  double d1 = x1 * x1 + y1 * y1;
-  double d2 = x2 * x2 + y2 * y2;
-  double dd = d2 - d1;
-
-  geometry_msgs::msg::Point p;
-  double sqrt_term = std::sqrt(r * r * dr2 - D * D);
-  p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2;
-  p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2;
-  return p;
-}
-
-geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
-  const double & lookahead_dist,
-  const nav_msgs::msg::Path & transformed_plan,
-  bool interpolate_after_goal)
-{
-  // Find the first pose which is at a distance greater than the lookahead distance
-  auto goal_pose_it = std::find_if(
-    transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) {
-      return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist;
-    });
-
-  // If the no pose is not far enough, take the last pose
-  if (goal_pose_it == transformed_plan.poses.end()) {
-    if (interpolate_after_goal) {
-      auto last_pose_it = std::prev(transformed_plan.poses.end());
-      auto prev_last_pose_it = std::prev(last_pose_it);
-
-      double end_path_orientation = atan2(
-        last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y,
-        last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x);
-
-      // Project the last segment out to guarantee it is beyond the look ahead
-      // distance
-      auto projected_position = last_pose_it->pose.position;
-      projected_position.x += cos(end_path_orientation) * lookahead_dist;
-      projected_position.y += sin(end_path_orientation) * lookahead_dist;
-
-      // Use the circle intersection to find the position at the correct look
-      // ahead distance
-      const auto interpolated_position = circleSegmentIntersection(
-        last_pose_it->pose.position, projected_position, lookahead_dist);
-
-      geometry_msgs::msg::PoseStamped interpolated_pose;
-      interpolated_pose.header = last_pose_it->header;
-      interpolated_pose.pose.position = interpolated_position;
-      return interpolated_pose;
-    } else {
-      goal_pose_it = std::prev(transformed_plan.poses.end());
-    }
-  } else if (goal_pose_it != transformed_plan.poses.begin()) {
-    // Find the point on the line segment between the two poses
-    // that is exactly the lookahead distance away from the robot pose (the origin)
-    // This can be found with a closed form for the intersection of a segment and a circle
-    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
-    // and goal_pose is guaranteed to be outside the circle.
-    auto prev_pose_it = std::prev(goal_pose_it);
-    auto point = circleSegmentIntersection(
-      prev_pose_it->pose.position,
-      goal_pose_it->pose.position, lookahead_dist);
-    geometry_msgs::msg::PoseStamped pose;
-    pose.header.frame_id = prev_pose_it->header.frame_id;
-    pose.header.stamp = goal_pose_it->header.stamp;
-    pose.pose.position = point;
-    return pose;
-  }
-
-  return *goal_pose_it;
-}
-
 void RegulatedPurePursuitController::applyConstraints(
   const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/,
   const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign)
diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
index 197cae4c5a0..4b3dbb905f8 100644
--- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
+++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
@@ -52,28 +52,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
     return getLookAheadDistance(twist);
   }
 
-  static geometry_msgs::msg::Point circleSegmentIntersectionWrapper(
-    const geometry_msgs::msg::Point & p1,
-    const geometry_msgs::msg::Point & p2,
-    double r)
-  {
-    return circleSegmentIntersection(p1, p2, r);
-  }
-
-  geometry_msgs::msg::PoseStamped
-  projectCarrotPastGoalWrapper(
-    const double & dist,
-    const nav_msgs::msg::Path & path)
-  {
-    return getLookAheadPoint(dist, path, true);
-  }
-
-  geometry_msgs::msg::PoseStamped getLookAheadPointWrapper(
-    const double & dist, const nav_msgs::msg::Path & path)
-  {
-    return getLookAheadPoint(dist, path);
-  }
-
   bool shouldRotateToPathWrapper(
     const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path)
   {
@@ -207,239 +185,6 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange)
   EXPECT_EQ(rtn, std::numeric_limits<double>::max());
 }
 
-using CircleSegmentIntersectionParam = std::tuple<
-  std::pair<double, double>,
-  std::pair<double, double>,
-  double,
-  std::pair<double, double>
->;
-
-class CircleSegmentIntersectionTest
-  : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
-{};
-
-TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection)
-{
-  auto pair1 = std::get<0>(GetParam());
-  auto pair2 = std::get<1>(GetParam());
-  auto r = std::get<2>(GetParam());
-  auto expected_pair = std::get<3>(GetParam());
-  auto pair_to_point = [](std::pair<double, double> p) -> geometry_msgs::msg::Point {
-      geometry_msgs::msg::Point point;
-      point.x = p.first;
-      point.y = p.second;
-      point.z = 0.0;
-      return point;
-    };
-  auto p1 = pair_to_point(pair1);
-  auto p2 = pair_to_point(pair2);
-  auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(p1, p2, r);
-  auto expected_point = pair_to_point(expected_pair);
-  EXPECT_DOUBLE_EQ(actual.x, expected_point.x);
-  EXPECT_DOUBLE_EQ(actual.y, expected_point.y);
-  // Expect that the intersection point is actually r away from the origin
-  EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y));
-}
-
-INSTANTIATE_TEST_SUITE_P(
-  InterpolationTest,
-  CircleSegmentIntersectionTest,
-  testing::Values(
-    // Origin to the positive X axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {2.0, 0.0},
-  1.0,
-  {1.0, 0.0}
-},
-    // Origin to the negative X axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-2.0, 0.0},
-  1.0,
-  {-1.0, 0.0}
-},
-    // Origin to the positive Y axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, 2.0},
-  1.0,
-  {0.0, 1.0}
-},
-    // Origin to the negative Y axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, -2.0},
-  1.0,
-  {0.0, -1.0}
-},
-    // non-origin to the X axis with non-unit circle, with the second point inside
-    CircleSegmentIntersectionParam{
-  {4.0, 0.0},
-  {-1.0, 0.0},
-  2.0,
-  {2.0, 0.0}
-},
-    // non-origin to the Y axis with non-unit circle, with the second point inside
-    CircleSegmentIntersectionParam{
-  {0.0, 4.0},
-  {0.0, -0.5},
-  2.0,
-  {0.0, 2.0}
-},
-    // origin to the positive X axis, on the circle
-    CircleSegmentIntersectionParam{
-  {2.0, 0.0},
-  {0.0, 0.0},
-  2.0,
-  {2.0, 0.0}
-},
-    // origin to the positive Y axis, on the circle
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, 2.0},
-  2.0,
-  {0.0, 2.0}
-},
-    // origin to the upper-right quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {6.0, 8.0},
-  5.0,
-  {3.0, 4.0}
-},
-    // origin to the lower-left quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-6.0, -8.0},
-  5.0,
-  {-3.0, -4.0}
-},
-    // origin to the upper-left quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-6.0, 8.0},
-  5.0,
-  {-3.0, 4.0}
-},
-    // origin to the lower-right quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {6.0, -8.0},
-  5.0,
-  {3.0, -4.0}
-}
-));
-
-TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
-  auto ctrl = std::make_shared<BasicAPIRPP>();
-  auto node = std::make_shared<nav2::LifecycleNode>("testRPP");
-  std::string name = "PathFollower";
-  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
-  auto costmap =
-    std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
-  rclcpp_lifecycle::State state;
-  costmap->on_configure(state);
-  ctrl->configure(node, name, tf, costmap);
-
-  double EPSILON = std::numeric_limits<float>::epsilon();
-
-  nav_msgs::msg::Path path;
-  // More than 2 poses
-  path.poses.resize(4);
-  path.poses[0].pose.position.x = 0.0;
-  path.poses[1].pose.position.x = 1.0;
-  path.poses[2].pose.position.x = 2.0;
-  path.poses[3].pose.position.x = 3.0;
-  auto pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
-
-  // 2 poses fwd
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 2.0;
-  path.poses[1].pose.position.x = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
-
-  // 2 poses at 45°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 2.0;
-  path.poses[0].pose.position.y = 2.0;
-  path.poses[1].pose.position.x = 3.0;
-  path.poses[1].pose.position.y = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at 90°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 0.0;
-  path.poses[0].pose.position.y = 2.0;
-  path.poses[1].pose.position.x = 0.0;
-  path.poses[1].pose.position.y = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at 135°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = -2.0;
-  path.poses[0].pose.position.y = 2.0;
-  path.poses[1].pose.position.x = -3.0;
-  path.poses[1].pose.position.y = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses back
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = -2.0;
-  path.poses[1].pose.position.x = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
-
-  // 2 poses at -135°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = -2.0;
-  path.poses[0].pose.position.y = -2.0;
-  path.poses[1].pose.position.x = -3.0;
-  path.poses[1].pose.position.y = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at -90°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 0.0;
-  path.poses[0].pose.position.y = -2.0;
-  path.poses[1].pose.position.x = 0.0;
-  path.poses[1].pose.position.y = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at -45°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 2.0;
-  path.poses[0].pose.position.y = -2.0;
-  path.poses[1].pose.position.x = 3.0;
-  path.poses[1].pose.position.y = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON);
-}
-
 TEST(RegulatedPurePursuitTest, lookaheadAPI)
 {
   auto ctrl = std::make_shared<BasicAPIRPP>();
@@ -476,24 +221,6 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI)
   twist.linear.x = 0.0;
   rtn = ctrl->getLookAheadDistanceWrapper(twist);
   EXPECT_EQ(rtn, 0.3);
-
-  // test getLookAheadPoint
-  double dist = 1.0;
-  nav_msgs::msg::Path path;
-  path.poses.resize(10);
-  for (uint i = 0; i != path.poses.size(); i++) {
-    path.poses[i].pose.position.x = static_cast<double>(i);
-  }
-
-  // test exact hits
-  auto pt = ctrl->getLookAheadPointWrapper(dist, path);
-  EXPECT_EQ(pt.pose.position.x, 1.0);
-
-  // test interpolation
-  ctrl->configure(node, name, tf, costmap);
-  dist = 3.8;
-  pt = ctrl->getLookAheadPointWrapper(dist, path);
-  EXPECT_EQ(pt.pose.position.x, 3.8);
 }
 
 TEST(RegulatedPurePursuitTest, rotateTests)
diff --git a/nav2_ros_common/CMakeLists.txt b/nav2_ros_common/CMakeLists.txt
index 13dfd418833..4765454321d 100644
--- a/nav2_ros_common/CMakeLists.txt
+++ b/nav2_ros_common/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_ros_common)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_ros_common/include/nav2_ros_common/service_client.hpp b/nav2_ros_common/include/nav2_ros_common/service_client.hpp
index c1bc2bd8435..1cf32f25fe0 100644
--- a/nav2_ros_common/include/nav2_ros_common/service_client.hpp
+++ b/nav2_ros_common/include/nav2_ros_common/service_client.hpp
@@ -218,6 +218,16 @@ class ServiceClient
     return service_name_;
   }
 
+  /**
+  * @brief Stop any running spin operations on the internal executor
+  */
+  void stop()
+  {
+    if (client_) {
+      callback_group_executor_->cancel();
+    }
+  }
+
 protected:
   std::string service_name_;
   rclcpp::Clock::SharedPtr clock_;
diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt
index 0adcf17d8f1..68b64fdc0a8 100644
--- a/nav2_rotation_shim_controller/CMakeLists.txt
+++ b/nav2_rotation_shim_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_rotation_shim_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt
index fcc53119651..db9c139c6f1 100644
--- a/nav2_route/CMakeLists.txt
+++ b/nav2_route/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_route CXX)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index 62e19b2f7a2..b917cbffc33 100644
--- a/nav2_rviz_plugins/CMakeLists.txt
+++ b/nav2_rviz_plugins/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_rviz_plugins)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
index 051818f7c9a..08563b3df41 100644
--- a/nav2_smac_planner/CMakeLists.txt
+++ b/nav2_smac_planner/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_smac_planner)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
index 5831d10189c..f65d18fa2f9 100644
--- a/nav2_smoother/CMakeLists.txt
+++ b/nav2_smoother/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_smoother)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index 88922fc900f..9817f2f3563 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_system_tests)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp
new file mode 100644
index 00000000000..dbe04b1f2f3
--- /dev/null
+++ b/nav2_util/include/nav2_util/controller_utils.hpp
@@ -0,0 +1,54 @@
+// Copyright (c) 2020 Shrijit Singh
+// Copyright (c) 2020 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef NAV2_UTIL__CONTROLLER_UTILS_HPP_
+#define NAV2_UTIL__CONTROLLER_UTILS_HPP_
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+namespace nav2_util
+{
+/**
+* @brief Find the intersection a circle and a line segment.
+* This assumes the circle is centered at the origin.
+* If no intersection is found, a floating point error will occur.
+* @param p1 first endpoint of line segment
+* @param p2 second endpoint of line segment
+* @param r radius of circle
+* @return point of intersection
+*/
+geometry_msgs::msg::Point circleSegmentIntersection(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  double r);
+
+/**
+* @brief Get lookahead point
+* @param lookahead_dist Optimal lookahead distance
+* @param path Current global path
+* @param interpolate_after_goal If true, interpolate the lookahead point after the goal based
+* on the orientation given by the position of the last two pose of the path
+* @return Lookahead point
+*/
+geometry_msgs::msg::PoseStamped getLookAheadPoint(
+  double &, const nav_msgs::msg::Path &,
+  const bool interpolate_after_goal = false);
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__CONTROLLER_UTILS_HPP_
diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp
index 64860f47e87..56eea6b5ccd 100644
--- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp
+++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp
@@ -57,6 +57,12 @@ class LifecycleServiceClient
     }
   }
 
+  ~LifecycleServiceClient()
+  {
+    change_state_.stop();
+    get_state_.stop();
+  }
+
   /// Trigger a state change
   /**
    * Throws std::runtime_error on failure
diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt
index 563742e8b72..99d24ee6348 100644
--- a/nav2_util/src/CMakeLists.txt
+++ b/nav2_util/src/CMakeLists.txt
@@ -3,6 +3,7 @@ add_library(${library_name} SHARED
   lifecycle_service_client.cpp
   string_utils.cpp
   robot_utils.cpp
+  controller_utils.cpp
   odometry_utils.cpp
   array_parser.cpp
 )
diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp
new file mode 100644
index 00000000000..18f2ae42dd3
--- /dev/null
+++ b/nav2_util/src/controller_utils.cpp
@@ -0,0 +1,142 @@
+// Copyright (c) 2020 Shrijit Singh
+// Copyright (c) 2020 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_util/controller_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+namespace nav2_util
+{
+geometry_msgs::msg::Point circleSegmentIntersection(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  double r)
+{
+  // Formula for intersection of a line with a circle centered at the origin,
+  // modified to always return the point that is on the segment between the two points.
+  // https://mathworld.wolfram.com/Circle-LineIntersection.html
+  // This works because the poses are transformed into the robot frame.
+  // This can be derived from solving the system of equations of a line and a circle
+  // which results in something that is just a reformulation of the quadratic formula.
+  // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
+  // https://www.desmos.com/calculator/td5cwbuocd
+  double x1 = p1.x;
+  double x2 = p2.x;
+  double y1 = p1.y;
+  double y2 = p2.y;
+
+  double dx = x2 - x1;
+  double dy = y2 - y1;
+  double dr2 = dx * dx + dy * dy;
+  double D = x1 * y2 - x2 * y1;
+
+  // Augmentation to only return point within segment
+  double d1 = x1 * x1 + y1 * y1;
+  double d2 = x2 * x2 + y2 * y2;
+  double dd = d2 - d1;
+
+  geometry_msgs::msg::Point p;
+  double sqrt_term = std::sqrt(r * r * dr2 - D * D);
+  p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2;
+  p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2;
+  return p;
+}
+
+geometry_msgs::msg::PoseStamped getLookAheadPoint(
+  double & lookahead_dist,
+  const nav_msgs::msg::Path & transformed_plan,
+  const bool interpolate_after_goal)
+{
+  // Find the first pose which is at a distance greater than the lookahead distance
+  // Using distance along the path
+  const auto & poses = transformed_plan.poses;
+  auto goal_pose_it = poses.begin();
+  double d = 0.0;
+
+  bool pose_found = false;
+  for (size_t i = 1; i < poses.size(); i++) {
+    const auto & prev_pose = poses[i - 1].pose.position;
+    const auto & curr_pose = poses[i].pose.position;
+
+    d += std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y);
+    if (d >= lookahead_dist) {
+      goal_pose_it = poses.begin() + i;
+      pose_found = true;
+      break;
+    }
+  }
+
+  if (!pose_found) {
+    goal_pose_it = poses.end();
+  }
+
+  // If the no pose is not far enough, take the last pose
+  if (goal_pose_it == transformed_plan.poses.end()) {
+    if (interpolate_after_goal) {
+      auto last_pose_it = std::prev(transformed_plan.poses.end());
+      auto prev_last_pose_it = std::prev(last_pose_it);
+
+      double end_path_orientation = atan2(
+        last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y,
+        last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x);
+
+      // Project the last segment out to guarantee it is beyond the look ahead
+      // distance
+      auto projected_position = last_pose_it->pose.position;
+      projected_position.x += cos(end_path_orientation) * lookahead_dist;
+      projected_position.y += sin(end_path_orientation) * lookahead_dist;
+
+      // Use the circle intersection to find the position at the correct look
+      // ahead distance
+      const auto interpolated_position = circleSegmentIntersection(
+        last_pose_it->pose.position, projected_position, lookahead_dist);
+
+      geometry_msgs::msg::PoseStamped interpolated_pose;
+      interpolated_pose.header = last_pose_it->header;
+      interpolated_pose.pose.position = interpolated_position;
+
+      return interpolated_pose;
+    } else {
+      lookahead_dist = d;  // Updating lookahead distance since using the final point
+      goal_pose_it = std::prev(transformed_plan.poses.end());
+    }
+  } else if (goal_pose_it != transformed_plan.poses.begin()) {
+    // Find the point on the line segment between the two poses
+    // that is exactly the lookahead distance away from the robot pose (the origin)
+    // This can be found with a closed form for the intersection of a segment and a circle
+    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
+    // and goal_pose is guaranteed to be outside the circle.
+    auto prev_pose_it = std::prev(goal_pose_it);
+    auto point = circleSegmentIntersection(
+      prev_pose_it->pose.position,
+      goal_pose_it->pose.position, lookahead_dist);
+
+    // Calculate orientation towards interpolated position
+    // Convert yaw to quaternion
+    double yaw = atan2(
+      point.y - prev_pose_it->pose.position.y,
+      point.x - prev_pose_it->pose.position.x);
+
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = prev_pose_it->header.frame_id;
+    pose.header.stamp = goal_pose_it->header.stamp;
+    pose.pose.position = point;
+    pose.pose.orientation = geometry_utils::orientationAroundZAxis(yaw);
+    return pose;
+  }
+
+  return *goal_pose_it;
+}
+}  // namespace nav2_util
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index 4527fd8373d..09d7d4a38c0 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -16,6 +16,9 @@ target_link_libraries(test_odometry_utils ${library_name} ${nav_msgs_TARGETS} ${
 ament_add_gtest(test_robot_utils test_robot_utils.cpp)
 target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS})
 
+ament_add_gtest(test_controller_utils test_controller_utils.cpp)
+target_link_libraries(test_controller_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS})
+
 ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp)
 target_include_directories(test_base_footprint_publisher PRIVATE "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/src>")
 
diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp
new file mode 100644
index 00000000000..3c13d8e9bbc
--- /dev/null
+++ b/nav2_util/test/test_controller_utils.cpp
@@ -0,0 +1,265 @@
+// Copyright (c) 2020 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_util/controller_utils.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "gtest/gtest.h"
+
+
+using CircleSegmentIntersectionParam = std::tuple<
+  std::pair<double, double>,
+  std::pair<double, double>,
+  double,
+  std::pair<double, double>
+>;
+
+class CircleSegmentIntersectionTest
+  : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
+{};
+
+TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection)
+{
+  auto pair1 = std::get<0>(GetParam());
+  auto pair2 = std::get<1>(GetParam());
+  auto r = std::get<2>(GetParam());
+  auto expected_pair = std::get<3>(GetParam());
+  auto pair_to_point = [](std::pair<double, double> p) -> geometry_msgs::msg::Point {
+      geometry_msgs::msg::Point point;
+      point.x = p.first;
+      point.y = p.second;
+      point.z = 0.0;
+      return point;
+    };
+  auto p1 = pair_to_point(pair1);
+  auto p2 = pair_to_point(pair2);
+  auto actual = nav2_util::circleSegmentIntersection(p1, p2, r);
+  auto expected_point = pair_to_point(expected_pair);
+  EXPECT_DOUBLE_EQ(actual.x, expected_point.x);
+  EXPECT_DOUBLE_EQ(actual.y, expected_point.y);
+  // Expect that the intersection point is actually r away from the origin
+  EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  InterpolationTest,
+  CircleSegmentIntersectionTest,
+  testing::Values(
+    // Origin to the positive X axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {2.0, 0.0},
+  1.0,
+  {1.0, 0.0}
+},
+    // Origin to the negative X axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-2.0, 0.0},
+  1.0,
+  {-1.0, 0.0}
+},
+    // Origin to the positive Y axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  1.0,
+  {0.0, 1.0}
+},
+    // Origin to the negative Y axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, -2.0},
+  1.0,
+  {0.0, -1.0}
+},
+    // non-origin to the X axis with non-unit circle, with the second point inside
+    CircleSegmentIntersectionParam{
+  {4.0, 0.0},
+  {-1.0, 0.0},
+  2.0,
+  {2.0, 0.0}
+},
+    // non-origin to the Y axis with non-unit circle, with the second point inside
+    CircleSegmentIntersectionParam{
+  {0.0, 4.0},
+  {0.0, -0.5},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the positive X axis, on the circle
+    CircleSegmentIntersectionParam{
+  {2.0, 0.0},
+  {0.0, 0.0},
+  2.0,
+  {2.0, 0.0}
+},
+    // origin to the positive Y axis, on the circle
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the upper-right quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {6.0, 8.0},
+  5.0,
+  {3.0, 4.0}
+},
+    // origin to the lower-left quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-6.0, -8.0},
+  5.0,
+  {-3.0, -4.0}
+},
+    // origin to the upper-left quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-6.0, 8.0},
+  5.0,
+  {-3.0, 4.0}
+},
+    // origin to the lower-right quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {6.0, -8.0},
+  5.0,
+  {3.0, -4.0}
+}
+));
+
+TEST(InterpolationUtils, lookaheadInterpolation)
+{
+  // test Lookahead Point Interpolation
+  double dist = 1.0;
+  nav_msgs::msg::Path path;
+  path.poses.resize(10);
+  for (uint i = 0; i != path.poses.size(); i++) {
+    path.poses[i].pose.position.x = static_cast<double>(i);
+  }
+
+  // test exact hits
+  auto pt = nav2_util::getLookAheadPoint(dist, path);
+  EXPECT_EQ(pt.pose.position.x, 1.0);
+
+  // test interpolation
+  dist = 3.8;
+  pt = nav2_util::getLookAheadPoint(dist, path);
+  EXPECT_EQ(pt.pose.position.x, 3.8);
+}
+
+TEST(InterpolationUtils, lookaheadExtrapolation)
+{
+  // Test extrapolation beyond goal
+  double EPSILON = std::numeric_limits<float>::epsilon();
+
+  nav_msgs::msg::Path path;
+  double lookahead_dist = 10.0;
+  // More than 2 poses
+  path.poses.resize(4);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[1].pose.position.x = 1.0;
+  path.poses[2].pose.position.x = 2.0;
+  path.poses[3].pose.position.x = 3.0;
+  auto pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses fwd
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[1].pose.position.x = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses at 45°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = 3.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at 90°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = 0.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at 135°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = -3.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses back
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[1].pose.position.x = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses at -135°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = -3.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at -90°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = 0.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at -45°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = 3.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON);
+}
diff --git a/nav2_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt
index 916ee93ed2c..f7437297d42 100644
--- a/nav2_velocity_smoother/CMakeLists.txt
+++ b/nav2_velocity_smoother/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_velocity_smoother)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt
index 093fee48a84..e4d6d1866a8 100644
--- a/nav2_voxel_grid/CMakeLists.txt
+++ b/nav2_voxel_grid/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_voxel_grid)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index 17d406adef9..aafd4548a5e 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_waypoint_follower)
 
 find_package(ament_cmake REQUIRED)
diff --git a/navigation2/CMakeLists.txt b/navigation2/CMakeLists.txt
index 3b82e6c0027..ac8cac17ffa 100644
--- a/navigation2/CMakeLists.txt
+++ b/navigation2/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(navigation2)
 
 find_package(ament_cmake REQUIRED)