diff --git a/planner_cspace/include/planner_cspace/blockmem_gridmap.h b/planner_cspace/include/planner_cspace/blockmem_gridmap.h
index 8c310747..6d0cd177 100644
--- a/planner_cspace/include/planner_cspace/blockmem_gridmap.h
+++ b/planner_cspace/include/planner_cspace/blockmem_gridmap.h
@@ -129,11 +129,11 @@ class BlockMemGridmap : public BlockMemGridmapBase<T, DIM, NONCYCLIC>
       const T zero, const CyclicVecInt<DIM, NONCYCLIC>& min, const CyclicVecInt<DIM, NONCYCLIC>& max)
   {
     CyclicVecInt<DIM, NONCYCLIC> p = min;
-    for (p[0] = min[0]; p[0] < max[0]; ++p[0])
+    for (p[0] = min[0]; p[0] <= max[0]; ++p[0])
     {
-      for (p[1] = min[1]; p[1] < max[1]; ++p[1])
+      for (p[1] = min[1]; p[1] <= max[1]; ++p[1])
       {
-        for (p[2] = min[2]; p[2] < max[2]; ++p[2])
+        for (p[2] = min[2]; p[2] <= max[2]; ++p[2])
         {
           (*this)[p] = zero;
         }
@@ -145,11 +145,11 @@ class BlockMemGridmap : public BlockMemGridmapBase<T, DIM, NONCYCLIC>
       const CyclicVecInt<DIM, NONCYCLIC>& max)
   {
     CyclicVecInt<DIM, NONCYCLIC> p = min;
-    for (p[0] = min[0]; p[0] < max[0]; ++p[0])
+    for (p[0] = min[0]; p[0] <= max[0]; ++p[0])
     {
-      for (p[1] = min[1]; p[1] < max[1]; ++p[1])
+      for (p[1] = min[1]; p[1] <= max[1]; ++p[1])
       {
-        for (p[2] = min[2]; p[2] < max[2]; ++p[2])
+        for (p[2] = min[2]; p[2] <= max[2]; ++p[2])
         {
           (*this)[p] = base[p];
         }
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
index 7f8b7447..a1ac5b76 100644
--- a/planner_cspace/src/planner_3d.cpp
+++ b/planner_cspace/src/planner_3d.cpp
@@ -700,12 +700,12 @@ class Planner3dNode
     const ros::Time now = ros::Time::now();
 
     const int map_update_x_min = static_cast<int>(msg->x);
-    const int map_update_x_max = static_cast<int>(msg->x + msg->width);
+    const int map_update_x_max = std::max(static_cast<int>(msg->x + msg->width) - 1, 0);
     const int map_update_y_min = static_cast<int>(msg->y);
-    const int map_update_y_max = static_cast<int>(msg->y + msg->height);
+    const int map_update_y_max = std::max(static_cast<int>(msg->y + msg->height) - 1, 0);
 
-    if (static_cast<size_t>(map_update_x_max) > map_info_.width ||
-        static_cast<size_t>(map_update_y_max) > map_info_.height ||
+    if (static_cast<size_t>(map_update_x_max) >= map_info_.width ||
+        static_cast<size_t>(map_update_y_max) >= map_info_.height ||
         msg->angle > map_info_.angle)
     {
       ROS_WARN(
@@ -719,21 +719,34 @@ class Planner3dNode
 
     last_costmap_ = now;
 
-    cm_.copy_partially(cm_base_, Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
-                       Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, static_cast<int>(map_info_.angle)));
-    cm_rough_.copy_partially(cm_rough_base_, Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
-                             Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 1));
-    cm_updates_.clear_partially(-1, Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
-                                Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 1));
+    cm_.copy_partially(
+        cm_base_,
+        Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
+        Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, static_cast<int>(map_info_.angle) - 1));
+    cm_rough_.copy_partially(
+        cm_rough_base_,
+        Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
+        Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 0));
+    cm_updates_.clear_partially(
+        -1,
+        Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
+        Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 0));
 
     // Should search 1px around the region to
     // update the costmap even if the edge of the local map is obstacle
-    const int search_range_x_min = std::max(0, std::min(prev_map_update_x_min_, map_update_x_min) - 1);
-    const int search_range_x_max = std::min(static_cast<int>(map_info_.width),
-                                            std::max(prev_map_update_x_max_, map_update_x_max) + 1);
-    const int search_range_y_min = std::max(0, std::min(prev_map_update_y_min_, map_update_y_min) - 1);
-    const int search_range_y_max = std::min(static_cast<int>(map_info_.height),
-                                            std::max(prev_map_update_y_max_, map_update_y_max) + 1);
+    const int search_range_x_min = std::max(
+        0,
+        std::min(prev_map_update_x_min_, map_update_x_min) - 1);
+    const int search_range_x_max = std::min(
+        static_cast<int>(map_info_.width - 1),
+        std::max(prev_map_update_x_max_, map_update_x_max) + 1);
+    const int search_range_y_min = std::max(
+        0,
+        std::min(prev_map_update_y_min_, map_update_y_min) - 1);
+    const int search_range_y_max = std::min(
+        static_cast<int>(map_info_.height - 1),
+        std::max(prev_map_update_y_max_, map_update_y_max) + 1);
+
     prev_map_update_x_min_ = map_update_x_min;
     prev_map_update_x_max_ = map_update_x_max;
     prev_map_update_y_min_ = map_update_y_min;
@@ -965,10 +978,10 @@ class Planner3dNode
     cm_base_ = cm_;
     bbf_costmap_.clear();
 
-    prev_map_update_x_min_ = std::numeric_limits<int>::max();
-    prev_map_update_x_max_ = std::numeric_limits<int>::lowest();
-    prev_map_update_y_min_ = std::numeric_limits<int>::max();
-    prev_map_update_y_max_ = std::numeric_limits<int>::lowest();
+    prev_map_update_x_min_ = map_info_.width;
+    prev_map_update_x_max_ = 0;
+    prev_map_update_y_min_ = map_info_.height;
+    prev_map_update_y_max_ = 0;
 
     updateGoal();
 
diff --git a/planner_cspace/test/src/test_blockmem_gridmap.cpp b/planner_cspace/test/src/test_blockmem_gridmap.cpp
index 372a6fd4..44c0d8f2 100644
--- a/planner_cspace/test/src/test_blockmem_gridmap.cpp
+++ b/planner_cspace/test/src/test_blockmem_gridmap.cpp
@@ -117,9 +117,9 @@ TEST(BlockmemGridmap, ClearAndCopyPartially)
     {
       for (p[2] = 0; p[2] < base_size[2]; ++p[2])
       {
-        if ((copy_min_pos[0] <= p[0]) && (p[0] < copy_max_pos[0]) &&
-            (copy_min_pos[1] <= p[1]) && (p[1] < copy_max_pos[1]) &&
-            (copy_min_pos[2] <= p[2]) && (p[2] < copy_max_pos[2]))
+        if ((copy_min_pos[0] <= p[0]) && (p[0] <= copy_max_pos[0]) &&
+            (copy_min_pos[1] <= p[1]) && (p[1] <= copy_max_pos[1]) &&
+            (copy_min_pos[2] <= p[2]) && (p[2] <= copy_max_pos[2]))
         {
           EXPECT_EQ(gm[p], -1) << p[0] << "," << p[1] << "," << p[2];
         }
@@ -153,9 +153,9 @@ TEST(BlockmemGridmap, ClearAndCopyPartially)
     {
       for (p[2] = 0; p[2] < base_size[2]; ++p[2])
       {
-        if ((copy_min_pos[0] <= p[0]) && (p[0] < copy_max_pos[0]) &&
-            (copy_min_pos[1] <= p[1]) && (p[1] < copy_max_pos[1]) &&
-            (copy_min_pos[2] <= p[2]) && (p[2] < copy_max_pos[2]))
+        if ((copy_min_pos[0] <= p[0]) && (p[0] <= copy_max_pos[0]) &&
+            (copy_min_pos[1] <= p[1]) && (p[1] <= copy_max_pos[1]) &&
+            (copy_min_pos[2] <= p[2]) && (p[2] <= copy_max_pos[2]))
         {
           ASSERT_EQ(gm[p], gm_update[p]);
         }