GameElementLayer prototype

toid_costmaps/src/game_elements_layer.cpp

@@ -0,0 +1,38 @@
+#include "toid_costmaps/game_elements_layer.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace toid
+{
+
+void GameElementLayer::onInitialize() {}
+
+void GameElementLayer::updateBounds(
+  double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
+{
+  touch(1.0 - 0.1, 0.5 - 0.1, min_x, min_y, max_x, max_y);
+  touch(1.0 + 0.1, 0.5 + 0.1, min_x, min_y, max_x, max_y);
+}
+
+void GameElementLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D & grid, int min_i, int min_j, int max_i, int max_j)
+{
+  bool in_bounds = true;
+  uint mx, my;
+  in_bounds &= worldToMap(1.0 - 0.1, 0.5 - 0.1, mx, my);
+  uint mmx, mmy;
+  in_bounds &= worldToMap(1.0 + 0.1, 0.5 + 0.1, mmx, mmy);
+
+  if (in_bounds) {
+    for (uint j = my; j < mmy; j++) {
+      for (uint i = mx; i < mmx; i++) {
+        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
+      }
+    }
+  }
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer);