diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp
index c3b406cfd4..713478d24d 100644
--- a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp
+++ b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp
@@ -49,7 +49,7 @@ typedef struct
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero();
+pf_vector_t pf_vector_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_vector_finite(pf_vector_t a);
@@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero();
+pf_matrix_t pf_matrix_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_matrix_finite(pf_matrix_t a);
diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c
index de4e3247d1..9d1f5a8289 100644
--- a/nav2_amcl/src/pf/pf.c
+++ b/nav2_amcl/src/pf/pf.c
@@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   // Workspace
   double m[4], c[2][2];
-  size_t count;
   double weight;
 
   // Cluster the samples
@@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   for (i = 0; i < set->cluster_max_count; i++) {
     cluster = set->clusters + i;
-    cluster->count = 0;
     cluster->weight = 0;
     cluster->mean = pf_vector_zero();
     cluster->cov = pf_matrix_zero();
@@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
   }
 
   // Initialize overall filter stats
-  count = 0;
   weight = 0.0;
   set->mean = pf_vector_zero();
   set->cov = pf_matrix_zero();
@@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
     cluster = set->clusters + cidx;
 
-    cluster->count += 1;
     cluster->weight += sample->weight;
 
-    count += 1;
     weight += sample->weight;
 
     // Compute mean
diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c
index a7a5cd39c6..00fa384060 100644
--- a/nav2_amcl/src/pf/pf_vector.c
+++ b/nav2_amcl/src/pf/pf_vector.c
@@ -35,7 +35,7 @@
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero()
+pf_vector_t pf_vector_zero(void)
 {
   pf_vector_t c;
 
@@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero()
+pf_matrix_t pf_matrix_zero(void)
 {
   int i, j;
   pf_matrix_t c;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
index 9ac2c4f0b4..8851ab660e 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -207,7 +207,7 @@ class BtActionNode : public BT::ActionNodeBase
       // if new goal was sent and action server has not yet responded
       // check the future goal handle
       if (future_goal_handle_) {
-        auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+        auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
         if (!is_future_goal_handle_complete(elapsed)) {
           // return RUNNING if there is still some time before timeout happens
           if (elapsed < server_timeout_) {
@@ -237,7 +237,7 @@ class BtActionNode : public BT::ActionNodeBase
         {
           goal_updated_ = false;
           send_new_goal();
-          auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+          auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
           if (!is_future_goal_handle_complete(elapsed)) {
             if (elapsed < server_timeout_) {
               return BT::NodeStatus::RUNNING;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
index e87ffbfac4..42d27c94ec 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -116,7 +116,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
     return basic;
   }
 
-  void halt()
+  void halt() override
   {
   }
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
index e050ab3038..3537f9e9a2 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -183,7 +183,7 @@ class BtServiceNode : public BT::ActionNodeBase
    */
   virtual BT::NodeStatus check_future()
   {
-    auto elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+    auto elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
     auto remaining = server_timeout_ - elapsed;
 
     if (remaining > std::chrono::milliseconds(0)) {
@@ -199,7 +199,7 @@ class BtServiceNode : public BT::ActionNodeBase
 
       if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
         on_wait_for_result();
-        elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+        elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
         if (elapsed < server_timeout_) {
           return BT::NodeStatus::RUNNING;
         }
diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp
index 211254dafe..a898dec947 100644
--- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp
@@ -95,7 +95,7 @@ void IsStuckCondition::logStuck(const std::string & msg) const
     return;
   }
 
-  RCLCPP_INFO(node_->get_logger(), msg.c_str());
+  RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str());
   prev_msg = msg;
 }
 
diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp
index e5c96eba2c..1ab530dab1 100644
--- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp
+++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp
@@ -43,7 +43,7 @@ SpeedController::SpeedController(
 
   if (min_rate_ <= 0.0 || max_rate_ <= 0.0) {
     std::string err_msg = "SpeedController node cannot have rate <= 0.0";
-    RCLCPP_FATAL(node_->get_logger(), err_msg.c_str());
+    RCLCPP_FATAL(node_->get_logger(), "%s" ,err_msg.c_str());
     throw BT::BehaviorTreeException(err_msg);
   }
 
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
index 98287b5be8..32dc1cd144 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
@@ -72,7 +72,7 @@ class NavigateToPoseNavigator
    * @brief Get action name for this navigator
    * @return string Name of action server
    */
-  std::string getName() {return std::string("navigate_to_pose");}
+  std::string getName() override {return std::string("navigate_to_pose");}
 
   /**
    * @brief Get navigator's default BT
diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
index 463af33fd0..1ccae77f77 100644
--- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp
+++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
@@ -100,7 +100,7 @@ class SmootherTest : public ::testing::Test
   SmootherTest() {SetUp();}
   ~SmootherTest() {}
 
-  void SetUp()
+  void SetUp() override
   {
     node_lifecycle_ =
       std::make_shared<rclcpp_lifecycle::LifecycleNode>(
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index a0bd0cec14..6c96915c9b 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -534,7 +534,7 @@ void ControllerServer::computeAndPublishVelocity()
     // other types will not be resolved with more attempts
   } catch (nav2_core::NoValidControl & e) {
     if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
-      RCLCPP_WARN(this->get_logger(), e.what());
+      RCLCPP_WARN(this->get_logger(), "%s", e.what());
       cmd_vel_2d.twist.angular.x = 0;
       cmd_vel_2d.twist.angular.y = 0;
       cmd_vel_2d.twist.angular.z = 0;
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 ee79894847..75c720cbaa 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
@@ -986,9 +986,9 @@ class GroupsRemover
     const IsBg & is_background) const
   {
     // Creates an image labels in which each obstacles group is labeled with a unique code
-    auto components = connectedComponents<connectivity>(image, buffer, label_trees, is_background);
-    const Label groups_count = components.second;
-    const Image<Label> & labels = components.first;
+    Label groups_count;
+    auto labels = connectedComponents<connectivity>(image, buffer, label_trees,
+        is_background, groups_count);
 
     // Calculates the size of each group.
     // Group size is equal to the number of pixels with the same label
@@ -1026,24 +1026,27 @@ class GroupsRemover
 }  // namespace imgproc_impl
 
 template<ConnectivityType connectivity, class Label, class IsBg>
-std::pair<Image<Label>, Label> connectedComponents(
+Image<Label> connectedComponents(
   const Image<uint8_t> & image, MemoryBuffer & buffer,
-  imgproc_impl::EquivalenceLabelTrees<Label> & label_trees, const IsBg & is_background)
+  imgproc_impl::EquivalenceLabelTrees<Label> & label_trees,
+  const IsBg & is_background,
+  Label & total_labels)
 {
   using namespace imgproc_impl;
   const size_t pixels = image.rows() * image.columns();
 
   if (pixels == 0) {
-    return {Image<Label>{}, 0};
+    total_labels = 0;
+    return Image<Label>{};
   }
 
   Label * image_buffer = buffer.get<Label>(pixels);
   Image<Label> labels(image.rows(), image.columns(), image_buffer, image.columns());
   label_trees.reset(image.rows(), image.columns(), connectivity);
-  const Label total_labels = connectedComponentsImpl<connectivity>(
+  total_labels = connectedComponentsImpl<connectivity>(
     image, labels, label_trees,
     is_background);
-  return std::make_pair(labels, total_labels);
+  return labels;
 }
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
index 48d126577d..2855e367c8 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
@@ -131,7 +131,7 @@ class InflationLayer : public Layer
   /**
    * @brief If clearing operations should be processed on this layer or not
    */
-  virtual bool isClearable() {return false;}
+  virtual bool isClearable() override {return false;}
 
   /**
    * @brief Reset this costmap
diff --git a/nav2_costmap_2d/plugins/denoise_layer.cpp b/nav2_costmap_2d/plugins/denoise_layer.cpp
index b8f6c427a6..bb126d524e 100644
--- a/nav2_costmap_2d/plugins/denoise_layer.cpp
+++ b/nav2_costmap_2d/plugins/denoise_layer.cpp
@@ -122,7 +122,7 @@ DenoiseLayer::updateCosts(
   try {
     denoise(roi_image);
   } catch (std::exception & ex) {
-    RCLCPP_ERROR(logger_, (std::string("Inner error: ") + ex.what()).c_str());
+    RCLCPP_ERROR(logger_, "%s", (std::string("Inner error: ") + ex.what()).c_str());
   }
 
   current_ = true;
diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
index 18039f4298..61c2388112 100644
--- a/nav2_costmap_2d/src/clear_costmap_service.cpp
+++ b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -67,7 +67,7 @@ void ClearCostmapService::clearExceptRegionCallback(
   const shared_ptr<ClearExceptRegion::Response>/*response*/)
 {
   RCLCPP_INFO(
-    logger_,
+    logger_, "%s",
     ("Received request to clear except a region the " + costmap_.getName()).c_str());
 
   clearRegion(request->reset_distance, true);
@@ -87,7 +87,7 @@ void ClearCostmapService::clearEntireCallback(
   const std::shared_ptr<ClearEntirely::Response>/*response*/)
 {
   RCLCPP_INFO(
-    logger_,
+    logger_, "%s",
     ("Received request to clear entirely the " + costmap_.getName()).c_str());
 
   clearEntirely();
@@ -99,7 +99,7 @@ void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
 
   if (!getPosition(x, y)) {
     RCLCPP_ERROR(
-      logger_,
+      logger_, "%s",
       "Cannot clear map because robot pose cannot be retrieved.");
     return;
   }
diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt
index c9a52aa8af..78fbfa6cf7 100644
--- a/nav2_costmap_2d/test/regression/CMakeLists.txt
+++ b/nav2_costmap_2d/test/regression/CMakeLists.txt
@@ -10,6 +10,9 @@ add_library(order_layer SHARED
 ament_target_dependencies(order_layer
   ${dependencies}
 )
+target_link_libraries(order_layer
+  nav2_costmap_2d_core
+)
 install(TARGETS
   order_layer
   ARCHIVE DESTINATION lib
diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
index 2c4a2b9840..43b21d05bb 100644
--- a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
+++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
@@ -407,7 +407,8 @@ TEST_F(DenoiseLayerTester, denoiseNothing) {
 
 TEST_F(DenoiseLayerTester, constructorAndDestructor) {
   ASSERT_NO_THROW(
-    []() {
+    // []()
+    {
       nav2_costmap_2d::DenoiseLayer layer;
     });
 }
diff --git a/nav2_costmap_2d/test/unit/image_processing_test.cpp b/nav2_costmap_2d/test/unit/image_processing_test.cpp
index 957fcf2732..7206575dee 100644
--- a/nav2_costmap_2d/test/unit/image_processing_test.cpp
+++ b/nav2_costmap_2d/test/unit/image_processing_test.cpp
@@ -139,7 +139,7 @@ Image<uint8_t> ConnectedComponentsTester::makeChessboardLikeImage(
 {
   Image<uint8_t> image = makeImage<uint8_t>(rows, cols, buffer, cols * 3);
 
-  auto inverse = [this](uint8_t v) {
+  auto inverse = [](uint8_t v) {
       return (v == BACKGROUND_CODE) ? FOREGROUND_CODE : BACKGROUND_CODE;
     };
 
@@ -161,71 +161,77 @@ Image<uint8_t> ConnectedComponentsTester::makeChessboardLikeImage(
 
 TEST_F(ConnectedComponentsTester, way4EmptyTest) {
   Image<uint8_t> empty;
-  const auto result = connectedComponents<ConnectivityType::Way4>(
+  uint8_t total_labels;
+  connectedComponents<ConnectivityType::Way4>(
     empty, buffer_, label_trees_,
-    isBackground);
-  ASSERT_EQ(result.second, uint8_t(0));
+    isBackground, total_labels);
+  ASSERT_EQ(total_labels, uint8_t(0));
 }
 
 TEST_F(ConnectedComponentsTester, way4SinglePixelTest) {
   Image<uint8_t> input = makeImage(1, 1, image_buffer_bytes_);
+  uint8_t total_labels;
   {
     input.row(0)[0] = BACKGROUND_CODE;
 
     const auto result = connectedComponents<ConnectivityType::Way4>(
       input, buffer_, label_trees_,
-      isBackground);
+      isBackground, total_labels);
 
-    ASSERT_EQ(result.first.row(0)[0], 0);
-    ASSERT_EQ(result.second, 1);
+    ASSERT_EQ(result.row(0)[0], 0);
+    ASSERT_EQ(total_labels, 1);
   }
   {
     input.row(0)[0] = FOREGROUND_CODE;
 
     const auto result = connectedComponents<ConnectivityType::Way4>(
       input, buffer_, label_trees_,
-      isBackground);
+      isBackground, total_labels);
 
-    ASSERT_EQ(result.first.row(0)[0], 1);
-    ASSERT_EQ(result.second, 2);
+    ASSERT_EQ(result.row(0)[0], 1);
+    ASSERT_EQ(total_labels, 2);
   }
 }
 
 TEST_F(ConnectedComponentsTester, way4ImageSmallTest) {
   {
     Image<uint8_t> input = makeImage(1, 2, image_buffer_bytes_);
+    uint8_t total_labels;
     input.row(0)[0] = BACKGROUND_CODE;
     input.row(0)[1] = FOREGROUND_CODE;
 
     const auto result = connectedComponents<ConnectivityType::Way4>(
       input, buffer_, label_trees_,
-      isBackground);
+      isBackground, total_labels);
 
-    ASSERT_EQ(result.second, uint8_t(2));
-    ASSERT_EQ(result.first.row(0)[0], 0);
-    ASSERT_EQ(result.first.row(0)[1], 1);
+    ASSERT_EQ(total_labels, uint8_t(2));
+    ASSERT_EQ(result.row(0)[0], 0);
+    ASSERT_EQ(result.row(0)[1], 1);
   }
   {
     Image<uint8_t> input = makeImage(2, 1, image_buffer_bytes_);
+    uint8_t total_labels;
     input.row(0)[0] = BACKGROUND_CODE;
     input.row(1)[0] = FOREGROUND_CODE;
 
     const auto result = connectedComponents<ConnectivityType::Way4>(
       input, buffer_, label_trees_,
-      isBackground);
+      isBackground, total_labels);
 
-    ASSERT_EQ(result.second, uint8_t(2));
-    ASSERT_EQ(result.first.row(0)[0], 0);
-    ASSERT_EQ(result.first.row(1)[0], 1);
+    ASSERT_EQ(total_labels, uint8_t(2));
+    ASSERT_EQ(result.row(0)[0], 0);
+    ASSERT_EQ(result.row(1)[0], 1);
   }
 }
 
 TEST_F(ConnectedComponentsTester, way4LabelsOverflowTest) {
   // big chessboard image
   Image<uint8_t> input = makeChessboardLikeImage(32, 17, image_buffer_bytes_);
+  uint8_t total_labels;
 
   ASSERT_THROW(
-    (connectedComponents<ConnectivityType::Way4>(input, buffer_, label_trees_, isBackground)),
+    (connectedComponents<ConnectivityType::Way4>(input, buffer_, label_trees_,
+        isBackground, total_labels)),
     LabelOverflow);
 }
 
@@ -273,12 +279,13 @@ TEST_F(ConnectedComponentsTester, way4ImageStepsTest) {
     ".xx."
     "xx.."
     "....", image_buffer_bytes2_);
+  uint8_t total_labels;
   const auto result = connectedComponents<ConnectivityType::Way4>(
     input, buffer_, label_trees_,
-    isBackground);
+    isBackground, total_labels);
 
-  ASSERT_EQ(result.second, uint8_t(2));
-  ASSERT_TRUE(isEqualLabels(result.first, expected_labels));
+  ASSERT_EQ(total_labels, uint8_t(2));
+  ASSERT_TRUE(isEqualLabels(result, expected_labels));
 }
 
 /// @brief create mapping '.'->0, 'a'->1, 'b'->2, ... max_symbol->n
@@ -307,13 +314,14 @@ TEST_F(ConnectedComponentsTester, way8ImageStepsTest) {
     "...bb."
     ".....b"
     "....b.", image_buffer_bytes2_, makeLabelsMap('b'));
+  uint8_t total_labels;
 
   const auto result = connectedComponents<ConnectivityType::Way8>(
     input, buffer_, label_trees_,
-    isBackground);
+    isBackground, total_labels);
 
-  ASSERT_EQ(result.second, uint8_t(3));
-  ASSERT_TRUE(isEqualLabels(result.first, expected_labels));
+  ASSERT_EQ(total_labels, uint8_t(3));
+  ASSERT_TRUE(isEqualLabels(result, expected_labels));
 }
 
 TEST_F(ConnectedComponentsTester, way4ImageSieveTest) {
@@ -329,13 +337,14 @@ TEST_F(ConnectedComponentsTester, way4ImageSieveTest) {
     "f.g.h"
     ".i.j."
     "k.l.m", image_buffer_bytes2_, makeLabelsMap('m'));
+  uint8_t total_labels;
 
   const auto result = connectedComponents<ConnectivityType::Way4>(
     input, buffer_, label_trees_,
-    isBackground);
+    isBackground, total_labels);
 
-  ASSERT_EQ(result.second, uint8_t(14));
-  ASSERT_TRUE(isEqualLabels(result.first, expected_labels));
+  ASSERT_EQ(total_labels, uint8_t(14));
+  ASSERT_TRUE(isEqualLabels(result, expected_labels));
 }
 
 template<ConnectivityType connectivity>
@@ -353,10 +362,12 @@ bool ConnectedComponentsTester::fingerTest()
     "..b.c"
     "a.b.c"
     "a.b.c", image_buffer_bytes2_, makeLabelsMap('c'));
+  uint8_t total_labels;
 
-  const auto result = connectedComponents<connectivity>(input, buffer_, label_trees_, isBackground);
+  const auto result = connectedComponents<connectivity>(input, buffer_,
+      label_trees_, isBackground, total_labels);
 
-  return result.second == 4 && isEqualLabels(result.first, expected_labels);
+  return total_labels == 4 && isEqualLabels(result, expected_labels);
 }
 
 TEST_F(ConnectedComponentsTester, way4ImageFingerTest) {
@@ -386,9 +397,11 @@ bool ConnectedComponentsTester::spiralTest()
     ".x.xx.x"
     ".x....x"
     ".xxxxxx", image_buffer_bytes2_);
+  uint8_t total_labels;
 
-  const auto result = connectedComponents<connectivity>(input, buffer_, label_trees_, isBackground);
-  return result.second == 2 && isEqualLabels(result.first, expected_labels);
+  const auto result = connectedComponents<connectivity>(input, buffer_,
+      label_trees_, isBackground, total_labels);
+  return total_labels == 2 && isEqualLabels(result, expected_labels);
 }
 
 TEST_F(ConnectedComponentsTester, way4ImageSpiralTest) {
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 eac7e2bc16..e04abd7d40 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
@@ -63,7 +63,7 @@ TEST(path_ops_test, AdjustResolutionSimple)
 
   nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, RESOLUTION);
   float length = 100;
-  ulong number_of_points = ceil(length / (2 * RESOLUTION));
+  uint32_t number_of_points = ceil(length / (2 * RESOLUTION));
   EXPECT_EQ(out.poses.size(), number_of_points);
   float max_length = length / (number_of_points - 1);
 
diff --git a/nav2_map_server/test/component/CMakeLists.txt b/nav2_map_server/test/component/CMakeLists.txt
index a394a5201e..58ddbac211 100644
--- a/nav2_map_server/test/component/CMakeLists.txt
+++ b/nav2_map_server/test/component/CMakeLists.txt
@@ -8,7 +8,6 @@ ament_add_gtest_executable(test_map_server_node
 ament_target_dependencies(test_map_server_node rclcpp nav_msgs)
 target_link_libraries(test_map_server_node
   ${library_name}
-  stdc++fs
 )
 
 ament_add_test(test_map_server_node
@@ -30,7 +29,6 @@ ament_add_gtest_executable(test_map_saver_node
 ament_target_dependencies(test_map_saver_node rclcpp nav_msgs)
 target_link_libraries(test_map_saver_node
   ${library_name}
-  stdc++fs
 )
 
 add_executable(test_map_saver_publisher
@@ -40,7 +38,6 @@ add_executable(test_map_saver_publisher
 
 target_link_libraries(test_map_saver_publisher
   ${map_io_library_name}
-  stdc++fs
 )
 
 ament_add_test(test_map_saver_node
diff --git a/nav2_map_server/test/component/test_map_saver_node.cpp b/nav2_map_server/test/component/test_map_saver_node.cpp
index 909675ae8a..df36f94408 100644
--- a/nav2_map_server/test/component/test_map_saver_node.cpp
+++ b/nav2_map_server/test/component/test_map_saver_node.cpp
@@ -18,7 +18,7 @@
 
 #include <string>
 #include <memory>
-#include <experimental/filesystem>  // NOLINT
+#include <filesystem>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -29,7 +29,7 @@
 
 #define TEST_DIR TEST_DIRECTORY
 
-using std::experimental::filesystem::path;
+using std::filesystem::path;
 using lifecycle_msgs::msg::Transition;
 using namespace nav2_map_server;  // NOLINT
 
diff --git a/nav2_map_server/test/component/test_map_saver_publisher.cpp b/nav2_map_server/test/component/test_map_saver_publisher.cpp
index 9e69672cde..9ae644c3f7 100644
--- a/nav2_map_server/test/component/test_map_saver_publisher.cpp
+++ b/nav2_map_server/test/component/test_map_saver_publisher.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <experimental/filesystem>
+#include <filesystem>
 #include <string>
 #include <memory>
 
@@ -23,7 +23,7 @@
 #define TEST_DIR TEST_DIRECTORY
 
 using namespace nav2_map_server;  // NOLINT
-using std::experimental::filesystem::path;
+using std::filesystem::path;
 
 class TestPublisher : public rclcpp::Node
 {
diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp
index f47dfbf718..d37427c735 100644
--- a/nav2_map_server/test/component/test_map_server_node.cpp
+++ b/nav2_map_server/test/component/test_map_server_node.cpp
@@ -16,7 +16,7 @@
 
 #include <string>
 #include <memory>
-#include <experimental/filesystem>  // NOLINT
+#include <filesystem>
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -29,7 +29,7 @@ using namespace rclcpp;  // NOLINT
 
 #define TEST_DIR TEST_DIRECTORY
 
-using std::experimental::filesystem::path;
+using std::filesystem::path;
 
 using lifecycle_msgs::msg::Transition;
 
diff --git a/nav2_map_server/test/map_saver_cli/CMakeLists.txt b/nav2_map_server/test/map_saver_cli/CMakeLists.txt
index f7e3b52950..01859094db 100644
--- a/nav2_map_server/test/map_saver_cli/CMakeLists.txt
+++ b/nav2_map_server/test/map_saver_cli/CMakeLists.txt
@@ -8,6 +8,5 @@ ament_add_gtest(test_map_saver_cli
 
 ament_target_dependencies(test_map_saver_cli rclcpp nav_msgs)
 target_link_libraries(test_map_saver_cli
-  stdc++fs
   ${dependencies}
 )
diff --git a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp
index 5b0c3a0339..79cf58997f 100644
--- a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp
+++ b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp
@@ -13,7 +13,7 @@
 // limitations under the License.
 
 #include <gtest/gtest.h>
-#include <experimental/filesystem>
+#include <filesystem>
 #include <string>
 #include <memory>
 #include <utility>
@@ -61,7 +61,7 @@ TEST(MapSaverCLI, CLITest)
   // succeed on real map
   RCLCPP_INFO(node->get_logger(), "Calling saver...");
 
-  EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml"));
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
 
   std::string command =
     std::string(
@@ -73,20 +73,20 @@ TEST(MapSaverCLI, CLITest)
 
   RCLCPP_INFO(node->get_logger(), "Checking on file...");
 
-  EXPECT_TRUE(std::experimental::filesystem::exists(file_path + ".pgm"));
-  EXPECT_EQ(std::experimental::filesystem::file_size(file_path + ".pgm"), 20ul);
+  EXPECT_TRUE(std::filesystem::exists(file_path + ".pgm"));
+  EXPECT_EQ(std::filesystem::file_size(file_path + ".pgm"), 20ul);
 
-  if (std::experimental::filesystem::exists(file_path + ".yaml")) {
-    std::experimental::filesystem::remove(file_path + ".yaml");
+  if (std::filesystem::exists(file_path + ".yaml")) {
+    std::filesystem::remove(file_path + ".yaml");
   }
-  if (std::experimental::filesystem::exists(file_path + ".pgm")) {
-    std::experimental::filesystem::remove(file_path + ".pgm");
+  if (std::filesystem::exists(file_path + ".pgm")) {
+    std::filesystem::remove(file_path + ".pgm");
   }
 
   // fail on bogus map
   RCLCPP_INFO(node->get_logger(), "Calling saver...");
 
-  EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml"));
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
 
   command =
     std::string(
@@ -100,7 +100,7 @@ TEST(MapSaverCLI, CLITest)
 
   RCLCPP_INFO(node->get_logger(), "Checking on file...");
 
-  EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml"));
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
 
   RCLCPP_INFO(node->get_logger(), "Testing help...");
   command =
diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt
index 095a6509ae..936c59ed89 100644
--- a/nav2_map_server/test/unit/CMakeLists.txt
+++ b/nav2_map_server/test/unit/CMakeLists.txt
@@ -7,7 +7,6 @@ ament_target_dependencies(test_map_io rclcpp nav_msgs)
 
 target_link_libraries(test_map_io
   ${map_io_library_name}
-  stdc++fs
 )
 
 # costmap_filter_info_server unit test
diff --git a/nav2_map_server/test/unit/test_map_io.cpp b/nav2_map_server/test/unit/test_map_io.cpp
index 419ce669ef..acc8d9debd 100644
--- a/nav2_map_server/test/unit/test_map_io.cpp
+++ b/nav2_map_server/test/unit/test_map_io.cpp
@@ -32,7 +32,7 @@
 /* Author: Brian Gerkey */
 
 #include <gtest/gtest.h>
-#include <experimental/filesystem>
+#include <filesystem>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -50,7 +50,7 @@
 
 using namespace std;  // NOLINT
 using namespace nav2_map_server;  // NOLINT
-using std::experimental::filesystem::path;
+using std::filesystem::path;
 
 class RclCppFixture
 {
diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp
index 45cf720370..e16cdafb6a 100644
--- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp
+++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp
@@ -389,7 +389,7 @@ TEST(OptimizerTests, shiftControlSequenceTests)
 
   // Test shiftControlSequence by setting the 2nd value to something unique to neighbors
   auto & sequence = optimizer_tester.grabControlSequence();
-  sequence.reset({100});
+  sequence.reset(100);
   sequence.vx(0) = 9999;
   sequence.vx(1) = 6;
   sequence.vx(2) = 888;
diff --git a/nav2_mppi_controller/test/utils/factory.hpp b/nav2_mppi_controller/test/utils/factory.hpp
index 2940465184..804cecb151 100644
--- a/nav2_mppi_controller/test/utils/factory.hpp
+++ b/nav2_mppi_controller/test/utils/factory.hpp
@@ -36,7 +36,9 @@
 
 namespace detail
 {
-auto setHeader(auto && msg, auto node, std::string frame)
+
+template<typename TMessage, typename TNode>
+void setHeader(TMessage && msg, TNode node, std::string frame)
 {
   auto time = node->get_clock()->now();
   msg.header.frame_id = frame;
@@ -145,9 +147,10 @@ getDummyNode(rclcpp::NodeOptions options, std::string node_name = std::string("d
   return node;
 }
 
+template<typename TNode, typename TCostMap, typename TParamHandler>
 std::shared_ptr<mppi::Optimizer> getDummyOptimizer(
-  auto node, auto costmap_ros,
-  auto * params_handler)
+  TNode node, TCostMap costmap_ros,
+  TParamHandler * params_handler)
 {
   std::shared_ptr<mppi::Optimizer> optimizer = std::make_shared<mppi::Optimizer>();
   std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
@@ -157,9 +160,10 @@ std::shared_ptr<mppi::Optimizer> getDummyOptimizer(
   return optimizer;
 }
 
+template<typename TNode, typename TCostMap, typename TFBuffer, typename TParamHandler>
 mppi::PathHandler getDummyPathHandler(
-  auto node, auto costmap_ros, auto tf_buffer,
-  auto * params_handler)
+  TNode node, TCostMap costmap_ros, TFBuffer tf_buffer,
+  TParamHandler * params_handler)
 {
   mppi::PathHandler path_handler;
   std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
@@ -169,9 +173,10 @@ mppi::PathHandler getDummyPathHandler(
   return path_handler;
 }
 
+template<typename TNode, typename TCostMap, typename TFBuffer>
 std::shared_ptr<nav2_mppi_controller::MPPIController> getDummyController(
-  auto node, auto tf_buffer,
-  auto costmap_ros)
+  TNode node, TFBuffer tf_buffer,
+  TCostMap costmap_ros)
 {
   auto controller = std::make_shared<nav2_mppi_controller::MPPIController>();
   std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
@@ -187,8 +192,9 @@ auto getDummyTwist()
   return twist;
 }
 
+template<typename TNode>
 geometry_msgs::msg::PoseStamped
-getDummyPointStamped(auto & node, std::string frame = std::string("odom"))
+getDummyPointStamped(TNode & node, std::string frame = std::string("odom"))
 {
   geometry_msgs::msg::PoseStamped point;
   detail::setHeader(point, node, frame);
@@ -196,7 +202,8 @@ getDummyPointStamped(auto & node, std::string frame = std::string("odom"))
   return point;
 }
 
-geometry_msgs::msg::PoseStamped getDummyPointStamped(auto & node, TestPose pose)
+template<typename TNode>
+geometry_msgs::msg::PoseStamped getDummyPointStamped(TNode & node, TestPose pose)
 {
   geometry_msgs::msg::PoseStamped point = getDummyPointStamped(node);
   point.pose.position.x = pose.x;
@@ -205,14 +212,16 @@ geometry_msgs::msg::PoseStamped getDummyPointStamped(auto & node, TestPose pose)
   return point;
 }
 
-nav_msgs::msg::Path getDummyPath(auto node, std::string frame = std::string("odom"))
+template<typename TNode>
+nav_msgs::msg::Path getDummyPath(TNode node, std::string frame = std::string("odom"))
 {
   nav_msgs::msg::Path path;
   detail::setHeader(path, node, frame);
   return path;
 }
 
-auto getDummyPath(size_t points_count, auto node)
+template<typename TNode>
+auto getDummyPath(size_t points_count, TNode node)
 {
   auto path = getDummyPath(node);
 
@@ -223,7 +232,8 @@ auto getDummyPath(size_t points_count, auto node)
   return path;
 }
 
-nav_msgs::msg::Path getIncrementalDummyPath(auto node, TestPathSettings s)
+template<typename TNode>
+nav_msgs::msg::Path getIncrementalDummyPath(TNode node, TestPathSettings s)
 {
   auto path = getDummyPath(node);
 
diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp
index 4c59089ef8..0093492706 100644
--- a/nav2_mppi_controller/test/utils/utils.hpp
+++ b/nav2_mppi_controller/test/utils/utils.hpp
@@ -31,7 +31,8 @@
 
 using namespace std::chrono_literals;  // NOLINT
 
-void waitSome(const std::chrono::nanoseconds & duration, auto & node)
+template<typename TNode>
+void waitSome(const std::chrono::nanoseconds & duration, TNode & node)
 {
   rclcpp::Time start_time = node->now();
   while (rclcpp::ok() && node->now() - start_time <= rclcpp::Duration(duration)) {
@@ -86,8 +87,9 @@ void printMap(const nav2_costmap_2d::Costmap2D & costmap)
  * @param trajectory trajectory container (xt::tensor) to be printed.
  * @param goal_point goal point to be printed.
  */
+template<typename TTrajectory>
 void printMapWithTrajectoryAndGoal(
-  nav2_costmap_2d::Costmap2D & costmap, const auto & trajectory,
+  nav2_costmap_2d::Costmap2D & costmap, const TTrajectory & trajectory,
   const geometry_msgs::msg::PoseStamped & goal)
 {
   const unsigned int trajectory_cost = 1;
@@ -174,7 +176,8 @@ void addObstacle(nav2_costmap_2d::Costmap2D * costmap, TestObstaclesSettings s)
  * @return true - if the trajectory crosses an obstacle on the map, false - if
  * not
  */
-bool inCollision(const auto & trajectory, const nav2_costmap_2d::Costmap2D & costmap)
+template<typename TTrajectory>
+bool inCollision(const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap)
 {
   unsigned int point_mx = 0;
   unsigned int point_my = 0;
@@ -198,8 +201,9 @@ unsigned char getCost(const nav2_costmap_2d::Costmap2D & costmap, double x, doub
   return costmap.getCost(point_mx, point_my);
 }
 
+template<typename TTrajectory>
 bool isGoalReached(
-  const auto & trajectory, const nav2_costmap_2d::Costmap2D & costmap,
+  const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap,
   const geometry_msgs::msg::PoseStamped & goal)
 {
   unsigned int trajectory_j = 0;
diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp
index 38f83b4805..f964181dd9 100644
--- a/nav2_mppi_controller/test/utils_test.cpp
+++ b/nav2_mppi_controller/test/utils_test.cpp
@@ -356,7 +356,7 @@ TEST(UtilsTests, SmootherTest)
   EXPECT_NEAR(history[3].wz, 0.23, 0.02);
 
   // Check that path is smoother
-  float smoothed_val, original_val;
+  float smoothed_val{0}, original_val{0};
   for (unsigned int i = 0; i != noisey_sequence.vx.shape(0); i++) {
     smoothed_val += fabs(noisey_sequence.vx(i) - 0.2);
     smoothed_val += fabs(noisey_sequence.vy(i) - 0.0);
diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp
index e1bc30f80e..1c172436e6 100644
--- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp
+++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp
@@ -109,7 +109,7 @@ TEST(RotationShimControllerTest, lifecycleTransitions)
 
   ctrl->activate();
 
-  ctrl->setSpeedLimit(50.0, 2.0);
+  ctrl->setSpeedLimit(50.0, true);
 
   ctrl->deactivate();
   ctrl->cleanup();
diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp
index 89df4c3a4f..053ff33ba2 100644
--- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp
+++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp
@@ -206,16 +206,15 @@ TEST(SmootherTest, test_sg_smoother_noisey_path)
   EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time));
 
   length = 0;
-  double non_refined_length = 0;
   for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) {
     length += std::hypot(
       noisey_path_refined.poses[i + 1].pose.position.x -
       noisey_path_refined.poses[i].pose.position.x,
       noisey_path_refined.poses[i + 1].pose.position.y -
       noisey_path_refined.poses[i].pose.position.y);
-    non_refined_length += std::hypot(
-      noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x,
-      noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y);
+    // std::hypot(
+    //   noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x,
+    //   noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y);
   }
 
   EXPECT_LT(length, base_length);
diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp
index a84b1cdc32..39427fd5d3 100644
--- a/nav2_smoother/test/test_smoother_server.cpp
+++ b/nav2_smoother/test/test_smoother_server.cpp
@@ -38,8 +38,7 @@ using namespace std::chrono_literals;
 class DummySmoother : public nav2_core::Smoother
 {
 public:
-  DummySmoother()
-  : initialized_(false) {}
+  DummySmoother() {}
 
   ~DummySmoother() {}
 
@@ -81,7 +80,6 @@ class DummySmoother : public nav2_core::Smoother
   }
 
 private:
-  bool initialized_;
   std::string command_;
   std::chrono::system_clock::time_point start_time_;
 };
diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt
index 61355a82c4..49d0c49ee9 100644
--- a/nav2_system_tests/src/planning/CMakeLists.txt
+++ b/nav2_system_tests/src/planning/CMakeLists.txt
@@ -53,7 +53,7 @@ ament_add_gtest(test_planner_plugins
 
 ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies})
 target_link_libraries(test_planner_plugins
-  stdc++fs
+  # stdc++fs
 )
 
 ament_add_gtest(test_planner_is_path_valid
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index a3b46942bc..45a0e81333 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -25,8 +25,6 @@ find_package(pluginlib REQUIRED)
 
 nav2_package()
 
-link_libraries(stdc++fs)
-
 include_directories(
   include
 )
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
index 180acc1101..628bc1836f 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
@@ -22,7 +22,7 @@
 #define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM
 
 
-#include <experimental/filesystem>
+#include <filesystem>
 #include <mutex>
 #include <string>
 #include <exception>
@@ -97,7 +97,7 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
   // to ensure safety when accessing global var curr_frame_
   std::mutex global_mutex_;
   // the taken photos will be saved under this directory
-  std::experimental::filesystem::path save_dir_;
+  std::filesystem::path save_dir_;
   // .png ? .jpg ? or some other well known format
   std::string image_format_;
   // the topic to subscribe in order capture a frame
diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
index 8a6cb74b96..296f8f41de 100644
--- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
+++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
@@ -61,14 +61,14 @@ void PhotoAtWaypoint::initialize(
   // get inputted save directory and make sure it exists, if not log and create  it
   save_dir_ = save_dir_as_string;
   try {
-    if (!std::experimental::filesystem::exists(save_dir_)) {
+    if (!std::filesystem::exists(save_dir_)) {
       RCLCPP_WARN(
         logger_,
         "Provided save directory for photo at waypoint plugin does not exist,"
         "provided directory is: %s, the directory will be created automatically.",
         save_dir_.c_str()
       );
-      if (!std::experimental::filesystem::create_directory(save_dir_)) {
+      if (!std::filesystem::create_directory(save_dir_)) {
         RCLCPP_ERROR(
           logger_,
           "Failed to create directory!: %s required by photo at waypoint plugin, "
@@ -110,10 +110,10 @@ bool PhotoAtWaypoint::processAtWaypoint(
   }
   try {
     // construct the full path to image filename
-    std::experimental::filesystem::path file_name = std::to_string(
+    std::filesystem::path file_name = std::to_string(
       curr_waypoint_index) + "_" +
       std::to_string(curr_pose.header.stamp.sec) + "." + image_format_;
-    std::experimental::filesystem::path full_path_image_path = save_dir_ / file_name;
+    std::filesystem::path full_path_image_path = save_dir_ / file_name;
 
     // save the taken photo at this waypoint to given directory
     std::lock_guard<std::mutex> guard(global_mutex_);
diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp
index da0a0e9944..87d1b2b0c2 100644
--- a/nav2_waypoint_follower/test/test_task_executors.cpp
+++ b/nav2_waypoint_follower/test/test_task_executors.cpp
@@ -71,7 +71,7 @@ TEST(WaypointFollowerTest, InputAtWaypoint)
   auto pub = node->create_publisher<std_msgs::msg::Empty>("input_at_waypoint/input", 1);
   pub->on_activate();
   auto publish_message =
-    [&, this]() -> void
+    [&]() -> void
     {
       rclcpp::Rate(5).sleep();
       auto msg = std::make_unique<std_msgs::msg::Empty>();
@@ -117,7 +117,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint)
   std::unique_lock<std::mutex> lck(mtx, std::defer_lock);
   bool data_published = false;
   auto publish_message =
-    [&, this]() -> void
+    [&]() -> void
     {
       rclcpp::Rate(5).sleep();
       auto msg = std::make_unique<sensor_msgs::msg::Image>();