138 lines
3.8 KiB
C++
138 lines
3.8 KiB
C++
#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()
|
|
{
|
|
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::SharedPtr msg)
|
|
{
|
|
active_elements_ = msg;
|
|
}
|
|
|
|
void GameElementLayer::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 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(ex + 0.001, ey + 0.001, mx, my);
|
|
element.end(ex, ey);
|
|
uint mmx, mmy;
|
|
in_bounds &= worldToMap(ex + 0.001, ey + 0.001, 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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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"
|
|
PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer); |