Added RivalLayer and finished GameElementLayer

toid_costmaps/CMakeLists.txt

@@ -9,9 +9,9 @@ set(library_name toid_costmaps)
 
 set(
   PACKAGE_DEPS
-
   rclcpp
-  angles
+  ament_index_cpp
+  Boost
   geometry_msgs
   pluginlib
   nav_msgs
@@ -28,6 +28,7 @@ set(
 set(
   SOURCES
   src/game_elements_layer.cpp
+  src/rival_layer.cpp
 )
 
 find_package(ament_cmake REQUIRED)
@@ -35,8 +36,16 @@ foreach(PACKAGE ${PACKAGE_DEPS})
   find_package(${PACKAGE} REQUIRED)
 endforeach()
 
+find_package(Boost REQUIRED COMPONENTS json)
+
+
 add_library(${library_name} SHARED ${SOURCES})
 
+target_link_libraries(
+  ${library_name}
+  Boost::json
+)
+
 target_include_directories(
   ${library_name} 
   PRIVATE 
@@ -60,6 +69,11 @@ install(
   DESTINATION include/
 )
 
+install(
+  DIRECTORY elements
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 ament_export_include_directories(include)
 ament_export_libraries(${library_name})
 ament_export_dependencies(${PACKAGE_DEPS})

toid_costmaps/elements/elements.json

@@ -0,0 +1,46 @@
+{
+  "game_elements": [
+    {
+      "x": 0.0,
+      "y": 0.7,
+      "width": 0.2,
+      "height": 0.2
+    },
+    {
+      "x": 0.05,
+      "y": 0.3,
+      "width": 0.2,
+      "height": 0.2
+    },
+    {
+      "x": 0.6,
+      "y": 0.0,
+      "width": 0.2,
+      "height": 0.2
+    },
+    {
+      "x": 0.05,
+      "y": 1.1,
+      "width": 0.2,
+      "height": 0.2
+    }
+  ],
+
+  "blue": [
+    {
+      "x": 0.00,
+      "y": 1.55,
+      "width": 0.6,
+      "height": 0.45
+    }
+  ],
+
+  "yellow": [
+    {
+      "x": 2.40,
+      "y": 1.55,
+      "width": 0.6,
+      "height": 0.45
+    }
+  ]
+}

toid_costmaps/include/toid_costmaps/element_info.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "boost/json.hpp"
+
+namespace toid
+{
+class GameElement
+{
+public:
+  GameElement(double x, double y, double width, double height)
+  : x_(x), y_(y), width_(width), height_(height)
+  {
+  }
+
+  void start(double & x, double & y) const
+  {
+    x = this->x_;
+    y = this->y_;
+  }
+
+  void end(double & x, double & y) const
+  {
+    x = this->x_ + this->width_;
+    y = this->y_ + this->height_;
+  }
+
+private:
+  double x_;
+  double y_;
+  double width_;
+  double height_;
+};
+
+inline GameElement tag_invoke(
+  boost::json::value_to_tag<GameElement>, boost::json::value const & jv)
+{
+    auto const& obj = jv.as_object();
+    return GameElement{
+        boost::json::value_to<double>(obj.at("x")) - 1.5,
+        boost::json::value_to<double>(obj.at("y")) - 1.0,
+        boost::json::value_to<double>(obj.at("width")),
+        boost::json::value_to<double>(obj.at("height"))
+    };
+}
+
+}  // namespace toid

toid_costmaps/include/toid_costmaps/game_elements_layer.hpp

@@ -1,39 +1,58 @@
 #pragma once
 
+#include "jsoncpp/json/json.h"
 #include "nav2_costmap_2d/costmap_layer.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "toid_costmaps/element_info.hpp"
+#include "toid_msgs/msg/active_elements.hpp"
 
 namespace toid
 {
 
 class GameElementLayer : public nav2_costmap_2d::CostmapLayer
 {
+  using ActiveElements = toid_msgs::msg::ActiveElements;
+
 public:
-  GameElementLayer(){
-    costmap_ = NULL;
-  }
-  ~GameElementLayer(){}
+  GameElementLayer() { costmap_ = NULL; }
+  ~GameElementLayer() {}
 
-  virtual void onInitialize();
-  virtual void updateBounds(
+  void onInitialize() override;
+  void activate() override;
+  void deactivate() override;
+
+  void updateBounds(
     double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y,
-    double * max_x, double * max_y);
+    double * max_x, double * max_y) override;
 
-  virtual void updateCosts(
-    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override;
 
-  virtual void reset() { return; }
+  void reset() override { return; }
 
-  virtual void onFootprintChanged() {}
+  void onFootprintChanged() override {}
 
-  virtual bool isClearable() { return false; }
+  void placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element);
+
+  bool isClearable() override { return true; }
+
+  void initGameElements();
+
+  void active_elements_cb(ActiveElements::UniquePtr msg);
 
 private:
   double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
 
   bool need_recalculation_;
+
+  std::vector<GameElement> game_elements_;
+  std::vector<GameElement> static_elements_;
+  std::vector<std::string> extra_elements_;
+  ActiveElements::UniquePtr active_elements_;
+
+  rclcpp::Subscription<ActiveElements>::SharedPtr active_elements_sub_;
 };
 
 }  // namespace toid

toid_costmaps/include/toid_costmaps/rival_layer.hpp

@@ -0,0 +1,55 @@
+#pragma once
+
+#include "nav2_costmap_2d/costmap_layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "toid_msgs/msg/rival.hpp"
+
+
+namespace toid
+{
+
+class RivalLayer : public nav2_costmap_2d::CostmapLayer
+{
+  using Rivals = toid_msgs::msg::Rival;
+
+public:
+  RivalLayer() { costmap_ = NULL; }
+  ~RivalLayer() {}
+
+  void onInitialize() override;
+  void activate() override;
+  void deactivate() override;
+
+  void updateBounds(
+    double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y,
+    double * max_x, double * max_y) override;
+
+  void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override;
+
+  void reset() override { return; }
+
+  void onFootprintChanged() override {}
+
+  void placeRival(nav2_costmap_2d::Costmap2D & grid, double x, double y);
+
+  bool isClearable() override { return true; }
+
+  void rival_cb(Rivals::UniquePtr msg);
+
+private:
+  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
+
+  bool need_recalculation_;
+
+  uint debounce_ = 0;
+
+  Rivals::UniquePtr rivals_;
+
+  rclcpp::Subscription<Rivals>::SharedPtr rival_sub_;
+
+  double rival_size_ = 0.15;
+};
+
+}  // namespace toid

toid_costmaps/package.xml

@@ -10,6 +10,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>angles</depend>
+  <depend>ament_index_cpp</depend>
+  <depend>boost</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>nav2_costmap_2d</depend>

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"

toid_costmaps/src/rival_layer.cpp

@@ -0,0 +1,108 @@
+
+#include "toid_costmaps/rival_layer.hpp"
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+void RivalLayer::onInitialize()
+{
+  auto node = node_.lock();
+  if (!node) {
+    return;
+  }
+}
+
+void RivalLayer::activate()
+{
+  auto node = node_.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + ".rival_size", rclcpp::ParameterValue(0.15));
+  node->get_parameter(name_ + ".rival_size", rival_size_);
+
+  using namespace std::placeholders;
+  rival_sub_ = node->create_subscription<Rivals>(
+    "/dynamicObstacle", 1, std::bind(&RivalLayer::rival_cb, this, _1));
+}
+
+void RivalLayer::deactivate()
+{
+  auto node = node_.lock();
+  rival_sub_.reset();
+  rivals_.reset();
+}
+
+void RivalLayer::rival_cb(Rivals::UniquePtr msg)
+{
+  if (msg->point.size() != 0 || debounce_++ > 10) {
+    rivals_ = std::move(msg);
+    debounce_ = 0;
+  }
+}
+
+void RivalLayer::updateBounds(
+  double, double, double, double * min_x, double * min_y, double * max_x, double * 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 RivalLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
+{
+  geometry_msgs::msg::TransformStamped tf_msg;
+  if(!rivals_) {
+    return;
+  }
+
+  try {
+    tf_msg = tf_->lookupTransform(
+      layered_costmap_->getGlobalFrameID(), rivals_->header.frame_id, rivals_->header.stamp);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "Failed to transform rival message to costmap");
+    return;
+  }
+
+  for (const auto & rival : rivals_->point) {
+    geometry_msgs::msg::Point point;
+    tf2::doTransform(rival, point, tf_msg);
+    placeRival(grid, point.x, point.y);
+  }
+}
+
+void RivalLayer::placeRival(nav2_costmap_2d::Costmap2D & grid, const double x, const double y)
+{
+  unsigned int mx, my;
+  if (!grid.worldToMap(x, y, mx, my)) {
+    return;  // Center is outside the map bounds
+  }
+
+  double res = grid.getResolution();
+  int cell_radius = static_cast<int>(rival_size_ / res);
+
+  int min_i = std::max(0, static_cast<int>(mx) - cell_radius);
+  int max_i =
+    std::min(static_cast<int>(grid.getSizeInCellsX()) - 1, static_cast<int>(mx) + cell_radius);
+  int min_j = std::max(0, static_cast<int>(my) - cell_radius);
+  int max_j =
+    std::min(static_cast<int>(grid.getSizeInCellsY()) - 1, static_cast<int>(my) + cell_radius);
+
+  for (int i = min_i; i <= max_i; ++i) {
+    for (int j = min_j; j <= max_j; ++j) {
+      double di = static_cast<double>(i) - static_cast<double>(mx);
+      double dj = static_cast<double>(j) - static_cast<double>(my);
+      double distance_sq = di * di + dj * dj;
+      if (distance_sq <= static_cast<double>(cell_radius * cell_radius)) {
+        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
+      }
+    }
+  }
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::RivalLayer, nav2_costmap_2d::Layer);

toid_costmaps/toid_costmaps.xml

@@ -3,5 +3,8 @@
         <class type="toid::GameElementLayer" base_class_type="nav2_costmap_2d::Layer">
             <description></description>
         </class>
+        <class type="toid::RivalLayer" base_class_type="nav2_costmap_2d::Layer">
+            <description></description>
+        </class>
     </library>
 </class_libraries>