Added RivalLayer and finished GameElementLayer

toid_costmaps/src/game_elements_layer.cpp

@@ -1,27 +1,86 @@
 #include "toid_costmaps/game_elements_layer.hpp"
 
+#include <fstream>
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "nav2_util/node_utils.hpp"
 
 namespace toid
 {
 
-void GameElementLayer::onInitialize() {}
+void GameElementLayer::onInitialize()
+{
+  auto node = node_.lock();
+  if (!node) {
+    return;
+  }
+}
+
+void GameElementLayer::activate()
+{
+  auto node = node_.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + ".extra_elements", rclcpp::ParameterValue(std::vector<std::string>{}));
+  node->get_parameter(name_ + ".extra_elements", extra_elements_);
+
+  initGameElements();
+
+  active_elements_ = std::make_unique<ActiveElements>();
+  active_elements_->active = std::vector<bool>(game_elements_.size(), true);
+
+  using namespace std::placeholders;
+  active_elements_sub_ = node->create_subscription<ActiveElements>(
+    "/active_elements", 1, std::bind(&GameElementLayer::active_elements_cb, this, _1));
+}
+
+void GameElementLayer::deactivate()
+{
+  auto node = node_.lock();
+  active_elements_sub_.reset();
+  active_elements_.reset();
+  game_elements_.clear();
+  static_elements_.clear();
+  extra_elements_.clear();
+}
+
+void GameElementLayer::active_elements_cb(ActiveElements::UniquePtr msg)
+{
+  active_elements_ = std::move(msg);
+}
 
 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);
+  touch(1.50, 1.0, min_x, min_y, max_x, max_y);
+  touch(-1.50, -1.0, 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)
+void GameElementLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
+{
+  ulong idx = 0;
+  for (const auto & elem : game_elements_) {
+    if (active_elements_->active.size() > idx && active_elements_->active.at(idx)) {
+      placeElement(grid, elem);
+    }
+    idx++;
+  }
+
+  for (const auto & elem : static_elements_) {
+    placeElement(grid, elem);
+  }
+}
+
+void GameElementLayer::placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element)
 {
   bool in_bounds = true;
+  double ex, ey;
+  element.start(ex, ey);
   uint mx, my;
-  in_bounds &= worldToMap(1.0 - 0.1, 0.5 - 0.1, mx, my);
+  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mx, my);
+  element.end(ex, ey);
   uint mmx, mmy;
-  in_bounds &= worldToMap(1.0 + 0.1, 0.5 + 0.1, mmx, mmy);
+  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mmx, mmy);
 
   if (in_bounds) {
     for (uint j = my; j < mmy; j++) {
@@ -32,6 +91,47 @@ void GameElementLayer::updateCosts(
   }
 }
 
+void GameElementLayer::initGameElements()
+{
+  std::string file_path = "/elements/elements.json";
+  std::string base_path = ament_index_cpp::get_package_share_directory("toid_costmaps");
+
+  RCLCPP_INFO(rclcpp::get_logger("toid_costmap"), "Bro hear me out");
+
+  std::ifstream fs(base_path + file_path);
+  if (!fs.is_open()) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("toid_costmap"), "Failed to open elements file: %s%s", base_path.c_str(),
+      file_path.c_str());
+    throw std::runtime_error("File not found");
+  }
+
+  boost::json::stream_parser p;
+  boost::json::error_code ec;
+
+  char buffer[4096];
+  while (fs.read(buffer, sizeof(buffer)) || fs.gcount() > 0) {
+    p.write(buffer, fs.gcount(), ec);
+    if (ec) {
+      RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
+      throw std::runtime_error("JsonError");
+    }
+  }
+  p.finish(ec);
+  if (ec) {
+    RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
+    throw std::runtime_error("JsonError");
+  }
+
+  boost::json::value jv = p.release();
+
+  game_elements_ = boost::json::value_to<std::vector<GameElement>>(jv.at("game_elements"));
+  for (const auto & t : extra_elements_) {
+    std::vector<GameElement> elements = boost::json::value_to<std::vector<GameElement>>(jv.at(t));
+    static_elements_.insert(static_elements_.end(), elements.begin(), elements.end());
+  }
+}
+
 }  // namespace toid
 
 #include "pluginlib/class_list_macros.hpp"