58 lines
1.5 KiB
C++
58 lines
1.5 KiB
C++
#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() {}
|
|
|
|
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 placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element);
|
|
|
|
bool isClearable() override { return true; }
|
|
|
|
void initGameElements();
|
|
|
|
void active_elements_cb(ActiveElements::SharedPtr 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::SharedPtr active_elements_;
|
|
|
|
rclcpp::Subscription<ActiveElements>::SharedPtr active_elements_sub_;
|
|
};
|
|
|
|
} // namespace toid
|