costmap_cspace: fixes memory access error on global map boundary. (#49)

* Fix unsigned variable with negative value in grid address calculation. * Also fix invalid use of Vec.
at-wat · Jul 28, 2017 · e64d606 · e64d606
1 parent c026b5f
commit e64d606
Showing 1 changed file with 29 additions and 24 deletions.
diff --git a/costmap_cspace/include/costmap_3d.h b/costmap_cspace/include/costmap_3d.h
@@ -420,47 +420,52 @@ class Costmap3DOF
       const ros::Time &stamp)
   {
     costmap_cspace::CSpace3DUpdate update;
-    update.header = map_.header;
-    map_.header.stamp = stamp;
-    update.x = x - range_max_;
-    update.y = y - range_max_;
-    update.width = width + range_max_ * 2;
-    update.height = height + range_max_ * 2;
-    if (update.x < 0)
+    update.header = map.header;
+    map.header.stamp = stamp;
+    int update_x = x - range_max_;
+    int update_y = y - range_max_;
+    int update_width = width + range_max_ * 2;
+    int update_height = height + range_max_ * 2;
+    if (update_x < 0)
     {
-      update.width += update.x;
-      update.x = 0;
+      update_width += update_x;
+      update_x = 0;
     }
-    if (update.y < 0)
+    if (update_y < 0)
     {
-      update.height += update.y;
-      update.y = 0;
+      update_height += update_y;
+      update_y = 0;
     }
-    if (update.x + update.width > map_.info.width)
+    if (update_x + update_width > map.info.width)
     {
-      update.width = map_.info.width - update.x;
+      update_width = map.info.width - update_x;
     }
-    if (update.y + update.height > map_.info.height)
+    if (update_y + update_height > map.info.height)
     {
-      update.height = map_.info.height - update.y;
+      update_height = map.info.height - update_y;
     }
+    update.x = update_x;
+    update.y = update_y;
+    update.width = update_width;
+    update.height = update_height;
     update.yaw = yaw;
     update.angle = angle;
     update.data.resize(update.width * update.height * update.angle);
 
-    costmap_cspace::Vec p;
-    for (p[0] = 0; p[0] < update.width; p[0]++)
+    for (int i = 0; i < update.width; i++)
     {
-      for (p[1] = 0; p[1] < update.height; p[1]++)
+      for (int j = 0; j < update.height; j++)
       {
-        for (p[2] = 0; p[2] < update.angle; p[2]++)
+        for (int k = 0; k < update.angle; k++)
         {
-          const int x2 = update.x + p[0];
-          const int y2 = update.y + p[1];
-          const int yaw2 = yaw + p[2];
+          const int x2 = update.x + i;
+          const int y2 = update.y + j;
+          const int yaw2 = yaw + k;
 
           const auto &m = getCostRef(x2, y2, yaw2, map);
-          auto &up = update.data[(p[2] * update.height + p[1]) * update.width + p[0]];
+          const size_t addr = (k * update.height + j) * update.width + i;
+          assert(addr < update.data.size());
+          auto &up = update.data[addr];
           up = m;
         }
       }