diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index ce0b6573bd..3e8b52caf4 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -52,7 +52,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
 
-  # add_subdirectory(src/planning)
+  add_subdirectory(src/planning)
   add_subdirectory(src/localization)
   add_subdirectory(src/system)
   add_subdirectory(src/updown)
diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp
index 6a411050e7..f03b8ee784 100644
--- a/nav2_system_tests/src/planning/planner_tester.hpp
+++ b/nav2_system_tests/src/planning/planner_tester.hpp
@@ -67,7 +67,8 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer
     costmap_ros_->getCostmap()->resizeMap(
       prop.size_x, prop.size_y,
       prop.resolution, prop.origin.position.x, prop.origin.position.x);
-    unsigned char * costmap_ptr = costmap_ros_->getCostmap()->getCharMap();
+    // Volatile prevents compiler from treating costmap_ptr as unused or changing its address
+    volatile unsigned char * costmap_ptr = costmap_ros_->getCostmap()->getCharMap();
     delete[] costmap_ptr;
     costmap_ptr = new unsigned char[prop.size_x * prop.size_y];
     std::copy(cm.data.begin(), cm.data.end(), costmap_ptr);
diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py
index 5adfa97f17..6f6f9f2e5e 100755
--- a/nav2_system_tests/src/system/test_multi_robot_launch.py
+++ b/nav2_system_tests/src/system/test_multi_robot_launch.py
@@ -41,9 +41,9 @@ def generate_launch_description():
 
     bringup_dir = get_package_share_directory('nav2_bringup')
     robot1_params_file = os.path.join(bringup_dir,  # noqa: F841
-        'params/nav2_multirobot_params_1.yaml')
+                                      'params/nav2_multirobot_params_1.yaml')
     robot2_params_file = os.path.join(bringup_dir,  # noqa: F841
-        'params/nav2_multirobot_params_2.yaml')
+                                      'params/nav2_multirobot_params_2.yaml')
 
     # Names and poses of the robots
     robots = [