From 3d8dd3127deb67c1008c5f1a80eae2f2b2fa1804 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 26 Mar 2026 18:33:32 +0100 Subject: [PATCH] Added RivalLayer and finished GameElementLayer --- toid_costmaps/CMakeLists.txt | 18 ++- toid_costmaps/elements/elements.json | 46 +++++++ .../include/toid_costmaps/element_info.hpp | 46 +++++++ .../toid_costmaps/game_elements_layer.hpp | 43 +++++-- .../include/toid_costmaps/rival_layer.hpp | 55 +++++++++ toid_costmaps/package.xml | 2 + toid_costmaps/src/game_elements_layer.cpp | 114 ++++++++++++++++-- toid_costmaps/src/rival_layer.cpp | 108 +++++++++++++++++ toid_costmaps/toid_costmaps.xml | 3 + toid_msgs/CMakeLists.txt | 1 + toid_msgs/msg/ActiveElements.msg | 2 + .../params/toid_costmap_params.yaml | 12 +- 12 files changed, 426 insertions(+), 24 deletions(-) create mode 100644 toid_costmaps/elements/elements.json create mode 100644 toid_costmaps/include/toid_costmaps/element_info.hpp create mode 100644 toid_costmaps/include/toid_costmaps/rival_layer.hpp create mode 100644 toid_costmaps/src/rival_layer.cpp create mode 100644 toid_msgs/msg/ActiveElements.msg diff --git a/toid_costmaps/CMakeLists.txt b/toid_costmaps/CMakeLists.txt index 9f31edc..2aae4e9 100644 --- a/toid_costmaps/CMakeLists.txt +++ b/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}) diff --git a/toid_costmaps/elements/elements.json b/toid_costmaps/elements/elements.json new file mode 100644 index 0000000..2d3e5b2 --- /dev/null +++ b/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 + } + ] +} diff --git a/toid_costmaps/include/toid_costmaps/element_info.hpp b/toid_costmaps/include/toid_costmaps/element_info.hpp new file mode 100644 index 0000000..a021b18 --- /dev/null +++ b/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, boost::json::value const & jv) +{ + auto const& obj = jv.as_object(); + return GameElement{ + boost::json::value_to(obj.at("x")) - 1.5, + boost::json::value_to(obj.at("y")) - 1.0, + boost::json::value_to(obj.at("width")), + boost::json::value_to(obj.at("height")) + }; +} + +} // namespace toid \ No newline at end of file diff --git a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp index de71151..d550e3a 100644 --- a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp +++ b/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 game_elements_; + std::vector static_elements_; + std::vector extra_elements_; + ActiveElements::UniquePtr active_elements_; + + rclcpp::Subscription::SharedPtr active_elements_sub_; }; } // namespace toid \ No newline at end of file diff --git a/toid_costmaps/include/toid_costmaps/rival_layer.hpp b/toid_costmaps/include/toid_costmaps/rival_layer.hpp new file mode 100644 index 0000000..d63177e --- /dev/null +++ b/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::SharedPtr rival_sub_; + + double rival_size_ = 0.15; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_costmaps/package.xml b/toid_costmaps/package.xml index 98952d0..80080d8 100644 --- a/toid_costmaps/package.xml +++ b/toid_costmaps/package.xml @@ -10,6 +10,8 @@ ament_cmake angles + ament_index_cpp + boost geometry_msgs nav_msgs nav2_costmap_2d diff --git a/toid_costmaps/src/game_elements_layer.cpp b/toid_costmaps/src/game_elements_layer.cpp index aecba67..05f0357 100644 --- a/toid_costmaps/src/game_elements_layer.cpp +++ b/toid_costmaps/src/game_elements_layer.cpp @@ -1,27 +1,86 @@ #include "toid_costmaps/game_elements_layer.hpp" +#include + +#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{})); + node->get_parameter(name_ + ".extra_elements", extra_elements_); + + initGameElements(); + + active_elements_ = std::make_unique(); + active_elements_->active = std::vector(game_elements_.size(), true); + + using namespace std::placeholders; + active_elements_sub_ = node->create_subscription( + "/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>(jv.at("game_elements")); + for (const auto & t : extra_elements_) { + std::vector elements = boost::json::value_to>(jv.at(t)); + static_elements_.insert(static_elements_.end(), elements.begin(), elements.end()); + } +} + } // namespace toid #include "pluginlib/class_list_macros.hpp" diff --git a/toid_costmaps/src/rival_layer.cpp b/toid_costmaps/src/rival_layer.cpp new file mode 100644 index 0000000..4ffbb0b --- /dev/null +++ b/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( + "/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(rival_size_ / res); + + int min_i = std::max(0, static_cast(mx) - cell_radius); + int max_i = + std::min(static_cast(grid.getSizeInCellsX()) - 1, static_cast(mx) + cell_radius); + int min_j = std::max(0, static_cast(my) - cell_radius); + int max_j = + std::min(static_cast(grid.getSizeInCellsY()) - 1, static_cast(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(i) - static_cast(mx); + double dj = static_cast(j) - static_cast(my); + double distance_sq = di * di + dj * dj; + if (distance_sq <= static_cast(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); \ No newline at end of file diff --git a/toid_costmaps/toid_costmaps.xml b/toid_costmaps/toid_costmaps.xml index bead28e..3211914 100644 --- a/toid_costmaps/toid_costmaps.xml +++ b/toid_costmaps/toid_costmaps.xml @@ -3,5 +3,8 @@ + + + \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index a505d52..1142ee6 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ActiveElements.msg" "msg/Rival.msg" "srv/SendDouble.srv" "action/SimpleMoveCoords.action" diff --git a/toid_msgs/msg/ActiveElements.msg b/toid_msgs/msg/ActiveElements.msg new file mode 100644 index 0000000..689d2c1 --- /dev/null +++ b/toid_msgs/msg/ActiveElements.msg @@ -0,0 +1,2 @@ +# Activate/deactivate game elements +bool[] active \ No newline at end of file diff --git a/toid_navigation/params/toid_costmap_params.yaml b/toid_navigation/params/toid_costmap_params.yaml index 56cb21c..aa0781a 100644 --- a/toid_navigation/params/toid_costmap_params.yaml +++ b/toid_navigation/params/toid_costmap_params.yaml @@ -1,14 +1,14 @@ global_costmap: ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 + update_frequency: 10.0 + publish_frequency: 10.0 global_frame: map robot_base_frame: base_link robot_radius: 0.17 resolution: 0.01 track_unknown_space: false rolling_window: false - plugins: ["static_layer", "inflation_layer"] + plugins: ["static_layer", "game_element_layer", "rival_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True @@ -16,6 +16,12 @@ global_costmap: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.23 + game_element_layer: + plugin: "toid::GameElementLayer" + extra_elements: ["blue"] + rival_layer: + plugin: "toid::RivalLayer" + rival_size: 0.15 always_send_full_costmap: True lifecycle_manager: