Added RivalLayer and finished GameElementLayer

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