diff --git a/costmap_cspace/src/costmap_3d.cpp b/costmap_cspace/src/costmap_3d.cpp
index 4a58f8840..c276b88ff 100644
--- a/costmap_cspace/src/costmap_3d.cpp
+++ b/costmap_cspace/src/costmap_3d.cpp
@@ -58,7 +58,8 @@ class Costmap3DOFNode
   costmap_cspace::Costmap3d::Ptr costmap_;
   std::vector<
       std::pair<nav_msgs::OccupancyGrid::ConstPtr,
-                costmap_cspace::Costmap3dLayerBase::Ptr>> map_buffer_;
+                costmap_cspace::Costmap3dLayerBase::Ptr>>
+      map_buffer_;
 
   void cbMap(
       const nav_msgs::OccupancyGrid::ConstPtr& msg,
diff --git a/costmap_cspace/test/src/test_costmap_3d.cpp b/costmap_cspace/test/src/test_costmap_3d.cpp
index fe9eecbc4..662e56550 100644
--- a/costmap_cspace/test/src/test_costmap_3d.cpp
+++ b/costmap_cspace/test/src/test_costmap_3d.cpp
@@ -10,8 +10,8 @@
  *     * Redistributions in binary form must reproduce the above copyright
  *       notice, this list of conditions and the following disclaimer in the
  *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the copyright holder nor the names of its 
- *       contributors may be used to endorse or promote products derived from 
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
  *       this software without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
@@ -61,12 +61,12 @@ const std::string footprint_str(
 //  0 0 0
 const char temp_dir[4][2] =
     {
-      // x, y which must be occupied in the template
-      -1, 0,
-      0, -1,
-      1, 0,
-      0, 1
-    };
+        // x, y which must be occupied in the template
+        {-1, 0},
+        {0, -1},
+        {1, 0},
+        {0, 1},
+};
 
 TEST(Costmap3dLayerFootprint, CSpaceTemplate)
 {
@@ -411,8 +411,8 @@ TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
   const int num_points_base_map = 2;
   const int points_base_map[num_points_base_map][2] =
       {
-        2, 3,
-        4, 4
+          {2, 3},
+          {4, 4},
       };
   for (int i = 0; i < num_points_base_map; ++i)
   {
@@ -422,9 +422,9 @@ TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
   const int num_points_local_map = 3;
   const int points_local_map[num_points_local_map][2] =
       {
-        3, 4,
-        5, 3,
-        4, 4
+          {3, 4},
+          {5, 3},
+          {4, 4},
       };
   for (int i = 0; i < num_points_local_map; ++i)
   {
@@ -437,8 +437,8 @@ TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
   // Overlay local map
   costmap_cspace_msgs::CSpace3DUpdate::Ptr updated(new costmap_cspace_msgs::CSpace3DUpdate);
   auto cb = [&updated](
-      const costmap_cspace::CSpace3DMsg::Ptr map,
-      const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
+                const costmap_cspace::CSpace3DMsg::Ptr map,
+                const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
   {
     updated = update;
     return true;
@@ -481,8 +481,8 @@ TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
   cm_over->setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
   costmap_cspace_msgs::CSpace3DUpdate::Ptr updated_max(new costmap_cspace_msgs::CSpace3DUpdate);
   auto cb_max = [&updated_max](
-      const costmap_cspace::CSpace3DMsg::Ptr map,
-      const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
+                    const costmap_cspace::CSpace3DMsg::Ptr map,
+                    const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
   {
     updated_max = update;
     return true;
@@ -613,32 +613,32 @@ TEST(Costmap3dLayerOutput, CSpaceOutOfBoundary)
   };
   const TestData dataset[] =
       {
-        { "inside0", { 0.0, 0.0 }, true, { 0u, 0u, 0u, 2u, 2u, 4u } },
-        { "inside1", { 1.0, 0.0 }, true, { 1u, 0u, 0u, 2u, 2u, 4u } },
-        { "half-outside x0", { -1.0, 0.0 }, true, { 0u, 0u, 0u, 1u, 2u, 4u } },
-        { "half-outside x1", { 3.0, 0.0 }, true, { 3u, 0u, 0u, 1u, 2u, 4u } },
-        { "half-outside y0", { 0.0, -1.0 }, true, { 0u, 0u, 0u, 2u, 1u, 4u } },
-        { "half-outside y1", { 0.0, 3.0 }, true, { 0u, 3u, 0u, 2u, 1u, 4u } },
-        { "half-outside xy0", { -1.0, -1.0 }, true, { 0u, 0u, 0u, 1u, 1u, 4u } },
-        { "half-outside xy1", { 3.0, -1.0 }, true, { 3u, 0u, 0u, 1u, 1u, 4u } },
-        { "half-outside xy2", { 3.0, 3.0 }, true, { 3u, 3u, 0u, 1u, 1u, 4u } },
-        { "half-outside xy3", { -1.0, 3.0 }, true, { 0u, 3u, 0u, 1u, 1u, 4u } },
-        { "boundary x0", { -2.0, 0.0 }, false },
-        { "boundary x1", { 4, 0.0 }, false },
-        { "boundary y0", { 0, -2.0 }, false },
-        { "boundary y1", { 0, 4.0 }, false },
-        { "boundary xy0", { -2.0, -2.0 }, false },
-        { "boundary xy1", { 4.0, -2.0 }, false },
-        { "boundary xy2", { 4.0, 4.0 }, false },
-        { "boundary xy3", { -2.0, 4.0 }, false },
-        { "outside x0", { -3.0, 0.0 }, false },
-        { "outside x1", { 5, 0.0 }, false },
-        { "outside y0", { 0, -3.0 }, false },
-        { "outside y1", { 0, 5.0 }, false },
-        { "outside xy0", { -3.0, -3.0 }, false },
-        { "outside xy1", { 5.0, -3.0 }, false },
-        { "outside xy2", { 5.0, 5.0 }, false },
-        { "outside xy3", { -3.0, 5.0 }, false },
+          {"inside0", {0.0, 0.0}, true, {0u, 0u, 0u, 2u, 2u, 4u}},
+          {"inside1", {1.0, 0.0}, true, {1u, 0u, 0u, 2u, 2u, 4u}},
+          {"half-outside x0", {-1.0, 0.0}, true, {0u, 0u, 0u, 1u, 2u, 4u}},
+          {"half-outside x1", {3.0, 0.0}, true, {3u, 0u, 0u, 1u, 2u, 4u}},
+          {"half-outside y0", {0.0, -1.0}, true, {0u, 0u, 0u, 2u, 1u, 4u}},
+          {"half-outside y1", {0.0, 3.0}, true, {0u, 3u, 0u, 2u, 1u, 4u}},
+          {"half-outside xy0", {-1.0, -1.0}, true, {0u, 0u, 0u, 1u, 1u, 4u}},
+          {"half-outside xy1", {3.0, -1.0}, true, {3u, 0u, 0u, 1u, 1u, 4u}},
+          {"half-outside xy2", {3.0, 3.0}, true, {3u, 3u, 0u, 1u, 1u, 4u}},
+          {"half-outside xy3", {-1.0, 3.0}, true, {0u, 3u, 0u, 1u, 1u, 4u}},
+          {"boundary x0", {-2.0, 0.0}, false},
+          {"boundary x1", {4, 0.0}, false},
+          {"boundary y0", {0, -2.0}, false},
+          {"boundary y1", {0, 4.0}, false},
+          {"boundary xy0", {-2.0, -2.0}, false},
+          {"boundary xy1", {4.0, -2.0}, false},
+          {"boundary xy2", {4.0, 4.0}, false},
+          {"boundary xy3", {-2.0, 4.0}, false},
+          {"outside x0", {-3.0, 0.0}, false},
+          {"outside x1", {5, 0.0}, false},
+          {"outside y0", {0, -3.0}, false},
+          {"outside y1", {0, 5.0}, false},
+          {"outside xy0", {-3.0, -3.0}, false},
+          {"outside xy1", {5.0, -3.0}, false},
+          {"outside xy2", {5.0, 5.0}, false},
+          {"outside xy3", {-3.0, 5.0}, false},
       };
 
   for (auto& d : dataset)
@@ -675,8 +675,8 @@ TEST(Costmap3dLayerOutput, CSpaceOutOfBoundary)
     // Overlay local map
     costmap_cspace_msgs::CSpace3DUpdate::Ptr updated;
     auto cb = [&updated](
-        const costmap_cspace::CSpace3DMsg::Ptr& map,
-        const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
+                  const costmap_cspace::CSpace3DMsg::Ptr& map,
+                  const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
     {
       updated = update;
       return true;
diff --git a/planner_cspace/include/planner_cspace/bbf.h b/planner_cspace/include/planner_cspace/bbf.h
index 60f021bae..31ca69686 100644
--- a/planner_cspace/include/planner_cspace/bbf.h
+++ b/planner_cspace/include/planner_cspace/bbf.h
@@ -57,7 +57,7 @@ class BinaryBayesFilter
 public:
   explicit BinaryBayesFilter(
       const float& initial_odds = 1.0) noexcept
-      : odds_(initial_odds)
+    : odds_(initial_odds)
   {
   }
   float update(const float& odds)
diff --git a/planner_cspace/include/planner_cspace/grid_astar.h b/planner_cspace/include/planner_cspace/grid_astar.h
index 995e0f6ba..4536a0a32 100644
--- a/planner_cspace/include/planner_cspace/grid_astar.h
+++ b/planner_cspace/include/planner_cspace/grid_astar.h
@@ -278,10 +278,10 @@ class GridAstar
             findPath(ss_normalized, better, path_tmp);
             const SearchStats stats =
                 {
-                  .num_loop = num_loop,
-                  .num_search_queue = num_search_queue,
-                  .num_prev_updates = num_updates,
-                  .num_total_updates = num_total_updates,
+                    .num_loop = num_loop,
+                    .num_search_queue = num_search_queue,
+                    .num_prev_updates = num_updates,
+                    .num_total_updates = num_total_updates,
                 };
             if (!cb_progress(path_tmp, stats))
             {
diff --git a/planner_cspace/src/motion_cache.cpp b/planner_cspace/src/motion_cache.cpp
index 2dbed91d5..f43a0a601 100644
--- a/planner_cspace/src/motion_cache.cpp
+++ b/planner_cspace/src/motion_cache.cpp
@@ -68,9 +68,9 @@ void MotionCache::reset(
           const float yaw_e = d[2] * angular_resolution;
           const float diff_val[3] =
               {
-                d[0] * linear_resolution,
-                d[1] * linear_resolution,
-                d[2] * angular_resolution
+                  d[0] * linear_resolution,
+                  d[1] * linear_resolution,
+                  d[2] * angular_resolution,
               };
           std::unordered_map<CyclicVecInt<3, 2>, bool, CyclicVecInt<3, 2>> registered;
           registered[d] = true;
@@ -133,9 +133,9 @@ void MotionCache::reset(
 
             const float posf_raw[3] =
                 {
-                  (cx2 - r * cosf(cyaw + M_PI / 2)) / linear_resolution,
-                  (cy2 - r * sinf(cyaw + M_PI / 2)) / linear_resolution,
-                  cyaw / angular_resolution
+                    (cx2 - r * cosf(cyaw + M_PI / 2)) / linear_resolution,
+                    (cy2 - r * sinf(cyaw + M_PI / 2)) / linear_resolution,
+                    cyaw / angular_resolution,
                 };
             const CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]);
             CyclicVecInt<3, 2> pos(posf_raw[0], posf_raw[1], posf_raw[2]);
diff --git a/planner_cspace/src/planner_2dof_serial_joints.cpp b/planner_cspace/src/planner_2dof_serial_joints.cpp
index a19e30a28..83cac2b51 100644
--- a/planner_cspace/src/planner_2dof_serial_joints.cpp
+++ b/planner_cspace/src/planner_2dof_serial_joints.cpp
@@ -164,7 +164,7 @@ class Planner2dofSerialJointsNode
 
   void cbJoint(const sensor_msgs::JointState::ConstPtr& msg)
   {
-    int id[2] = { -1, -1 };
+    int id[2] = {-1, -1};
     for (size_t i = 0; i < msg->name.size(); i++)
     {
       if (msg->name[i].compare(links_[0].name_) == 0)
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
index 5270d1347..1b559cfcf 100644
--- a/planner_cspace/src/planner_3d.cpp
+++ b/planner_cspace/src/planner_3d.cpp
@@ -1115,9 +1115,9 @@ class Planner3dNode
 
     const int size[3] =
         {
-          static_cast<int>(map_info_.width),
-          static_cast<int>(map_info_.height),
-          static_cast<int>(map_info_.angle)
+            static_cast<int>(map_info_.width),
+            static_cast<int>(map_info_.height),
+            static_cast<int>(map_info_.angle),
         };
     as_.reset(Astar::Vec(size[0], size[1], size[2]));
     cm_.reset(Astar::Vec(size[0], size[1], size[2]));
@@ -1645,14 +1645,8 @@ class Planner3dNode
     std::vector<Astar::VecWithCost> starts;
     if (antialias_start_)
     {
-      const int x_cand[] =
-          {
-            0, ((sf[0] - s[0]) < 0.5 ? -1 : 1)
-          };
-      const int y_cand[] =
-          {
-            0, ((sf[1] - s[1]) < 0.5 ? -1 : 1)
-          };
+      const int x_cand[] = {0, ((sf[0] - s[0]) < 0.5 ? -1 : 1)};
+      const int y_cand[] = {0, ((sf[1] - s[1]) < 0.5 ? -1 : 1)};
       for (const int x : x_cand)
       {
         for (const int y : y_cand)
diff --git a/planner_cspace/test/src/test_blockmem_gridmap_performance.cpp b/planner_cspace/test/src/test_blockmem_gridmap_performance.cpp
index b1c521778..8cad9caee 100644
--- a/planner_cspace/test/src/test_blockmem_gridmap_performance.cpp
+++ b/planner_cspace/test/src/test_blockmem_gridmap_performance.cpp
@@ -38,14 +38,8 @@ namespace planner_cspace
 {
 TEST(BlockmemGridmap, SpacialAccessPerformance)
 {
-  constexpr int size[3] =
-      {
-        0x420, 0x420, 0x28
-      };
-  constexpr int pad[3] =
-      {
-        0x204, 0x204, 0x10
-      };
+  constexpr int size[3] = {0x420, 0x420, 0x28};
+  constexpr int pad[3] = {0x204, 0x204, 0x10};
   constexpr int range = 0x10;
   constexpr int repeat = 4;
 
diff --git a/planner_cspace/test/src/test_costmap_bbf.cpp b/planner_cspace/test/src/test_costmap_bbf.cpp
index 16fead6b9..6ca2d6d15 100644
--- a/planner_cspace/test/src/test_costmap_bbf.cpp
+++ b/planner_cspace/test/src/test_costmap_bbf.cpp
@@ -74,27 +74,27 @@ TEST(CostmapBBF, Update)
   const float odds_miss = bbf::probabilityToOdds(0.3);
   const char costmap0[3][3] =
       {
-        { 10, 100, 100 },
-        { 100, 100, 100 },
-        { 100, 100, 100 },
+          {10, 100, 100},
+          {100, 100, 100},
+          {100, 100, 100},
       };
   const char costmap1[3][3] =
       {
-        { 0, -1, 0 },
-        { 100, 0, 0 },
-        { 0, 0, 100 },
+          {0, -1, 0},
+          {100, 0, 0},
+          {0, 0, 100},
       };
   const float expected_odds[3][3] =
       {
-        { bbf::MIN_ODDS, bbf::MIN_ODDS * odds_hit, bbf::MIN_ODDS * odds_hit * odds_miss },
-        { bbf::MIN_ODDS * odds_hit * odds_hit, bbf::MIN_ODDS * odds_hit * odds_miss, bbf::MIN_ODDS },
-        { bbf::MIN_ODDS * odds_hit * odds_miss, bbf::MIN_ODDS, bbf::MIN_ODDS },
+          {bbf::MIN_ODDS, bbf::MIN_ODDS * odds_hit, bbf::MIN_ODDS * odds_hit * odds_miss},
+          {bbf::MIN_ODDS * odds_hit * odds_hit, bbf::MIN_ODDS * odds_hit * odds_miss, bbf::MIN_ODDS},
+          {bbf::MIN_ODDS * odds_hit * odds_miss, bbf::MIN_ODDS, bbf::MIN_ODDS},
       };
   const char expected_cost[3][3] =
       {
-        { 0, 26, 8 },
-        { 68, 8, 0 },
-        { 8, 0, 0 },
+          {0, 26, 8},
+          {68, 8, 0},
+          {8, 0, 0},
       };
   BlockMemGridmap<char, 3, 2> cm;
 
diff --git a/planner_cspace/test/src/test_cyclic_vec.cpp b/planner_cspace/test/src/test_cyclic_vec.cpp
index 3c78730d6..5f7d0f175 100644
--- a/planner_cspace/test/src/test_cyclic_vec.cpp
+++ b/planner_cspace/test/src/test_cyclic_vec.cpp
@@ -38,10 +38,7 @@ namespace planner_cspace
 {
 TEST(CyclicVec, InitFloat)
 {
-  const float val[3] =
-      {
-        1.0, 2.0, 3.0
-      };
+  const float val[3] = {1.0, 2.0, 3.0};
   CyclicVecFloat<3, 2> v(1.0f, 2.0f, 3.0f);
   const CyclicVecFloat<3, 2> vc(1.0f, 2.0f, 3.0f);
 
@@ -54,10 +51,7 @@ TEST(CyclicVec, InitFloat)
 
 TEST(CyclicVec, InitInt)
 {
-  const int val[3] =
-      {
-        1, 2, 3
-      };
+  const int val[3] = {1, 2, 3};
   CyclicVecInt<3, 2> v(1, 2, 3);
   const CyclicVecInt<3, 2> vc(1, 2, 3);
 
diff --git a/planner_cspace/test/src/test_debug_outputs.cpp b/planner_cspace/test/src/test_debug_outputs.cpp
index 4ef6e16a8..37f43f633 100644
--- a/planner_cspace/test/src/test_debug_outputs.cpp
+++ b/planner_cspace/test/src/test_debug_outputs.cpp
@@ -160,18 +160,18 @@ TEST_F(DebugOutputsTest, Hysteresis)
   // Robot is at (25, 4) and goal is at (10, 4)
   const PositionAndValue data_set[] =
       {
-        { 25, 4, 0 },
-        { 20, 4, 0 },
-        { 15, 4, 0 },
-        { 10, 4, 0 },
-        { 25, 1, 100 },
-        { 20, 1, 100 },
-        { 15, 1, 100 },
-        { 10, 1, 100 },
-        { 25, 7, 100 },
-        { 20, 7, 100 },
-        { 15, 7, 100 },
-        { 10, 7, 100 },
+          {25, 4, 0},
+          {20, 4, 0},
+          {15, 4, 0},
+          {10, 4, 0},
+          {25, 1, 100},
+          {20, 1, 100},
+          {15, 1, 100},
+          {10, 1, 100},
+          {25, 7, 100},
+          {20, 7, 100},
+          {15, 7, 100},
+          {10, 7, 100},
       };
   for (auto data : data_set)
   {
@@ -193,12 +193,12 @@ TEST_F(DebugOutputsTest, Remembered)
   // Costmap is expanded by 1 grid.
   const PositionAndValue data_set[] =
       {
-        { 17, 0, 100 },   // occupied
-        { 17, 1, 100 },   // expanded
-        { 17, 2, 0 },     // free
-        { 17, 8, 0 },     // free
-        { 17, 9, 100 },   // expanded
-        { 17, 10, 100 },  // occupied
+          {17, 0, 100},   // occupied
+          {17, 1, 100},   // expanded
+          {17, 2, 0},     // free
+          {17, 8, 0},     // free
+          {17, 9, 100},   // expanded
+          {17, 10, 100},  // occupied
       };
   for (auto data : data_set)
   {
diff --git a/planner_cspace/test/src/test_grid_metric_converter.cpp b/planner_cspace/test/src/test_grid_metric_converter.cpp
index 65da7f9c1..509972a75 100644
--- a/planner_cspace/test/src/test_grid_metric_converter.cpp
+++ b/planner_cspace/test/src/test_grid_metric_converter.cpp
@@ -51,23 +51,23 @@ TEST(GridMetricConverter, SinglePose)
 
   const float metric[][3] =
       {
-        { 100.01, 100.01, M_PI / 8 },
-        { 100.01, 100.01, -M_PI / 8 },
-        { 105.01, 102.09, M_PI / 2 },
-        { 100.21, 102.51, -M_PI / 2 },
-        { 99.99, 100.01, 0 },
-        { 100.01, 100.01, 2.1 * M_PI / 8 },
-        { 100.01, 100.01, 1.9 * M_PI / 8 }
+          {100.01, 100.01, M_PI / 8},
+          {100.01, 100.01, -M_PI / 8},
+          {105.01, 102.09, M_PI / 2},
+          {100.21, 102.51, -M_PI / 2},
+          {99.99, 100.01, 0},
+          {100.01, 100.01, 2.1 * M_PI / 8},
+          {100.01, 100.01, 1.9 * M_PI / 8},
       };
   const int grid[][3] =
       {
-        { 0, 0, 1 },
-        { 0, 0, -1 },
-        { 50, 20, 4 },
-        { 2, 25, -4 },  // angle must not normalized
-        { -1, 0, 0 },   // x, y must be rounded downword
-        { 0, 0, 2 },    // angle must be rounded to nearest integer
-        { 0, 0, 2 }     // angle must be rounded to nearest integer
+          {0, 0, 1},
+          {0, 0, -1},
+          {50, 20, 4},
+          {2, 25, -4},  // angle must not normalized
+          {-1, 0, 0},   // x, y must be rounded downword
+          {0, 0, 2},    // angle must be rounded to nearest integer
+          {0, 0, 2}     // angle must be rounded to nearest integer
       };
   for (size_t i = 0; i < sizeof(grid) / sizeof(grid[0]); ++i)
   {
@@ -102,17 +102,17 @@ TEST(GridMetricConverter, Path)
 
   const float metric_expected[][3] =
       {
-        { 100.15, 100.15, M_PI / 8 },
-        { 100.15, 100.25, M_PI / 8 },
-        { 100.25, 100.35, M_PI / 8 },
-        { 100.35, 100.35, 2 * M_PI / 8 }
+          {100.15, 100.15, M_PI / 8},
+          {100.15, 100.25, M_PI / 8},
+          {100.25, 100.35, M_PI / 8},
+          {100.35, 100.35, 2 * M_PI / 8},
       };
   const int grid[][3] =
       {
-        { 1, 1, 1 },
-        { 1, 2, 1 },
-        { 2, 3, 1 },
-        { 3, 3, 2 }
+          {1, 1, 1},
+          {1, 2, 1},
+          {2, 3, 1},
+          {3, 3, 2},
       };
   std::vector<CyclicVecFloat<3, 2>> path_grid;
   for (const auto& g : grid)
diff --git a/planner_cspace/test/src/test_motion_cache.cpp b/planner_cspace/test/src/test_motion_cache.cpp
index dc875d4fd..645489bff 100644
--- a/planner_cspace/test/src/test_motion_cache.cpp
+++ b/planner_cspace/test/src/test_motion_cache.cpp
@@ -56,10 +56,10 @@ TEST(MotionCache, Generate)
   // Straight motions
   const int xy_yaw_straight[][3] =
       {
-        { 1, 0, 0 },
-        { 0, 1, 1 },
-        { -1, 0, 2 },
-        { 0, -1, 3 },
+          {1, 0, 0},
+          {0, 1, 1},
+          {-1, 0, 2},
+          {0, -1, 3},
       };
   for (auto& xy_yaw : xy_yaw_straight)
   {
@@ -94,14 +94,14 @@ TEST(MotionCache, Generate)
   // 90 deg rotation
   const int xy_syaw_gyaw_90[][4] =
       {
-        { 1, 1, 0, 1 },
-        { -1, 1, 1, 2 },
-        { -1, -1, 2, 3 },
-        { 1, -1, 3, 0 },
-        { 1, -1, 0, 3 },
-        { 1, 1, 1, 0 },
-        { -1, 1, 2, 1 },
-        { -1, -1, 3, 2 },
+          {1, 1, 0, 1},
+          {-1, 1, 1, 2},
+          {-1, -1, 2, 3},
+          {1, -1, 3, 0},
+          {1, -1, 0, 3},
+          {1, 1, 1, 0},
+          {-1, 1, 2, 1},
+          {-1, -1, 3, 2},
       };
   for (auto& xy_syaw_gyaw : xy_syaw_gyaw_90)
   {
diff --git a/planner_cspace/test/src/test_motion_primitive_builder.cpp b/planner_cspace/test/src/test_motion_primitive_builder.cpp
index e2f1589a6..41f7b7038 100644
--- a/planner_cspace/test/src/test_motion_primitive_builder.cpp
+++ b/planner_cspace/test/src/test_motion_primitive_builder.cpp
@@ -95,165 +95,163 @@ std::vector<Vec> buildExpectedPrimitives(const std::vector<std::vector<Vec>> ori
 
 TEST(MotionPrimitiveBuilder, Generate)
 {
-  // clang-format off
   const std::vector<Vec> expected_0deg_straight_primitives =
-  {
-      Vec(1, 0, 0),
-      Vec(2, 0, 0),
-      Vec(3, 0, 0),
-      Vec(4, 0, 0),
-  };
+      {
+          Vec(1, 0, 0),
+          Vec(2, 0, 0),
+          Vec(3, 0, 0),
+          Vec(4, 0, 0),
+      };
   const std::vector<Vec> expected_0deg_rotation_1_primitives =
-  {
-      Vec(3, -2, 15),
-      Vec(3, -1, 15),
-      Vec(3,  1,  1),
-      Vec(3,  2,  1),
-  };
+      {
+          Vec(3, -2, 15),
+          Vec(3, -1, 15),
+          Vec(3, 1, 1),
+          Vec(3, 2, 1),
+      };
   const std::vector<Vec> expected_0deg_rotation_2_primitives =
-  {
-      Vec(3, -2, 14),
-      Vec(3, -1, 14),
-      Vec(3,  1,  2),
-      Vec(3,  2,  2),
-  };
+      {
+          Vec(3, -2, 14),
+          Vec(3, -1, 14),
+          Vec(3, 1, 2),
+          Vec(3, 2, 2),
+      };
   const std::vector<Vec> expected_0deg_rotation_3_primitives =
-  {
-      Vec(3, -2, 13),
-      Vec(3, -1, 13),
-      Vec(3,  1,  3),
-      Vec(3,  2,  3),
-  };
+      {
+          Vec(3, -2, 13),
+          Vec(3, -1, 13),
+          Vec(3, 1, 3),
+          Vec(3, 2, 3),
+      };
   const std::vector<std::vector<Vec>> expected_0deg_primitives =
-  {
-    expected_0deg_straight_primitives,
-    expected_0deg_rotation_1_primitives,
-    expected_0deg_rotation_2_primitives,
-    expected_0deg_rotation_3_primitives,
-  };
+      {
+          expected_0deg_straight_primitives,
+          expected_0deg_rotation_1_primitives,
+          expected_0deg_rotation_2_primitives,
+          expected_0deg_rotation_3_primitives,
+      };
 
   const std::vector<Vec> expected_23deg_straight_primitives =
-  {
-      Vec(2, 1, 0),
-      Vec(3, 1, 0),
-      Vec(3, 2, 0),
-  };
+      {
+          Vec(2, 1, 0),
+          Vec(3, 1, 0),
+          Vec(3, 2, 0),
+      };
   const std::vector<Vec> expected_23deg_rotation_1_primitives =
-  {
-      Vec(2, 3,  1),
-      Vec(3, 0, 15),
-      Vec(3, 2,  1),
-      Vec(4, 0, 15),
-  };
+      {
+          Vec(2, 3, 1),
+          Vec(3, 0, 15),
+          Vec(3, 2, 1),
+          Vec(4, 0, 15),
+      };
   const std::vector<Vec> expected_23deg_rotation_2_primitives =
-  {
-      Vec(2,  3,  2),
-      Vec(3,  2,  2),
-      Vec(3, -1, 14),
-      Vec(3,  0, 14),
-      Vec(4,  0, 14),
-  };
+      {
+          Vec(2, 3, 2),
+          Vec(3, 2, 2),
+          Vec(3, -1, 14),
+          Vec(3, 0, 14),
+          Vec(4, 0, 14),
+      };
   const std::vector<Vec> expected_23deg_rotation_3_primitives =
-  {
-      Vec(1,  3,  3),
-      Vec(2,  3,  3),
-      Vec(3, -1, 13),
-      Vec(3,  0, 13),
-      Vec(4,  0, 13),
-  };
+      {
+          Vec(1, 3, 3),
+          Vec(2, 3, 3),
+          Vec(3, -1, 13),
+          Vec(3, 0, 13),
+          Vec(4, 0, 13),
+      };
   const std::vector<std::vector<Vec>> expected_23deg_primitives =
-  {
-    expected_23deg_straight_primitives,
-    expected_23deg_rotation_1_primitives,
-    expected_23deg_rotation_2_primitives,
-    expected_23deg_rotation_3_primitives,
-  };
+      {
+          expected_23deg_straight_primitives,
+          expected_23deg_rotation_1_primitives,
+          expected_23deg_rotation_2_primitives,
+          expected_23deg_rotation_3_primitives,
+      };
 
   const std::vector<Vec> expected_45deg_straight_primitives =
-  {
-      Vec(1, 1, 0),
-      Vec(2, 2, 0),
-      Vec(3, 2, 0),
-      Vec(2, 3, 0),
-  };
+      {
+          Vec(1, 1, 0),
+          Vec(2, 2, 0),
+          Vec(3, 2, 0),
+          Vec(2, 3, 0),
+      };
   const std::vector<Vec> expected_45deg_rotation_1_primitives =
-  {
-      Vec(1, 3,  1),
-      Vec(2, 3,  1),
-      Vec(3, 1, 15),
-      Vec(3, 2, 15),
-  };
+      {
+          Vec(1, 3, 1),
+          Vec(2, 3, 1),
+          Vec(3, 1, 15),
+          Vec(3, 2, 15),
+      };
   const std::vector<Vec> expected_45deg_rotation_2_primitives =
-  {
-      Vec(0, 3,  2),
-      Vec(1, 3,  2),
-      Vec(2, 3,  2),
-      Vec(3, 0, 14),
-      Vec(3, 1, 14),
-      Vec(3, 2, 14),
-  };
+      {
+          Vec(0, 3, 2),
+          Vec(1, 3, 2),
+          Vec(2, 3, 2),
+          Vec(3, 0, 14),
+          Vec(3, 1, 14),
+          Vec(3, 2, 14),
+      };
   const std::vector<Vec> expected_45deg_rotation_3_primitives =
-  {
-      Vec(0, 3,  3),
-      Vec(0, 4,  3),
-      Vec(1, 3,  3),
-      Vec(3, 0, 13),
-      Vec(3, 1, 13),
-      Vec(4, 0, 13),
-  };
+      {
+          Vec(0, 3, 3),
+          Vec(0, 4, 3),
+          Vec(1, 3, 3),
+          Vec(3, 0, 13),
+          Vec(3, 1, 13),
+          Vec(4, 0, 13),
+      };
   const std::vector<std::vector<Vec>> expected_45deg_primitives =
-  {
-    expected_45deg_straight_primitives,
-    expected_45deg_rotation_1_primitives,
-    expected_45deg_rotation_2_primitives,
-    expected_45deg_rotation_3_primitives,
-  };
+      {
+          expected_45deg_straight_primitives,
+          expected_45deg_rotation_1_primitives,
+          expected_45deg_rotation_2_primitives,
+          expected_45deg_rotation_3_primitives,
+      };
 
   const std::vector<Vec> expected_67deg_straight_primitives =
-  {
-      Vec(1, 2, 0),
-      Vec(1, 3, 0),
-      Vec(2, 3, 0),
-  };
+      {
+          Vec(1, 2, 0),
+          Vec(1, 3, 0),
+          Vec(2, 3, 0),
+      };
   const std::vector<Vec> expected_67deg_rotation_1_primitives =
-  {
-      Vec(0, 3,  1),
-      Vec(0, 4,  1),
-      Vec(2, 3, 15),
-      Vec(3, 2, 15),
-  };
+      {
+          Vec(0, 3, 1),
+          Vec(0, 4, 1),
+          Vec(2, 3, 15),
+          Vec(3, 2, 15),
+      };
   const std::vector<Vec> expected_67deg_rotation_2_primitives =
-  {
-      Vec(0, 3,  2),
-      Vec(0, 4,  2),
-      Vec(-1, 3,  2),
-      Vec(2, 3, 14),
-      Vec(3, 2, 14),
-  };
+      {
+          Vec(0, 3, 2),
+          Vec(0, 4, 2),
+          Vec(-1, 3, 2),
+          Vec(2, 3, 14),
+          Vec(3, 2, 14),
+      };
   const std::vector<Vec> expected_67deg_rotation_3_primitives =
-  {
-      Vec(0,  3,  3),
-      Vec(0,  4,  3),
-      Vec(1, -3,  3),
-      Vec(3,  1, 13),
-      Vec(3,  2, 13),
-  };
+      {
+          Vec(0, 3, 3),
+          Vec(0, 4, 3),
+          Vec(1, -3, 3),
+          Vec(3, 1, 13),
+          Vec(3, 2, 13),
+      };
   const std::vector<std::vector<Vec>> expected_67deg_primitives =
-  {
-    expected_67deg_straight_primitives,
-    expected_67deg_rotation_1_primitives,
-    expected_67deg_rotation_2_primitives,
-    expected_67deg_rotation_3_primitives,
-  };
+      {
+          expected_67deg_straight_primitives,
+          expected_67deg_rotation_1_primitives,
+          expected_67deg_rotation_2_primitives,
+          expected_67deg_rotation_3_primitives,
+      };
 
   const std::vector<std::vector<std::vector<Vec>>> expected_original_primitives =
-  {
-    expected_0deg_primitives,
-    expected_23deg_primitives,
-    expected_45deg_primitives,
-    expected_67deg_primitives,
-  };
-  // clang-format on
+      {
+          expected_0deg_primitives,
+          expected_23deg_primitives,
+          expected_45deg_primitives,
+          expected_67deg_primitives,
+      };
 
   costmap_cspace_msgs::MapMetaData3D map_info;
   map_info.linear_resolution = 0.1f;
diff --git a/safety_limiter/src/safety_limiter.cpp b/safety_limiter/src/safety_limiter.cpp
index cb788e49e..31f04833d 100644
--- a/safety_limiter/src/safety_limiter.cpp
+++ b/safety_limiter/src/safety_limiter.cpp
@@ -576,13 +576,13 @@ class SafetyLimiterNode
   limitMaxVelocities(const geometry_msgs::Twist& in)
   {
     auto out = in;
-    out.linear.x =  (out.linear.x > 0) ?
-                    std::min(out.linear.x, max_values_[0]) :
-                    std::max(out.linear.x, -max_values_[0]);
+    out.linear.x = (out.linear.x > 0) ?
+                       std::min(out.linear.x, max_values_[0]) :
+                       std::max(out.linear.x, -max_values_[0]);
 
     out.angular.z = (out.angular.z > 0) ?
-                    std::min(out.angular.z, max_values_[1]) :
-                    std::max(out.angular.z, -max_values_[1]);
+                        std::min(out.angular.z, max_values_[1]) :
+                        std::max(out.angular.z, -max_values_[1]);
 
     return out;
   }
diff --git a/safety_limiter/test/src/test_safety_limiter.cpp b/safety_limiter/test/src/test_safety_limiter.cpp
index 45ee20c94..f6a2aedae 100644
--- a/safety_limiter/test/src/test_safety_limiter.cpp
+++ b/safety_limiter/test/src/test_safety_limiter.cpp
@@ -10,8 +10,8 @@
  *     * Redistributions in binary form must reproduce the above copyright
  *       notice, this list of conditions and the following disclaimer in the
  *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the copyright holder nor the names of its 
- *       contributors may be used to endorse or promote products derived from 
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
  *       this software without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
@@ -292,7 +292,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearEscape)
     ros::spinOnce();
   }
 
-  const float vel_ref[] = { -0.2, -0.4, 0.2, 0.4 };
+  const float vel_ref[] = {-0.2, -0.4, 0.2, 0.4};
   for (const float vel : vel_ref)
   {
     // 1.0 m/ss, obstacle at -0.05 m (already in collision): escape motion must be allowed
@@ -412,7 +412,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape)
     ros::spinOnce();
   }
 
-  const float vel_ref[] = { -0.2, -0.4, 0.2, 0.4 };
+  const float vel_ref[] = {-0.2, -0.4, 0.2, 0.4};
   for (const float vel : vel_ref)
   {
     // already colliding at rear-right side: only positive rotation must be allowed
@@ -516,10 +516,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulation)
   const float dt = 0.02;
   ros::Rate wait(1.0 / dt);
 
-  const float velocities[] =
-      {
-        -0.8, -0.5, 0.5, 0.8
-      };
+  const float velocities[] = {-0.8, -0.5, 0.5, 0.8};
   for (const float vel : velocities)
   {
     float x = 0;
@@ -564,24 +561,25 @@ TEST_F(SafetyLimiterTest, SafetyLimitMaxVelocitiesValues)
   ros::Rate wait(20);
 
   const float linear_velocities[] =
-  {-1.7, -1.5, 0.0, 1.5, 1.7 };
+      {-1.7, -1.5, 0.0, 1.5, 1.7};
   const float angular_velocities[] =
-  {-2.7, -2.5, 0.0, 2.5, 2.7 };
+      {-2.7, -2.5, 0.0, 2.5, 2.7};
   const float expected_linear_velocities[] =
-  {-1.5, -1.5, 0.0, 1.5, 1.5 };
+      {-1.5, -1.5, 0.0, 1.5, 1.5};
   const float expected_angular_velocities[] =
-  {-2.5, -2.5, 0.0, 2.5, 2.5 };
+      {-2.5, -2.5, 0.0, 2.5, 2.5};
 
   for (int linear_index = 0; linear_index < 5; linear_index++)
   {
-    for  (int angular_index = 0; angular_index < 5; angular_index++)
+    for (int angular_index = 0; angular_index < 5; angular_index++)
     {
       bool received = false;
       bool en = false;
 
       for (int i = 0; i < 10 && ros::ok(); ++i)
       {
-        if (i > 5) en = true;
+        if (i > 5)
+          en = true;
         publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now());
         publishWatchdogReset();
         publishTwist(linear_velocities[linear_index], angular_velocities[angular_index]);
@@ -611,7 +609,6 @@ TEST_F(SafetyLimiterTest, SafetyLimitMaxVelocitiesValues)
   }
 }
 
-
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/safety_limiter/test/src/test_safety_limiter2.cpp b/safety_limiter/test/src/test_safety_limiter2.cpp
index 6922b9dae..12dd647aa 100644
--- a/safety_limiter/test/src/test_safety_limiter2.cpp
+++ b/safety_limiter/test/src/test_safety_limiter2.cpp
@@ -10,8 +10,8 @@
  *     * Redistributions in binary form must reproduce the above copyright
  *       notice, this list of conditions and the following disclaimer in the
  *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the copyright holder nor the names of its 
- *       contributors may be used to endorse or promote products derived from 
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
  *       this software without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
@@ -45,10 +45,7 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulationWithMargin)
   const float dt = 0.02;
   ros::Rate wait(1.0 / dt);
 
-  const float velocities[] =
-      {
-        -0.8, -0.4, 0.4, 0.8
-      };
+  const float velocities[] = {-0.8, -0.4, 0.4, 0.8};
   for (const float vel : velocities)
   {
     float x = 0;
diff --git a/trajectory_tracker/test/src/test_trajectory_recorder.cpp b/trajectory_tracker/test/src/test_trajectory_recorder.cpp
index 195cca0ca..b2f29af48 100644
--- a/trajectory_tracker/test/src/test_trajectory_recorder.cpp
+++ b/trajectory_tracker/test/src/test_trajectory_recorder.cpp
@@ -10,8 +10,8 @@
  *     * Redistributions in binary form must reproduce the above copyright
  *       notice, this list of conditions and the following disclaimer in the
  *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the copyright holder nor the names of its 
- *       contributors may be used to endorse or promote products derived from 
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
  *       this software without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
@@ -55,10 +55,10 @@ TEST(TrajectoryRecorder, TfToPath)
 
   const tf2::Transform points[] =
       {
-        tf2::Transform(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(0, 0, 0)),
-        tf2::Transform(tf2::Quaternion(0, 0, 1, 0), tf2::Vector3(2, 0, 0)),
-        tf2::Transform(tf2::Quaternion(0, 0, 0, -1), tf2::Vector3(3, 5, 0)),
-        tf2::Transform(tf2::Quaternion(0, 0, -1, 0), tf2::Vector3(-1, 5, 1))
+          tf2::Transform(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(0, 0, 0)),
+          tf2::Transform(tf2::Quaternion(0, 0, 1, 0), tf2::Vector3(2, 0, 0)),
+          tf2::Transform(tf2::Quaternion(0, 0, 0, -1), tf2::Vector3(3, 5, 0)),
+          tf2::Transform(tf2::Quaternion(0, 0, -1, 0), tf2::Vector3(-1, 5, 1)),
       };
   const size_t len = sizeof(points) / sizeof(tf2::Transform);
 
diff --git a/trajectory_tracker/test/src/test_trajectory_tracker.cpp b/trajectory_tracker/test/src/test_trajectory_tracker.cpp
index 99a57ea05..d690e9b4e 100644
--- a/trajectory_tracker/test/src/test_trajectory_tracker.cpp
+++ b/trajectory_tracker/test/src/test_trajectory_tracker.cpp
@@ -79,9 +79,9 @@ TEST_F(TrajectoryTrackerTest, StraightStopOvershoot)
 {
   const double resolutions[] =
       {
-        0.1,
-        0.001,  // default epsilon
-        0.0001,
+          0.1,
+          0.001,  // default epsilon
+          0.0001,
       };
   for (const double resolution : resolutions)
   {
@@ -134,10 +134,7 @@ TEST_F(TrajectoryTrackerTest, StraightStopOvershoot)
 
 TEST_F(TrajectoryTrackerTest, StraightStopConvergence)
 {
-  const double vels[] =
-      {
-        0.02, 0.05, 0.1, 0.2, 0.5, 1.0
-      };
+  const double vels[] = {0.02, 0.05, 0.1, 0.2, 0.5, 1.0};
   const double path_length = 2.0;
   for (const double vel : vels)
   {
@@ -293,23 +290,19 @@ TEST_F(TrajectoryTrackerTest, CurveFollow)
 
 TEST_F(TrajectoryTrackerTest, InPlaceTurn)
 {
-  const float init_yaw_array[] =
-      {
-        0.0,
-        3.0
-      };
+  const float init_yaw_array[] = {0.0, 3.0};
   for (const float init_yaw : init_yaw_array)
   {
     const std::vector<float> target_angle_array[] =
         {
-          { 0.5 },
-          { -0.5 },
-          { 0.1, 0.2, 0.3, 0.4, 0.5 },
-          { -0.1, -0.2, -0.3, -0.4, -0.5 },
+            {0.5},
+            {-0.5},
+            {0.1, 0.2, 0.3, 0.4, 0.5},
+            {-0.1, -0.2, -0.3, -0.4, -0.5},
         };
     for (const auto& angles : target_angle_array)
     {
-      for (const bool& has_short_path : { false, true })
+      for (const bool& has_short_path : {false, true})
       {
         std::stringstream condition_name;
         condition_name