Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added RivalLayer and finished GameElementLayer

Browse Source
This commit is contained in:
pimpest
2026-03-26 18:33:32 +01:00
parent 8ba8585a29
commit 3d8dd3127d
12 changed files with 426 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
toid_costmaps/src/game_elements_layer.cpp
View File

@@ -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"

108
toid_costmaps/src/rival_layer.cpp Normal file
View File

@@ -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);