Fix system_tests flaky issue (#1723)

* Return back planning system tests

* Fix testcase failure related to not updating costmap

This appeared while compiler treated costmap pointer
to be unused and optimized it out

* Fix flake8 E128 failure

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)

nav2_system_tests/src/planning/planner_tester.hpp

@@ -67,7 +67,8 @@ public:
     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);