From 427d74219fc32193bd00e3e2262fd97200f9921b Mon Sep 17 00:00:00 2001
From: Dan Rose <dan@digilabs.io>
Date: Tue, 25 Jun 2019 20:08:34 -0500
Subject: [PATCH] Make sure static layer gets initialized

Make sure static layer gets initialized, even with an empty layer, so it doesn't crash if you disable the map service and enable the obstacle layer
---
 nav2_costmap_2d/plugins/static_layer.cpp | 28 +++++++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index 9967b5435c..b65ccf8c5a 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -74,7 +74,13 @@ StaticLayer::onInitialize()
   getParameters();
 
   if (first_map_from_service_) {
+
     getMap();
+  } else {
+    nav_msgs::msg::OccupancyGrid empty_map;
+    // processMap updates the size of other costmap layers, so we have to call
+    // it, even if the map is 0x0
+    processMap(empty_map);
   }
 
   if (!first_map_only_) {
@@ -91,6 +97,7 @@ StaticLayer::onInitialize()
       rclcpp::SystemDefaultsQoS(),
       std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
   }
+  current_ = true;
 }
 
 void
@@ -219,7 +226,6 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
   width_ = size_x_;
   height_ = size_y_;
   has_updated_data_ = true;
-  current_ = true;
 }
 
 void
@@ -263,6 +269,26 @@ void
 StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
 {
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  if (update->y < static_cast<int32_t>(y_) ||
+      y_ + height_ < update->y + update->height ||
+      update->x < static_cast<int32_t>(x_) ||
+      x_ + width_ < update->x + update->width) {
+    RCLCPP_WARN(
+        node_->get_logger(),
+        "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
+        "Static layer origin: %d, %d   bounds: %d X %d\n"
+        "Update origin: %d, %d   bounds: %d X %d",
+        x_, y_, width_, height_, update->x, update->y, update->width,
+        update->height);
+    return;
+  }
+
+  if (update->header.frame_id != map_frame_) {
+    RCLCPP_WARN(node_->get_logger(),
+                "StaticLayer: Map update ignored. Current map is in frame %s "
+                "but update was in frame %s",
+                map_frame_.c_str(), update->header.frame_id.c_str());
+  }
 
   unsigned int di = 0;
   for (unsigned int y = 0; y < update->height; y++) {