costmap_cspace: always store received overlay map (#109)

If an overlay map arrives before a root map, overlay map was ignored
since frame_id of the root map is not yet given.
The commit enables to store overlay map and keep until receiving
the root map.

costmap_cspace/include/costmap_3d_layer/base.h

@@ -278,7 +278,16 @@ class Costmap3dLayerBase
 
     *map_overlay_ = *map_;
     if (map_updated_.info.width > 0 && map_updated_.info.height > 0)
-      updateCSpace(map_updated_);
+    {
+      if (map_->header.frame_id == map_updated_.header.frame_id)
+      {
+        updateCSpace(map_updated_);
+      }
+      else
+      {
+        ROS_ERROR("map and map_overlay must have same frame_id. skipping");
+      }
+    }
 
     if (updateChain())
       return true;

costmap_cspace/src/costmap_3d.cpp

@@ -77,11 +77,6 @@ class Costmap3DOFNode
     ROS_DEBUG("Overlay 2D costmap received");
 
     auto map_msg = map->getMap();
-    if (map_msg->header.frame_id != msg->header.frame_id)
-    {
-      ROS_ERROR("map and map_overlay must have same frame_id");
-      return;
-    }
     if (map_msg->info.width < 1 ||
         map_msg->info.height < 1)
       return;