diff --git a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/base.h b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/base.h
index 8196552e..a0fb9573 100644
--- a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/base.h
+++ b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/base.h
@@ -321,12 +321,14 @@ class Costmap3dLayerBase
         map_->data[i + yaw * xy_size] = -1;
       }
     }
+    const auto m1 = ros::WallTime::now();
     updateCSpace(
         base_map,
         UpdatedRegion(
             0, 0, 0,
             map_->info.width, map_->info.height, map_->info.angle,
             map_->header.stamp));
+    ROS_WARN("INITAL updateCSpace: %f", (ros::WallTime::now() - m1).toSec());
     for (size_t yaw = 0; yaw < map_->info.angle; yaw++)
     {
       for (unsigned int i = 0; i < xy_size; i++)
@@ -412,7 +414,9 @@ class Costmap3dLayerBase
     {
       if (map_->header.frame_id == map_updated_->header.frame_id)
       {
+        const auto m1 = ros::WallTime::now();
         updateCSpace(map_updated_, region_);
+        ROS_WARN("Update: %f", (ros::WallTime::now() - m1).toSec());
       }
       else
       {
diff --git a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h
index 9ff63230..55539356 100644
--- a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h
+++ b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h
@@ -287,9 +287,9 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
       result.y_max =
           (gy == static_cast<int>(msg->info.height) - 1 || (*(ptr + msg->info.width) >= *ptr)) ? 0 : range_max_;
     };
-    const auto range_getter = (msg->info.resolution == map->info.linear_resolution) ?
-                                  getMaskedRange :
-                                  std::function<void(const int, Rect&)>([](const int, Rect&) {});
+    const auto getRange = (msg->info.resolution == map->info.linear_resolution) ?
+                              getMaskedRange :
+                              std::function<void(const int, Rect&)>([](const int, Rect&) {});
 
     const int ox =
         std::lround((msg->info.origin.position.x - map->info.origin.position.x) / map->info.linear_resolution);
@@ -333,7 +333,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
           m = 0;
         continue;
       }
-      range_getter(i, range);
+      getRange(i, range);
       const int map_x_min = std::max(gx + range.x_min, 0);
       const int map_x_max = std::min(gx + range.x_max, static_cast<int>(map->info.width) - 1);
       const int map_y_min = std::max(gy + range.y_min, 0);