update function dataCallBack and file test static layer

plugins/static_layer.cpp

@@ -160,6 +160,122 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
   return scale * LETHAL_OBSTACLE;
 }
 
+void StaticLayer::handleImpl(const void* data, const std::type_info& type)  
+{
+    if (type == typeid(nav_msgs::OccupancyGrid)) {
+        process(*static_cast<const nav_msgs::OccupancyGrid*>(data));
+    } else if (type == typeid(map_msgs::OccupancyGridUpdate)) {
+        process(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
+    } else {
+        std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
+    }
+}
+
+void StaticLayer::process(const nav_msgs::OccupancyGrid& new_map)
+{
+  if(!map_shutdown_)
+  {
+    std::cout << "Received new map!" << std::endl;
+
+    unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
+
+    printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+
+    // resize costmap if size, resolution or origin do not match
+    Costmap2D* master = layered_costmap_->getCostmap();
+    if (!layered_costmap_->isRolling() &&
+        (master->getSizeInCellsX() != size_x ||
+        master->getSizeInCellsY() != size_y ||
+        master->getResolution() != new_map.info.resolution ||
+        master->getOriginX() != new_map.info.origin.position.x ||
+        master->getOriginY() != new_map.info.origin.position.y))
+    {
+      // Update the size of the layered costmap (and all layers, including this one)
+      printf("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+      layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
+                                  new_map.info.origin.position.y,
+                                  true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
+    }
+    else if (size_x_ != size_x || size_y_ != size_y ||
+            resolution_ != new_map.info.resolution ||
+            origin_x_ != new_map.info.origin.position.x ||
+            origin_y_ != new_map.info.origin.position.y)
+    {
+      // only update the size of the costmap stored locally in this layer
+      printf("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+      resizeMap(size_x, size_y, new_map.info.resolution,
+                new_map.info.origin.position.x, new_map.info.origin.position.y);
+    }
+
+    unsigned int index = 0;
+
+    // initialize the costmap with static data
+    for (unsigned int i = 0; i < size_y; ++i)
+    {
+      for (unsigned int j = 0; j < size_x; ++j)
+      {
+        unsigned char value = new_map.data[index];
+        costmap_[index] = interpretValue(value);
+        ++index;
+      }
+    }
+    map_frame_ = new_map.header.frame_id;
+
+    // // Print current costmap
+    // std::cout << "[StaticLayer] Costmap (" << size_y_ << " x " << size_x_ << "):" << std::endl;
+    // for (unsigned int i = 0; i < size_y_; ++i)
+    // {
+    //   for (unsigned int j = 0; j < size_x_; ++j)
+    //   {
+    //     unsigned int idx = i * size_x_ + j;
+    //     std::cout << static_cast<int>(costmap_[idx]);
+    //     if (j + 1 < size_x_) std::cout << ' ';
+    //   }
+    //   std::cout << std::endl;
+    // }
+
+    // we have a new map, update full size of map
+    x_ = y_ = 0;
+    width_ = size_x_;
+    height_ = size_y_;
+    map_received_ = true;
+    has_updated_data_ = true;
+  }
+  else
+  {
+    printf("Stop receive new map!");
+  }
+  // shutdown the map subscrber if firt_map_only_ flag is on
+  if (first_map_only_)
+  {
+    printf("Shutting down the map subscriber. first_map_only flag is on");
+    map_shutdown_ = true;
+    // map_sub_.shutdown();
+  }
+}
+void StaticLayer::process(const map_msgs::OccupancyGridUpdate& update)
+{
+  if(!map_update_shutdown_)
+  {
+    std::cout << "Update new map!" << std::endl;
+    unsigned int di = 0;
+    for (unsigned int y = 0; y < update.height ; y++)
+    {
+      unsigned int index_base = (update.y + y) * size_x_;
+      for (unsigned int x = 0; x < update.width ; x++)
+      {
+        unsigned int index = index_base + x + update.x;
+        costmap_[index] = interpretValue(update.data[di++]);
+      }
+    }
+    x_ = update.x;
+    y_ = update.y;
+    width_ = update.width;
+    height_ = update.height;
+    has_updated_data_ = true;
+  }
+}
+
 void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
 {
   if(!map_shutdown_)