diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 9b069f343..f5ceebb31 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -17,9 +17,7 @@ find_package(ateam_geometry REQUIRED) find_package(control_toolbox REQUIRED) find_package(ompl REQUIRED) find_package(angles REQUIRED) -find_package(OpenCV REQUIRED) find_package(nlohmann_json REQUIRED) -find_package(ateam_spatial REQUIRED) add_library(kenobi_node_component SHARED src/kenobi_node.cpp @@ -60,7 +58,6 @@ target_sources(kenobi_node_component PRIVATE src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp src/plays/passing_plays/pass_to_lane_play.cpp src/plays/passing_plays/pass_to_segment_play.cpp - src/plays/passing_plays/spatial_pass_play.cpp src/plays/stop_plays/default_stop_play.cpp src/plays/stop_plays/defensive_stop_play.cpp src/plays/stop_plays/offensive_stop_play.cpp @@ -131,9 +128,7 @@ ament_target_dependencies( "control_toolbox" "OMPL" "angles" - OpenCV nlohmann_json - ateam_spatial ) rclcpp_components_register_node( kenobi_node_component diff --git a/ateam_kenobi/package.xml b/ateam_kenobi/package.xml index 12a24d576..935055cce 100644 --- a/ateam_kenobi/package.xml +++ b/ateam_kenobi/package.xml @@ -21,8 +21,6 @@ ompl nlohmann-json-dev angles - libopencv-dev - ateam_spatial ament_lint_auto ament_lint_common diff --git a/ateam_kenobi/src/core/play_selector.cpp b/ateam_kenobi/src/core/play_selector.cpp index e314ee5a0..8826d4bdf 100644 --- a/ateam_kenobi/src/core/play_selector.cpp +++ b/ateam_kenobi/src/core/play_selector.cpp @@ -89,8 +89,6 @@ PlaySelector::PlaySelector(rclcpp::Node & node) addPlay( "PassRightBackwardPlay", stp_options, play_helpers::lanes::Lane::Right, PassToLanePlay::PassDirection::Backward); - addPlay(stp_options); - addPlay(stp_options); addPlay(stp_options); addPlay(stp_options); addPlay(stp_options); diff --git a/ateam_kenobi/src/core/types/world.hpp b/ateam_kenobi/src/core/types/world.hpp index c69d69597..aa5c1e8c4 100644 --- a/ateam_kenobi/src/core/types/world.hpp +++ b/ateam_kenobi/src/core/types/world.hpp @@ -25,7 +25,6 @@ #include #include #include -#include #include "core/types/ball.hpp" #include "core/types/field.hpp" @@ -55,8 +54,6 @@ struct World // Holds the ID of the robot not allowed to touch the ball, if any std::optional double_touch_forbidden_id_; - - ateam_spatial::SpatialEvaluator * spatial_evaluator; }; } // namespace ateam_kenobi diff --git a/ateam_kenobi/src/core/visualization/overlays.cpp b/ateam_kenobi/src/core/visualization/overlays.cpp index 0a4664f4d..7871b99b5 100644 --- a/ateam_kenobi/src/core/visualization/overlays.cpp +++ b/ateam_kenobi/src/core/visualization/overlays.cpp @@ -23,7 +23,6 @@ #include #include #include -#include namespace ateam_kenobi::visualization { @@ -257,103 +256,6 @@ void Overlays::drawArrows( addOverlay(msg); } -void Overlays::drawHeatmap( - const std::string & name, const ateam_geometry::Rectangle & bounds, - const cv::Mat & data, const uint8_t alpha, const uint32_t lifetime) -{ - ateam_msgs::msg::Overlay msg; - msg.ns = ns_; - msg.name = name; - msg.visible = true; - msg.type = ateam_msgs::msg::Overlay::HEATMAP; - msg.command = ateam_msgs::msg::Overlay::REPLACE; - msg.lifetime = lifetime; - msg.position.x = std::midpoint(bounds.xmin(), bounds.xmax()); - msg.position.y = std::midpoint(bounds.ymin(), bounds.ymax()); - msg.scale.x = bounds.xmax() - bounds.xmin(); - msg.scale.y = bounds.ymax() - bounds.ymin(); - msg.heatmap_resolution_width = data.cols; - msg.heatmap_resolution_height = data.rows; - msg.heatmap_alpha = {alpha}; - msg.heatmap_data.reserve(data.rows * data.cols); - if(!data.empty()) { - switch(data.type()) { - case CV_8UC1: - std::copy(data.begin(), data.end(), std::back_inserter(msg.heatmap_data)); - break; - case CV_32FC1: - { - cv::Mat byte_data{data.rows, data.cols, CV_8UC1}; - cv::normalize(data, byte_data, 0, 255, cv::NORM_MINMAX, CV_8UC1); - std::copy(byte_data.begin(), byte_data.end(), - std::back_inserter(msg.heatmap_data)); - break; - } - default: - throw std::runtime_error("Unsupported heatmap data type: " + std::to_string(data.type())); - } - } - addOverlay(msg); -} - -void Overlays::drawHeatmap( - const std::string & name, const ateam_geometry::Rectangle & bounds, - const cv::Mat & data, const cv::Mat & alpha, const uint32_t lifetime) -{ - ateam_msgs::msg::Overlay msg; - msg.ns = ns_; - msg.name = name; - msg.visible = true; - msg.type = ateam_msgs::msg::Overlay::HEATMAP; - msg.command = ateam_msgs::msg::Overlay::REPLACE; - msg.lifetime = lifetime; - msg.position.x = std::midpoint(bounds.xmin(), bounds.xmax()); - msg.position.y = std::midpoint(bounds.ymin(), bounds.ymax()); - msg.scale.x = bounds.xmax() - bounds.xmin(); - msg.scale.y = bounds.ymax() - bounds.ymin(); - msg.heatmap_resolution_width = data.cols; - msg.heatmap_resolution_height = data.rows; - msg.heatmap_alpha.reserve(alpha.rows * alpha.cols); - if(!alpha.empty()) { - switch(alpha.type()) { - case CV_8UC1: - std::copy(alpha.begin(), alpha.end(), - std::back_inserter(msg.heatmap_alpha)); - break; - case CV_32FC1: - { - cv::Mat byte_alpha{alpha.rows, alpha.cols, CV_8UC1}; - cv::normalize(alpha, byte_alpha, 0, 255, cv::NORM_MINMAX, CV_8UC1); - std::copy(byte_alpha.begin(), byte_alpha.end(), - std::back_inserter(msg.heatmap_alpha)); - break; - } - default: - throw std::runtime_error("Unsupported heatmap alpha type: " + std::to_string(data.type())); - } - } - msg.heatmap_data.reserve(data.rows * data.cols); - if(!data.empty()) { - switch(data.type()) { - case CV_8UC1: - std::copy(data.begin(), data.end(), std::back_inserter(msg.heatmap_data)); - break; - case CV_32FC1: - { - cv::Mat byte_data{data.rows, data.cols, CV_8UC1}; - cv::normalize(data, byte_data, 0, 255, cv::NORM_MINMAX, CV_8UC1); - std::copy(byte_data.begin(), byte_data.end(), - std::back_inserter(msg.heatmap_data)); - break; - } - default: - throw std::runtime_error("Unsupported heatmap data type: " + std::to_string(data.type())); - } - } - addOverlay(msg); -} - - void Overlays::drawHeatmap( const std::string & name, const ateam_geometry::Rectangle & bounds, const std::vector & data, const std::size_t resolution_width, diff --git a/ateam_kenobi/src/core/visualization/overlays.hpp b/ateam_kenobi/src/core/visualization/overlays.hpp index 61fa6796d..80f78ecf8 100644 --- a/ateam_kenobi/src/core/visualization/overlays.hpp +++ b/ateam_kenobi/src/core/visualization/overlays.hpp @@ -28,7 +28,6 @@ #include #include #include -#include namespace ateam_kenobi::visualization { @@ -86,14 +85,6 @@ class Overlays const std::string & stroke_color = "blue", const uint8_t stroke_width = 5, const uint32_t lifetime = kDefaultLifetime); - void drawHeatmap( - const std::string & name, const ateam_geometry::Rectangle & bounds, - const cv::Mat & data, const uint8_t alpha = 255, const uint32_t lifetime = kDefaultLifetime); - - void drawHeatmap( - const std::string & name, const ateam_geometry::Rectangle & bounds, - const cv::Mat & data, const cv::Mat & alpha, const uint32_t lifetime = kDefaultLifetime); - void drawHeatmap( const std::string & name, const ateam_geometry::Rectangle & bounds, const std::vector & data, const std::size_t resolution_width, diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 6de38e499..44035069a 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -54,7 +54,6 @@ #include "plays/halt_play.hpp" #include "core/defense_area_enforcement.hpp" #include "core/joystick_enforcer.hpp" -#include #include "core/fps_tracker.hpp" namespace ateam_kenobi @@ -74,8 +73,6 @@ class KenobiNode : public rclcpp::Node overlays_(""), game_controller_listener_(*this) { - world_.spatial_evaluator = &spatial_evaluator_; - initialize_robot_ids(); declare_parameter("use_world_velocities", false); @@ -173,7 +170,6 @@ class KenobiNode : public rclcpp::Node } private: - ateam_spatial::SpatialEvaluator spatial_evaluator_; World world_; PlaySelector play_selector_; InPlayEval in_play_eval_; @@ -414,7 +410,6 @@ class KenobiNode : public rclcpp::Node } in_play_eval_.Update(world_); double_touch_eval_.update(world_, overlays_); - UpdateSpatialEvaluator(); if (get_parameter("use_emulated_ballsense").as_bool()) { ballsense_emulator_.Update(world_); } @@ -505,41 +500,6 @@ class KenobiNode : public rclcpp::Node { return ateam_common::getCacheDirectory() / "kenobi"; } - - void UpdateSpatialEvaluator() - { - ateam_spatial::FieldDimensions field{ - world_.field.field_width, - world_.field.field_length, - world_.field.goal_width, - world_.field.goal_depth, - world_.field.boundary_width, - world_.field.defense_area_width, - world_.field.defense_area_depth - }; - ateam_spatial::Ball ball{ - static_cast(world_.ball.pos.x()), - static_cast(world_.ball.pos.y()), - static_cast(world_.ball.vel.x()), - static_cast(world_.ball.vel.y()) - }; - auto make_spatial_robot = [](const Robot & robot){ - return ateam_spatial::Robot{ - robot.visible, - static_cast(robot.pos.x()), - static_cast(robot.pos.y()), - static_cast(robot.theta), - static_cast(robot.vel.x()), - static_cast(robot.vel.y()), - static_cast(robot.omega) - }; - }; - std::array our_robots; - std::ranges::transform(world_.our_robots, our_robots.begin(), make_spatial_robot); - std::array their_robots; - std::ranges::transform(world_.their_robots, their_robots.begin(), make_spatial_robot); - spatial_evaluator_.UpdateMaps(field, ball, our_robots, their_robots); - } }; } // namespace ateam_kenobi diff --git a/ateam_kenobi/src/plays/passing_plays/all_passing_plays.hpp b/ateam_kenobi/src/plays/passing_plays/all_passing_plays.hpp index 2c46bbc2f..63acc4b90 100644 --- a/ateam_kenobi/src/plays/passing_plays/all_passing_plays.hpp +++ b/ateam_kenobi/src/plays/passing_plays/all_passing_plays.hpp @@ -24,6 +24,5 @@ #include "pass_to_lane_play.hpp" #include "pass_to_segment_play.hpp" #include "segment_passing_target_funcs.hpp" -#include "spatial_pass_play.hpp" #endif // PLAYS__PASSING_PLAYS__ALL_PASSING_PLAYS_HPP_ diff --git a/ateam_kenobi/src/plays/passing_plays/spatial_pass_play.cpp b/ateam_kenobi/src/plays/passing_plays/spatial_pass_play.cpp deleted file mode 100644 index 4625c0519..000000000 --- a/ateam_kenobi/src/plays/passing_plays/spatial_pass_play.cpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "spatial_pass_play.hpp" -#include -#include -#include -#include -#include -#include "core/play_helpers/available_robots.hpp" -#include "core/play_helpers/robot_assignment.hpp" -#include "core/play_helpers/window_evaluation.hpp" -#include "core/play_helpers/possession.hpp" - -namespace ateam_kenobi::plays -{ - -SpatialPassPlay::SpatialPassPlay(stp::Options stp_options) -: stp::Play(kPlayName, stp_options), - defense_tactic_(createChild("defense")), - pass_tactic_(createChild("pass")), - idler_skill_(createChild("idler")) -{ - getParamInterface().declareParameter("show_heatmap", false); -} - - -stp::PlayScore SpatialPassPlay::getScore(const World & world) -{ - if (!world.in_play && ( - world.referee_info.prev_command == ateam_common::GameCommand::PrepareKickoffOurs || - world.referee_info.prev_command == ateam_common::GameCommand::PrepareKickoffTheirs)) - { - return stp::PlayScore::NaN(); - } - if (!world.in_play && - world.referee_info.running_command != ateam_common::GameCommand::ForceStart && - world.referee_info.running_command != ateam_common::GameCommand::NormalStart && - world.referee_info.running_command != ateam_common::GameCommand::DirectFreeOurs) - { - return stp::PlayScore::NaN(); - } - if(play_helpers::WhoHasPossession(world) == play_helpers::PossessionResult::Theirs) { - return stp::PlayScore::Min(); - } - - ateam_geometry::Point pass_target; - - if(started_) { - pass_target = target_; - } else { - const auto spatial_target = - world.spatial_evaluator->GetMaxLocation(ateam_spatial::MapId::ReceiverPositionQuality); - pass_target = ateam_geometry::Point{spatial_target.x, spatial_target.y}; - } - - const auto their_robots = play_helpers::getVisibleRobots(world.their_robots); - const ateam_geometry::Segment goal_segment( - ateam_geometry::Point(world.field.field_length / 2.0, -world.field.goal_width), - ateam_geometry::Point(world.field.field_length / 2.0, world.field.goal_width) - ); - const auto windows = play_helpers::window_evaluation::getWindows(goal_segment, pass_target, - their_robots); - const auto largest_window = play_helpers::window_evaluation::getLargestWindow(windows); - if(!largest_window) { - return stp::PlayScore::Min(); - } - return std::min(90.0, - stp::PlayScore::Max() * (largest_window->squared_length() / goal_segment.squared_length())); -} - -stp::PlayCompletionState SpatialPassPlay::getCompletionState() -{ - if (!started_) { - return stp::PlayCompletionState::NotApplicable; - } - if (!pass_tactic_.isDone()) { - return stp::PlayCompletionState::Busy; - } - return stp::PlayCompletionState::Done; -} - -void SpatialPassPlay::reset() -{ - defense_tactic_.reset(); - pass_tactic_.reset(); - idler_skill_.Reset(); - started_ = false; - target_ = ateam_geometry::Point{}; -} - -std::array, - 16> SpatialPassPlay::runFrame(const World & world) -{ - std::array, 16> motion_commands; - - const auto spatial_map_id = ateam_spatial::MapId::ReceiverPositionQuality; - const auto is_pass_blocked = world.spatial_evaluator->GetValueAtLocation(spatial_map_id, - ateam_spatial::Point(target_.x(), target_.y())) < 1; - if(!started_ || is_pass_blocked) { - // TODO(barulicm): need to granularize "started" state to not change target while ball is moving - const auto spatial_target = world.spatial_evaluator->GetMaxLocation(spatial_map_id); - target_ = ateam_geometry::Point{spatial_target.x, spatial_target.y}; - } - - getOverlays().drawCircle("target", ateam_geometry::makeCircle(target_, kRobotRadius), "grey"); - const auto half_field_length = (world.field.field_length / 2.0) + world.field.boundary_width; - const auto half_field_width = (world.field.field_width / 2.0) + world.field.boundary_width; - ateam_geometry::Rectangle bounds { - ateam_geometry::Point{-half_field_length, -half_field_width}, - ateam_geometry::Point{half_field_length, half_field_width} - }; - - if(getParamInterface().getParameter("show_heatmap")) { - world.spatial_evaluator->RenderMapBuffer(spatial_map_id, spatial_map_rendering_data_); - const auto spatial_settings = world.spatial_evaluator->GetSettings(); - getOverlays().drawHeatmap("heatmap", bounds, spatial_map_rendering_data_, - spatial_settings.width, spatial_settings.height); - } - - pass_tactic_.setTarget(target_); - - idler_skill_.SetLane(getIdleLane(world)); - - auto available_robots = play_helpers::getAvailableRobots(world); - play_helpers::removeGoalie(available_robots, world); - - play_helpers::GroupAssignmentSet groups; - - std::vector disallowed_strikers; - if (world.double_touch_forbidden_id_) { - disallowed_strikers.push_back(*world.double_touch_forbidden_id_); - } - groups.AddPosition("kicker", pass_tactic_.getKickerAssignmentPoint(world), disallowed_strikers); - groups.AddPosition("receiver", pass_tactic_.getReceiverAssignmentPoint()); - const auto enough_bots_for_defense = available_robots.size() >= 4; - if (enough_bots_for_defense) { - groups.AddGroup("defense", defense_tactic_.getAssignmentPoints(world)); - } - const auto enough_bots_for_idler = available_robots.size() >= 5; - if (enough_bots_for_idler) { - groups.AddPosition("idler", idler_skill_.GetAssignmentPoint(world)); - } - - const auto assignments = play_helpers::assignGroups(available_robots, groups); - - defense_tactic_.runFrame(world, assignments.GetGroupFilledAssignmentsOrEmpty("defense"), - motion_commands); - - const auto maybe_kicker = assignments.GetPositionAssignment("kicker"); - const auto maybe_receiver = assignments.GetPositionAssignment("receiver"); - if (maybe_kicker && maybe_receiver) { - auto & kicker_command = - *(motion_commands[maybe_kicker->id] = ateam_msgs::msg::RobotMotionCommand{}); - auto & receiver_command = - *(motion_commands[maybe_receiver->id] = ateam_msgs::msg::RobotMotionCommand{}); - - if (CGAL::squared_distance(world.ball.pos, maybe_kicker->pos) < 0.5) { - started_ = true; - } - - pass_tactic_.runFrame(world, *maybe_kicker, *maybe_receiver, kicker_command, receiver_command); - - ForwardPlayInfo(pass_tactic_); - } - - std::string play_state; - if(!started_) { - play_state = "Not Started"; - } else if(!pass_tactic_.isDone()) { - play_state = "Passing"; - } else { - play_state = "Done"; - } - getPlayInfo()["play state"] = play_state; - - if (enough_bots_for_idler) { - assignments.RunPositionIfAssigned( - "idler", [this, &world, &motion_commands](const Robot & robot) { - motion_commands[robot.id] = idler_skill_.RunFrame(world, robot); - }); - } - - return motion_commands; -} - - -play_helpers::lanes::Lane SpatialPassPlay::getIdleLane(const World & world) -{ - std::vector lanes = { - play_helpers::lanes::Lane::Left, - play_helpers::lanes::Lane::Center, - play_helpers::lanes::Lane::Right, - }; - for(const auto lane : lanes) { - if(!play_helpers::lanes::IsPointInLane(world, target_, lane) && - !play_helpers::lanes::IsBallInLane(world, lane)) - { - return lane; - } - } - return play_helpers::lanes::Lane::Center; -} - -} // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/passing_plays/spatial_pass_play.hpp b/ateam_kenobi/src/plays/passing_plays/spatial_pass_play.hpp deleted file mode 100644 index 2228d1f2f..000000000 --- a/ateam_kenobi/src/plays/passing_plays/spatial_pass_play.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include "core/stp/play.hpp" -#include "tactics/standard_defense.hpp" -#include "tactics/pass.hpp" -#include "skills/lane_idler.hpp" - -#ifndef PLAYS__PASSING_PLAYS__SPATIAL_PASS_PLAY_HPP_ -#define PLAYS__PASSING_PLAYS__SPATIAL_PASS_PLAY_HPP_ - -namespace ateam_kenobi::plays -{ - -class SpatialPassPlay : public stp::Play -{ -public: - static constexpr const char * kPlayName = "SpatialPassPlay"; - - explicit SpatialPassPlay(stp::Options stp_options); - - virtual ~SpatialPassPlay() = default; - - stp::PlayScore getScore(const World & world) override; - - stp::PlayCompletionState getCompletionState() override; - - void reset() override; - - std::array, - 16> runFrame(const World & world) override; - -private: - tactics::StandardDefense defense_tactic_; - tactics::Pass pass_tactic_; - skills::LaneIdler idler_skill_; - ateam_geometry::Point target_; - bool started_ = false; - std::vector spatial_map_rendering_data_; - - play_helpers::lanes::Lane getIdleLane(const World & world); -}; - -} // namespace ateam_kenobi::plays - -#endif // PLAYS__PASSING_PLAYS__SPATIAL_PASS_PLAY_HPP_ diff --git a/ateam_kenobi/src/plays/test_plays/all_test_plays.hpp b/ateam_kenobi/src/plays/test_plays/all_test_plays.hpp index ea4753f6e..3657c62dc 100644 --- a/ateam_kenobi/src/plays/test_plays/all_test_plays.hpp +++ b/ateam_kenobi/src/plays/test_plays/all_test_plays.hpp @@ -27,7 +27,6 @@ #include "test_pass_play.hpp" #include "test_pivot_play.hpp" #include "test_play.hpp" -#include "test_spatial_map_play.hpp" #include "test_window_eval.hpp" #include "triangle_pass_play.hpp" #include "waypoints_play.hpp" diff --git a/ateam_kenobi/src/plays/test_plays/test_spatial_map_play.hpp b/ateam_kenobi/src/plays/test_plays/test_spatial_map_play.hpp deleted file mode 100644 index 2c1d8adff..000000000 --- a/ateam_kenobi/src/plays/test_plays/test_spatial_map_play.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2024 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef PLAYS__TEST_PLAYS__TEST_SPATIAL_MAP_PLAY_HPP_ -#define PLAYS__TEST_PLAYS__TEST_SPATIAL_MAP_PLAY_HPP_ - -#include -#include "core/path_planning/path_planner.hpp" -#include "core/motion/motion_controller.hpp" -#include "core/stp/play.hpp" -#include -#include - -namespace ateam_kenobi::plays -{ -class TestSpatialMapPlay : public stp::Play -{ -public: - static constexpr const char * kPlayName = "TestSpatialMapPlay"; - - explicit TestSpatialMapPlay(stp::Options stp_options) - : stp::Play(kPlayName, stp_options) - { - } - - void reset() override {} - - std::array, - 16> runFrame(const World & world) override - { - const auto half_field_length = (world.field.field_length / 2.0) + world.field.boundary_width; - const auto half_field_width = (world.field.field_width / 2.0) + world.field.boundary_width; - ateam_geometry::Rectangle bounds { - ateam_geometry::Point{-half_field_length, -half_field_width}, - ateam_geometry::Point{half_field_length, half_field_width} - }; - - const auto map_id = ateam_spatial::MapId::ReceiverPositionQuality; - - std::vector rendered_map; - world.spatial_evaluator->RenderMapBuffer(map_id, rendered_map); - - const auto spatial_settings = world.spatial_evaluator->GetSettings(); - - getOverlays().drawHeatmap("heatmap", bounds, rendered_map, spatial_settings.width, - spatial_settings.height); - - const auto max_pos = world.spatial_evaluator->GetMaxLocation(map_id); - - getOverlays().drawCircle("heatmap_max", - ateam_geometry::makeCircle(ateam_geometry::Point(max_pos.x, max_pos.y), kRobotRadius)); - - return {}; - } -}; -} // namespace ateam_kenobi::plays - -#endif // PLAYS__TEST_PLAYS__TEST_SPATIAL_MAP_PLAY_HPP_ diff --git a/ateam_spatial/CMakeLists.txt b/ateam_spatial/CMakeLists.txt deleted file mode 100644 index b92a44c49..000000000 --- a/ateam_spatial/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ateam_spatial) - -enable_language(CUDA) - -find_package(ament_cmake REQUIRED) -find_package(CUDAToolkit REQUIRED) - -add_library(${PROJECT_NAME} SHARED - src/layers/distance_down_field.cu - src/layers/distance_from_field_edge.cu - src/layers/distance_to_their_bots.cu - src/layers/in_field.cu - src/layers/line_of_sight_ball.cu - src/layers/line_of_sight.cu - src/layers/outside_their_defense_area.cu - src/layers/width_of_shot_on_goal.cu - - src/maps/receiver_position_quality.cu - - src/device_availability.cu - src/render_kernel.cu - src/spatial_evaluator.cu - src/update_maps_kernel.cu -) -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ -) -target_link_libraries(${PROJECT_NAME} PUBLIC CUDA::cudart) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_20) -set_target_properties(${PROJECT_NAME} PROPERTIES - CUDA_SEPARABLE_COMPILATION ON - CUDA_STANDARD 17 - CUDA_ARCHITECTURES native -) -target_compile_options(${PROJECT_NAME} PRIVATE $<$:--maxrregcount 62>) -if(ATEAM_SPATIAL_CUDA_DEBUG) - target_compile_options(${PROJECT_NAME} PRIVATE $<$:-g -G>) -endif() -if(ATEAM_SPATIAL_CUDA_COMPILE_STATS) - target_compile_options(${PROJECT_NAME} PRIVATE $<$:-Xptxas="-v">) -endif() - -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(CUDAToolkit) - -install( - DIRECTORY include/ - DESTINATION include -) -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # Disabling code linters because uncrustify doesn't play well with CUDA-C++ - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - add_subdirectory(test) -endif() - -ament_package() diff --git a/ateam_spatial/include/ateam_spatial/coordinate_conversions.hpp b/ateam_spatial/include/ateam_spatial/coordinate_conversions.hpp deleted file mode 100644 index 36b681148..000000000 --- a/ateam_spatial/include/ateam_spatial/coordinate_conversions.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__COORDINATE_CONVERSIONS_HPP_ -#define ATEAM_SPATIAL__COORDINATE_CONVERSIONS_HPP_ - -#include "types.hpp" -#include "cuda_hostdev.hpp" - -namespace ateam_spatial -{ - -CUDA_HOSTDEV inline float WorldWidthReal(const FieldDimensions & field) -{ - return field.field_length + (2.0 * field.boundary_width); -} - -CUDA_HOSTDEV inline float WorldHeightReal(const FieldDimensions & field) -{ - return field.field_width + (2.0 * field.boundary_width); -} - -CUDA_HOSTDEV inline int WorldWidthSpatial( - const FieldDimensions & field, - const SpatialSettings & settings) -{ - return WorldWidthReal(field) / settings.resolution; -} - -CUDA_HOSTDEV inline int WorldHeightSpatial( - const FieldDimensions & field, - const SpatialSettings & settings) -{ - return WorldHeightReal(field) / settings.resolution; -} - -CUDA_HOSTDEV inline float SpatialToRealX( - const int x, const FieldDimensions & field, - const SpatialSettings & settings) -{ - return (x * settings.resolution) - (WorldWidthReal(field) / 2.0); -} - -CUDA_HOSTDEV inline float SpatialToRealY( - const int y, const FieldDimensions & field, - const SpatialSettings & settings) -{ - return (y * settings.resolution) - (WorldHeightReal(field) / 2.0); -} - -CUDA_HOSTDEV inline int RealToSpatialDist(const float d, const SpatialSettings & settings) -{ - return static_cast(d / settings.resolution); -} - -CUDA_HOSTDEV inline int RealToSpatialX(const float x, const FieldDimensions & field, const SpatialSettings & settings) -{ - return (x + (WorldWidthReal(field) / 2.f)) / settings.resolution; -} - -CUDA_HOSTDEV inline int RealToSpatialY(const float x, const FieldDimensions & field, const SpatialSettings & settings) -{ - return (x + (WorldHeightReal(field) / 2.f)) / settings.resolution; -} - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__COORDINATE_CONVERSIONS_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/cuda_hostdev.hpp b/ateam_spatial/include/ateam_spatial/cuda_hostdev.hpp deleted file mode 100644 index 224517f72..000000000 --- a/ateam_spatial/include/ateam_spatial/cuda_hostdev.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__CUDA_HOSTDEV_HPP_ -#define ATEAM_SPATIAL__CUDA_HOSTDEV_HPP_ - -#ifdef __CUDACC__ -#define CUDA_HOSTDEV __host__ __device__ -#else -#define CUDA_HOSTDEV -#endif - -#endif // ATEAM_SPATIAL__CUDA_HOSTDEV_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/device_availability.hpp b/ateam_spatial/include/ateam_spatial/device_availability.hpp deleted file mode 100644 index 1f46335b4..000000000 --- a/ateam_spatial/include/ateam_spatial/device_availability.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__DEVICE_AVAILABILITY_HPP_ -#define ATEAM_SPATIAL__DEVICE_AVAILABILITY_HPP_ - -namespace ateam_spatial -{ - -bool IsCudaDeviceAvailable(); - -} // namespace ateam_spatial - - -#endif // ATEAM_SPATIAL__DEVICE_AVAILABILITY_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/gpu_array.hpp b/ateam_spatial/include/ateam_spatial/gpu_array.hpp deleted file mode 100644 index 2e2ab29db..000000000 --- a/ateam_spatial/include/ateam_spatial/gpu_array.hpp +++ /dev/null @@ -1,118 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__GPU_ARRAY_HPP_ -#define ATEAM_SPATIAL__GPU_ARRAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include "device_availability.hpp" - -namespace ateam_spatial -{ - -template -class GpuArray { -public: - GpuArray() - { - assert(std::is_trivially_copyable_v); - if(!IsCudaDeviceAvailable()) { - return; - } - const auto ret = cudaMalloc(&gpu_memory_, S * sizeof(T)); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMalloc failed: ") + cudaGetErrorString(ret)); - } - } - - GpuArray(const GpuArray &) = delete; - - GpuArray(GpuArray && other) - { - gpu_memory_ = other.gpu_memory_; - other.gpu_memory_ = nullptr; - } - - ~GpuArray() - { - if(gpu_memory_ == nullptr) - { - return; - } - const auto ret = cudaFree(gpu_memory_); - if(ret != cudaSuccess) { - std::cerr << "cudaFree failed: " << cudaGetErrorString(ret) << std::endl; - } - } - - GpuArray& operator=(GpuArray & other) - { - if(gpu_memory_ != nullptr) { - const auto ret = cudaFree(gpu_memory_); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaFree failed: ") + cudaGetErrorString(ret)); - } - } - gpu_memory_ = other.gpu_memory_; - other.gpu_memory_ = nullptr; - return *this; - } - - T * Get() - { - return reinterpret_cast(gpu_memory_); - } - - std::size_t Size() const - { - return S; - } - - void CopyToGpu(const typename std::array & values) - { - const auto ret = cudaMemcpy(gpu_memory_, values.data(), values.size() * sizeof(T), - cudaMemcpyHostToDevice); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - - void CopyFromGpu(typename std::array & values) - { - const auto ret = cudaMemcpy(values.data(), gpu_memory_, values.size() * sizeof(T), - cudaMemcpyDeviceToHost); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - -private: - void * gpu_memory_ = nullptr; -}; - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__GPU_ARRAY_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/gpu_multibuffer.hpp b/ateam_spatial/include/ateam_spatial/gpu_multibuffer.hpp deleted file mode 100644 index d3900c169..000000000 --- a/ateam_spatial/include/ateam_spatial/gpu_multibuffer.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__GPU_MULTIBUFFER_HPP_ -#define ATEAM_SPATIAL__GPU_MULTIBUFFER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include "device_availability.hpp" - -namespace ateam_spatial -{ - -/** - * @brief Manages a collection of buffers in GPU memory. - * - * Buffers are allocated in one continuous block. - * - * @tparam T The value type to store in the buffers - */ -template -class GpuMultibuffer { -public: - GpuMultibuffer(const std::size_t buffer_count, const std::size_t buffer_size) - : buffer_count_(buffer_count), - buffer_size_(buffer_size) - { - assert(std::is_trivially_copyable_v); - if(buffer_count == 0 || buffer_size == 0) { - return; - } - if(!IsCudaDeviceAvailable()) { - return; - } - const auto buffer_byte_size = buffer_size_ * sizeof(T); - const auto total_byte_size = buffer_count_ * buffer_byte_size; - { - const auto ret = cudaMalloc(&gpu_memory_, total_byte_size); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMalloc failed: ") + cudaGetErrorString(ret)); - } - } - { - const auto ret = cudaMemset(gpu_memory_, 0, total_byte_size); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemset failed: ") + cudaGetErrorString(ret)); - } - } - } - - GpuMultibuffer() - : GpuMultibuffer(0, 0) {} - - GpuMultibuffer(const GpuMultibuffer &) = delete; - - GpuMultibuffer(GpuMultibuffer && other) - { - gpu_memory_ = other.gpu_memory_; - buffer_count_ = other.buffer_count_; - buffer_size_ = other.buffer_size_; - other.gpu_memory_ = nullptr; - other.buffer_count_ = 0; - other.buffer_size_ = 0; - } - - ~GpuMultibuffer() - { - if(gpu_memory_ == nullptr) { - return; - } - const auto ret = cudaFree(gpu_memory_); - if(ret != cudaSuccess) { - std::cerr << "cudaFree failed: " << cudaGetErrorString(ret) << std::endl; - } - } - - GpuMultibuffer & operator=(GpuMultibuffer && other) - { - if(gpu_memory_ != nullptr) { - const auto ret = cudaFree(gpu_memory_); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaFree failed: ") + cudaGetErrorString(ret)); - } - } - gpu_memory_ = other.gpu_memory_; - buffer_count_ = other.buffer_count_; - buffer_size_ = other.buffer_size_; - other.gpu_memory_ = nullptr; - other.buffer_count_ = 0; - other.buffer_size_ = 0; - return *this; - } - - T * Get() - { - return reinterpret_cast(gpu_memory_); - } - - T * Get(std::size_t index) - { - return GetBufferStartPointer(index); - } - - std::size_t BufferCount() const - { - return buffer_count_; - } - - std::size_t BufferSize() const - { - return buffer_size_; - } - - bool IsEmpty() const - { - return buffer_count_ == 0 || buffer_size_ == 0; - } - - void CopyToGpu(const std::size_t buffer_index, const typename std::vector & values) - { - if(values.size() != buffer_size_) { - throw std::invalid_argument("Incorrect size of values given to GpuMultibuffer::CopyToGpu"); - } - auto buffer_ptr = GetBufferStartPointer(buffer_index); - const auto ret = cudaMemcpy(buffer_ptr, values.data(), buffer_size_ * sizeof(T), - cudaMemcpyHostToDevice); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - - void CopyFromGpu(const std::size_t buffer_index, typename std::vector & values) - { - auto buffer_ptr = GetBufferStartPointer(buffer_index); - values.resize(buffer_size_); - - const auto ret = cudaMemcpy(values.data(), buffer_ptr, buffer_size_ * sizeof(T), - cudaMemcpyDeviceToHost); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - - T CopyValueFromGpu(const std::size_t buffer_index, const std::size_t value_index) - { - auto buffer_ptr = GetBufferStartPointer(buffer_index); - buffer_ptr += value_index; - T dest; - const auto ret = cudaMemcpy(&dest, buffer_ptr, sizeof(T), cudaMemcpyDeviceToHost); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - return dest; - } - -private: - void * gpu_memory_ = nullptr; - std::size_t buffer_count_ = 0; - std::size_t buffer_size_ = 0; - - T * GetBufferStartPointer(const std::size_t index) - { - if(index >= buffer_count_) { - throw std::invalid_argument( - "Index must be less than the number of buffers in GpuMultibuffer."); - } - return reinterpret_cast(gpu_memory_) + (index * buffer_size_); - } -}; - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__GPU_MULTIBUFFER_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/gpu_object.hpp b/ateam_spatial/include/ateam_spatial/gpu_object.hpp deleted file mode 100644 index 1a0b5738d..000000000 --- a/ateam_spatial/include/ateam_spatial/gpu_object.hpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__GPU_OBJECT_HPP_ -#define ATEAM_SPATIAL__GPU_OBJECT_HPP_ - -#include -#include -#include -#include -#include -#include -#include "device_availability.hpp" - -namespace ateam_spatial -{ - - template - class GpuObject - { - public: - GpuObject() - { - assert(std::is_trivially_copyable_v); - if(!IsCudaDeviceAvailable()) { - return; - } - const auto ret = cudaMalloc(&gpu_memory_, sizeof(T)); - if (ret != cudaSuccess) - { - throw std::runtime_error(std::string("cudaMalloc failed: ") + cudaGetErrorString(ret)); - } - } - - explicit GpuObject(const T &value) - : GpuObject() - { - CopyToGpu(value); - } - - GpuObject(const GpuObject &) = delete; - - GpuObject(GpuObject &other) - { - gpu_memory_ = other.gpu_memory_; - other.gpu_memory_ = nullptr; - } - - ~GpuObject() - { - if (gpu_memory_ == nullptr) - { - return; - } - const auto ret = cudaFree(gpu_memory_); - if (ret != cudaSuccess) - { - std::cerr << "cudaFree failed: " << cudaGetErrorString(ret) << std::endl; - } - } - - GpuObject& operator=(GpuObject &&other) - { - if (gpu_memory_ != nullptr) - { - const auto ret = cudaFree(gpu_memory_); - if (ret != cudaSuccess) - { - throw std::runtime_error(std::string("cudaFree failed: ") + cudaGetErrorString(ret)); - } - } - gpu_memory_ = other.gpu_memory_; - other.gpu_memory_ = nullptr; - return *this; - } - - T *Get() - { - return reinterpret_cast(gpu_memory_); - } - - void CopyToGpu(const T &value) - { - const auto ret = cudaMemcpy(gpu_memory_, &value, sizeof(T), cudaMemcpyHostToDevice); - if (ret != cudaSuccess) - { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - - void CopyFromGpu(T &value) - { - const auto ret = cudaMemcpy(&value, gpu_memory_, sizeof(T), cudaMemcpyDeviceToHost); - if (ret != cudaSuccess) - { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - - private: - void *gpu_memory_ = nullptr; - }; - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__GPU_OBJECT_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/gpu_vector.hpp b/ateam_spatial/include/ateam_spatial/gpu_vector.hpp deleted file mode 100644 index 28887940f..000000000 --- a/ateam_spatial/include/ateam_spatial/gpu_vector.hpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__GPU_VECTOR_HPP_ -#define ATEAM_SPATIAL__GPU_VECTOR_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include "device_availability.hpp" - -namespace ateam_spatial -{ - -template -class GpuVector { -public: - - GpuVector() - { - assert(std::is_trivially_copyable_v); - } - - explicit GpuVector(const std::size_t size) - { - assert(std::is_trivially_copyable_v); - AllocateGpuMemory(size); - } - - GpuVector(const GpuVector &) = delete; - - GpuVector(GpuVector && other) - { - gpu_memory_ = other.gpu_memory_; - size_ = other.size_; - other.gpu_memory_ = nullptr; - other.size_ = 0; - } - - ~GpuVector() - { - if(gpu_memory_ == nullptr) { - return; - } - const auto ret = cudaFree(gpu_memory_); - if(ret != cudaSuccess) { - std::cerr << "cudaFree failed: " << cudaGetErrorString(ret) << std::endl; - } - } - - GpuVector& operator=(GpuVector && other) - { - FreeGpuMemory(); - gpu_memory_ = other.gpu_memory_; - size_ = other.size_; - other.gpu_memory_ = nullptr; - other.size_ = 0; - return *this; - } - - T * Get() - { - return reinterpret_cast(gpu_memory_); - } - - std::size_t Size() const - { - return size_; - } - - void CopyToGpu(const typename std::vector & values) - { - AllocateGpuMemory(values.size()); - const auto ret = cudaMemcpy(gpu_memory_, values.data(), values.size() * sizeof(T), - cudaMemcpyHostToDevice); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - - void CopyFromGpu(typename std::vector & values) - { - if(size_ == 0) { - values.clear(); - return; - } - values.resize(size_); - const auto ret = cudaMemcpy(values.data(), gpu_memory_, size_ * sizeof(T), - cudaMemcpyDeviceToHost); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMemcpy failed: ") + cudaGetErrorString(ret)); - } - } - -private: - void * gpu_memory_ = nullptr; - std::size_t size_ = 0; - - void AllocateGpuMemory(const std::size_t new_size) - { - if(new_size == size_) { - return; - } - if(!IsCudaDeviceAvailable()) { - return; - } - FreeGpuMemory(); - if(new_size == 0) { - return; - } - const auto ret = cudaMalloc(&gpu_memory_, new_size * sizeof(T)); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaMalloc failed: ") + cudaGetErrorString(ret)); - } - size_ = new_size; - } - - void FreeGpuMemory() - { - if(gpu_memory_ == nullptr) { - return; - } - const auto ret = cudaFree(gpu_memory_); - if(ret != cudaSuccess) { - throw std::runtime_error(std::string("cudaFree failed: ") + cudaGetErrorString(ret)); - } - gpu_memory_ = nullptr; - size_ = 0; - } -}; - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__GPU_VECTOR_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/distance_down_field.hpp b/ateam_spatial/include/ateam_spatial/layers/distance_down_field.hpp deleted file mode 100644 index a9cbff859..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/distance_down_field.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__DISTANCE_DOWN_FIELD_HPP_ -#define ATEAM_SPATIAL__LAYERS__DISTANCE_DOWN_FIELD_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float DistanceDownField( - const float x, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__DISTANCE_DOWN_FIELD_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/distance_from_field_edge.hpp b/ateam_spatial/include/ateam_spatial/layers/distance_from_field_edge.hpp deleted file mode 100644 index 1468da747..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/distance_from_field_edge.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__DISTANCE_FROM_FIELD_EDGE_HPP_ -#define ATEAM_SPATIAL__LAYERS__DISTANCE_FROM_FIELD_EDGE_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float DistanceFromFieldEdge( - const float x, const float y, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__DISTANCE_FROM_FIELD_EDGE_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/distance_to_their_bots.hpp b/ateam_spatial/include/ateam_spatial/layers/distance_to_their_bots.hpp deleted file mode 100644 index e4fbc9b91..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/distance_to_their_bots.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__DISTANCE_TO_THEIR_BOTS_HPP_ -#define ATEAM_SPATIAL__LAYERS__DISTANCE_TO_THEIR_BOTS_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float DistanceToTheirBots( - const float x, const float y, const Robot * their_bots, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__DISTANCE_TO_THEIR_BOTS_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/in_field.hpp b/ateam_spatial/include/ateam_spatial/layers/in_field.hpp deleted file mode 100644 index fa6a7177d..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/in_field.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__IN_FIELD_HPP_ -#define ATEAM_SPATIAL__LAYERS__IN_FIELD_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float InField( - const float x, const float y, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__IN_FIELD_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/line_of_sight.hpp b/ateam_spatial/include/ateam_spatial/layers/line_of_sight.hpp deleted file mode 100644 index e76e84fdb..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/line_of_sight.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__LINE_OF_SIGHT_HPP_ -#define ATEAM_SPATIAL__LAYERS__LINE_OF_SIGHT_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float LineOfSight( - const float src_x, const float src_y, const float dst_x, const float dst_y, const Robot * their_bots, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__LINE_OF_SIGHT_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/line_of_sight_ball.hpp b/ateam_spatial/include/ateam_spatial/layers/line_of_sight_ball.hpp deleted file mode 100644 index 551393f2b..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/line_of_sight_ball.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__LINE_OF_SIGHT_BALL_HPP_ -#define ATEAM_SPATIAL__LAYERS__LINE_OF_SIGHT_BALL_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float LineOfSightBall( - const float x, const float y, const Ball & ball, const Robot * their_bots, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__LINE_OF_SIGHT_BALL_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/outside_their_defense_area.hpp b/ateam_spatial/include/ateam_spatial/layers/outside_their_defense_area.hpp deleted file mode 100644 index 36ff1129c..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/outside_their_defense_area.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__OUTSIDE_THEIR_DEFENSE_AREA_HPP_ -#define ATEAM_SPATIAL__LAYERS__OUTSIDE_THEIR_DEFENSE_AREA_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float OutsideTheirDefenseArea( - const float x, const float y, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__OUTSIDE_THEIR_DEFENSE_AREA_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/layers/width_of_shot_on_goal.hpp b/ateam_spatial/include/ateam_spatial/layers/width_of_shot_on_goal.hpp deleted file mode 100644 index 28282232e..000000000 --- a/ateam_spatial/include/ateam_spatial/layers/width_of_shot_on_goal.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__LAYERS__WIDTH_OF_SHOT_ON_GOAL_HPP_ -#define ATEAM_SPATIAL__LAYERS__WIDTH_OF_SHOT_ON_GOAL_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float WidthOfShotOnGoal( - const float x, const float y, const Robot * their_bots, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::layers - -#endif // ATEAM_SPATIAL__LAYERS__WIDTH_OF_SHOT_ON_GOAL_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/map_id.hpp b/ateam_spatial/include/ateam_spatial/map_id.hpp deleted file mode 100644 index 3f88a196e..000000000 --- a/ateam_spatial/include/ateam_spatial/map_id.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__MAP_ID_HPP_ -#define ATEAM_SPATIAL__MAP_ID_HPP_ - -#include - -namespace ateam_spatial -{ - -enum class MapId : std::size_t { - ReceiverPositionQuality = 0, - MapCount -}; - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__MAP_ID_HPP_ \ No newline at end of file diff --git a/ateam_spatial/include/ateam_spatial/maps/receiver_position_quality.hpp b/ateam_spatial/include/ateam_spatial/maps/receiver_position_quality.hpp deleted file mode 100644 index 3774eec9f..000000000 --- a/ateam_spatial/include/ateam_spatial/maps/receiver_position_quality.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__MAPS__RECEIVER_POSITION_QUALITY_HPP_ -#define ATEAM_SPATIAL__MAPS__RECEIVER_POSITION_QUALITY_HPP_ - -#include "ateam_spatial/types.hpp" -#include "ateam_spatial/cuda_hostdev.hpp" - -namespace ateam_spatial::maps -{ - -CUDA_HOSTDEV float ReceiverPositionQuality( - const int x, const int y, const Ball & ball, const Robot * their_bots, const FieldDimensions & field_dims, - const SpatialSettings & settings); - -} // namespace ateam_spatial::maps - -#endif // ATEAM_SPATIAL__MAPS__RECEIVER_POSITION_QUALITY_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/min_max_loc_kernel.hpp b/ateam_spatial/include/ateam_spatial/min_max_loc_kernel.hpp deleted file mode 100644 index 432ebe2dc..000000000 --- a/ateam_spatial/include/ateam_spatial/min_max_loc_kernel.hpp +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__MIN_MAX_LOC_KERNEL_HPP_ -#define ATEAM_SPATIAL__MIN_MAX_LOC_KERNEL_HPP_ - -namespace ateam_spatial -{ - -__device__ void atomicMax(uint32_t* const address, const uint32_t index, const float * buffer) -{ - if(buffer[*address] >= buffer[index]) { - return; - } - - uint32_t old = *address; - uint32_t assumed = old; - - do - { - assumed = old; - if (buffer[assumed] >= buffer[index]) - { - break; - } - - old = atomicCAS(address, assumed, index); - } while (assumed != old); -} - - -__device__ void atomicMin(uint32_t* const address, const uint32_t index, const float * buffer) -{ - if(buffer[*address] <= buffer[index]) { - return; - } - - uint32_t old = *address; - uint32_t assumed = old; - - do - { - assumed = old; - if (buffer[assumed] <= buffer[index]) - { - break; - } - - old = atomicCAS(address, assumed, index); - } while (assumed != old); -} - -template -__global__ void max_loc_kernel(const float * buffer, const uint32_t buffer_size, uint32_t * loc_out) -{ - const auto idx = blockDim.x * blockIdx.x + threadIdx.x; - __shared__ uint32_t shared_index_data[blockSize]; - shared_index_data[threadIdx.x] = idx; - __syncthreads(); - - for(int stride=1; stride < blockDim.x; stride *= 2) { - if (idx < buffer_size) { - if (threadIdx.x % (2*stride) == 0) { - const auto lhs_block_index = threadIdx.x; - const auto rhs_block_index = threadIdx.x + stride; - const auto lhs_index = shared_index_data[lhs_block_index]; - const auto rhs_index = shared_index_data[rhs_block_index]; - if (rhs_block_index < buffer_size) { - const float lhs_value = buffer[lhs_index]; - const float rhs_value = buffer[rhs_index]; - if(lhs_value > rhs_value) { - shared_index_data[lhs_block_index] = lhs_index; - } else { - shared_index_data[lhs_block_index] = rhs_index; - } - } else { - shared_index_data[lhs_block_index] = lhs_index; - } - } - } - __syncthreads(); - } - if (threadIdx.x == 0) { - atomicMax(loc_out, shared_index_data[0], buffer); - } -} - -template -__global__ void min_loc_kernel(const float * buffer, const uint32_t buffer_size, uint32_t * loc_out) -{ - const auto idx = blockDim.x * blockIdx.x + threadIdx.x; - __shared__ uint32_t shared_index_data[blockSize]; - shared_index_data[threadIdx.x] = idx; - __syncthreads(); - - for(int stride=1; stride < blockDim.x; stride *= 2) { - if (idx < buffer_size) { - if (threadIdx.x % (2*stride) == 0) { - const auto lhs_block_index = threadIdx.x; - const auto rhs_block_index = threadIdx.x + stride; - const auto lhs_index = shared_index_data[lhs_block_index]; - const auto rhs_index = shared_index_data[rhs_block_index]; - if (rhs_block_index < buffer_size) { - const float lhs_value = buffer[lhs_index]; - const float rhs_value = buffer[rhs_index]; - if(lhs_value < rhs_value) { - shared_index_data[lhs_block_index] = lhs_index; - } else { - shared_index_data[lhs_block_index] = rhs_index; - } - } else { - shared_index_data[lhs_block_index] = lhs_index; - } - } - } - __syncthreads(); - } - if (threadIdx.x == 0) { - atomicMin(loc_out, shared_index_data[0], buffer); - } -} - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__MIN_MAX_LOC_KERNEL_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/render_kernel.hpp b/ateam_spatial/include/ateam_spatial/render_kernel.hpp deleted file mode 100644 index bf5aefa23..000000000 --- a/ateam_spatial/include/ateam_spatial/render_kernel.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__RENDER_KERNEL_HPP_ -#define ATEAM_SPATIAL__RENDER_KERNEL_HPP_ - -#include "map_id.hpp" -#include "types.hpp" - -#ifdef __cplusplus -extern "C" { -#endif - -namespace ateam_spatial -{ - -__global__ void render_kernel(const float * input_buffer, const std::size_t buffer_size, - const float min_value, const float max_value, - uint8_t * output_buffer); - -} // namespace ateam_spatial - -#ifdef __cplusplus -} -#endif - -#endif // ATEAM_SPATIAL__UPDATE_MAPS_KERNEL_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/spatial_evaluator.hpp b/ateam_spatial/include/ateam_spatial/spatial_evaluator.hpp deleted file mode 100644 index 87d2ce79b..000000000 --- a/ateam_spatial/include/ateam_spatial/spatial_evaluator.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__SPATIAL_EVALUATOR_HPP_ -#define ATEAM_SPATIAL__SPATIAL_EVALUATOR_HPP_ - -#include -#include -#include "gpu_array.hpp" -#include "gpu_object.hpp" -#include "gpu_multibuffer.hpp" -#include "gpu_vector.hpp" -#include "types.hpp" -#include "map_id.hpp" - -namespace ateam_spatial -{ - - class SpatialEvaluator - { - public: - SpatialEvaluator(); - - void UpdateMaps(const FieldDimensions &field, - const Ball &ball, - const std::array &our_bots, - const std::array &their_bots); - - void CopyMapBuffer(const MapId map, std::vector & destination); - - void RenderMapBuffer(const MapId map, std::vector & destination); - - const SpatialSettings & GetSettings() const { - return settings_; - } - - Point GetMaxLocation(const MapId map); - - float GetValueAtLocation(const MapId map, const Point & location); - - private: - bool cuda_device_available_; - SpatialSettings settings_; - FieldDimensions cached_field_dims_; - GpuObject gpu_spatial_settings_; - GpuObject gpu_field_dims_; - GpuObject gpu_ball_; - GpuArray gpu_our_bots_; - GpuArray gpu_their_bots_; - GpuMultibuffer gpu_map_buffers_; - GpuVector gpu_render_buffer_; - - bool IsReady(); - - void UpdateBufferSizes(const FieldDimensions &field); - - std::size_t GetMaxLoc(const MapId map); - std::size_t GetMinLoc(const MapId map); - }; - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__SPATIAL_EVALUATOR_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/types.hpp b/ateam_spatial/include/ateam_spatial/types.hpp deleted file mode 100644 index 12122980b..000000000 --- a/ateam_spatial/include/ateam_spatial/types.hpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__TYPES_HPP_ -#define ATEAM_SPATIAL__TYPES_HPP_ - -namespace ateam_spatial -{ - -struct Point -{ - float x; - float y; -}; - -struct Robot -{ - bool visible = false; - float x = 0.0; - float y = 0.0; - float theta = 0.0; - float x_vel = 0.0; - float y_vel = 0.0; - float theta_vel = 0.0; -}; - -struct Ball -{ - float x = 0.0; - float y = 0.0; - float x_vel = 0.0; - float y_vel = 0.0; -}; - -struct FieldDimensions -{ - float field_width = 0.0; - float field_length = 0.0; - float goal_width = 0.0; - float goal_depth = 0.0; - float boundary_width = 0.0; - float defense_area_width = 0.0; - float defense_area_depth = 0.0; - - bool operator==(const FieldDimensions & other) const - { - const float kEpsilon = 1e-3; - auto near_eq = [&kEpsilon](const float & a, const float & b) { - return std::abs(a - b) < kEpsilon; - }; - return near_eq(field_width, other.field_width) && - near_eq(field_length, other.field_length) && - near_eq(goal_width, other.goal_width) && - near_eq(goal_depth, other.goal_depth) && - near_eq(boundary_width, other.boundary_width) && - near_eq(defense_area_width, other.defense_area_width) && - near_eq(defense_area_depth, other.defense_area_depth); - } - - bool operator!=(const FieldDimensions & other) const - { - return !operator==(other); - } -}; - -struct SpatialSettings -{ - float resolution = 0.0; // m / px - std::size_t width = 0; - std::size_t height = 0; -}; - -} // namespace ateam_spatial - -#endif // ATEAM_SPATIAL__TYPES_HPP_ diff --git a/ateam_spatial/include/ateam_spatial/update_maps_kernel.hpp b/ateam_spatial/include/ateam_spatial/update_maps_kernel.hpp deleted file mode 100644 index 00789749e..000000000 --- a/ateam_spatial/include/ateam_spatial/update_maps_kernel.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_SPATIAL__UPDATE_MAPS_KERNEL_HPP_ -#define ATEAM_SPATIAL__UPDATE_MAPS_KERNEL_HPP_ - -#include "types.hpp" - -#ifdef __cplusplus -extern "C" { -#endif - -namespace ateam_spatial -{ - -__global__ void update_maps_kernel(const SpatialSettings * spatial_settings, - const FieldDimensions * field_dimensions, const Ball * ball, - const Robot * our_bots, const Robot * their_bots, - float * map_buffers, const std::size_t map_count, - const std::size_t map_buffer_size); - -} // namespace ateam_spatial - -#ifdef __cplusplus -} -#endif - -#endif // ATEAM_SPATIAL__UPDATE_MAPS_KERNEL_HPP_ diff --git a/ateam_spatial/package.xml b/ateam_spatial/package.xml deleted file mode 100644 index 7016ca1a0..000000000 --- a/ateam_spatial/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - ateam_spatial - 0.0.0 - Spatial analysis library. - Matthew Barulic - MIT - - ament_cmake - - nvidia-cuda - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ateam_spatial/src/device_availability.cu b/ateam_spatial/src/device_availability.cu deleted file mode 100644 index 910cc991e..000000000 --- a/ateam_spatial/src/device_availability.cu +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "ateam_spatial/device_availability.hpp" -#include - -namespace ateam_spatial -{ - -bool IsCudaDeviceAvailable() -{ - int num_cuda_devices = 0; - const auto ret = cudaGetDeviceCount(&num_cuda_devices); - return ret == cudaSuccess; -} - -} // namespace ateam_spatial diff --git a/ateam_spatial/src/layers/distance_down_field.cu b/ateam_spatial/src/layers/distance_down_field.cu deleted file mode 100644 index e9df79e80..000000000 --- a/ateam_spatial/src/layers/distance_down_field.cu +++ /dev/null @@ -1,11 +0,0 @@ -#include "ateam_spatial/layers/distance_down_field.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float DistanceDownField(const float x, const FieldDimensions & field_dims, const SpatialSettings & settings) { - return x + (WorldWidthReal(field_dims) / 2.0f); -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/layers/distance_from_field_edge.cu b/ateam_spatial/src/layers/distance_from_field_edge.cu deleted file mode 100644 index af43bd6e5..000000000 --- a/ateam_spatial/src/layers/distance_from_field_edge.cu +++ /dev/null @@ -1,21 +0,0 @@ -#include "ateam_spatial/layers/distance_from_field_edge.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float DistanceFromFieldEdge(const float x, const float y, const FieldDimensions & field_dims, const SpatialSettings & settings) { - const auto x_real = x; - const auto y_real = y; - const auto half_world_width = WorldWidthReal(field_dims) / 2.0f; - const auto half_world_height = WorldHeightReal(field_dims) / 2.0f; - const auto half_world_diff = half_world_width - half_world_height; - - if(fabs(x_real) < half_world_diff || (fabs(x_real) - half_world_diff) < fabs(y_real)) { - return half_world_height - fabs(y_real); - } else { - return half_world_width - fabs(x_real); - } -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/layers/distance_to_their_bots.cu b/ateam_spatial/src/layers/distance_to_their_bots.cu deleted file mode 100644 index 0bd370816..000000000 --- a/ateam_spatial/src/layers/distance_to_their_bots.cu +++ /dev/null @@ -1,23 +0,0 @@ -#include "ateam_spatial/layers/width_of_shot_on_goal.hpp" -#include "ateam_spatial/layers/line_of_sight.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float DistanceToTheirBots(const float x, const float y, const Robot * their_robots, const FieldDimensions & field_dims, const SpatialSettings & settings) { - const auto real_x = x; - const auto real_y = y; - auto result = MAXFLOAT; - for(auto i = 0; i < 16; ++i) { - const auto & robot = their_robots[i]; - if(!robot.visible) { - continue; - } - const auto distance = hypotf(robot.x - real_x, robot.y - real_y); - result = min(result, distance); - } - return result; -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/layers/in_field.cu b/ateam_spatial/src/layers/in_field.cu deleted file mode 100644 index 890c82069..000000000 --- a/ateam_spatial/src/layers/in_field.cu +++ /dev/null @@ -1,16 +0,0 @@ -#include "ateam_spatial/layers/in_field.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float InField(const float x, const float y, const FieldDimensions & field_dims, const SpatialSettings & settings) { - const auto pos_x_thresh = field_dims.field_length / 2.0f; - const auto neg_x_thresh = -field_dims.field_length / 2.0f; - const auto pos_y_thresh = field_dims.field_width / 2.0f; - const auto neg_y_thresh = -field_dims.field_width / 2.0f; - - return (x <= pos_x_thresh) && (x >= neg_x_thresh) && (y <= pos_y_thresh) && (y >= neg_y_thresh); -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/layers/line_of_sight.cu b/ateam_spatial/src/layers/line_of_sight.cu deleted file mode 100644 index e3b8d7dda..000000000 --- a/ateam_spatial/src/layers/line_of_sight.cu +++ /dev/null @@ -1,40 +0,0 @@ -#include "ateam_spatial/layers/line_of_sight_ball.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float PointDistance(const float x_1, const float y_1, const float x_2, const float y_2) { - return hypotf((x_2 - x_1), (y_2 - y_1)); -} - -CUDA_HOSTDEV float DistanceToSegment(const float line_1_x, const float line_1_y, const float line_2_x, const float line_2_y, const float point_x, const float point_y) -{ - const auto l2 = PointDistance(line_1_x, line_1_y, line_2_x, line_2_y); - if (fabsf(l2) < 1e-8f) { - return PointDistance(line_1_x, line_1_y, point_x, point_y); - } - auto t = (((point_x - line_1_x) * (line_2_x - line_1_x)) + ((point_y - line_1_y)*(line_2_y - line_1_y))) / (l2*l2); - t = max(0.0f, min(1.0f, t)); - const auto projected_x = line_1_x + (t * (line_2_x - line_1_x)); - const auto projected_y = line_1_y + (t * (line_2_y - line_1_y)); - return PointDistance(point_x, point_y, projected_x, projected_y); -} - -CUDA_HOSTDEV float LineOfSight(const float src_x, const float src_y, const float dst_x, const float dst_y, const Robot * their_bots, const FieldDimensions & field_dims, const SpatialSettings & settings) { - const auto robot_radius = 0.09f; - - bool result = 1; - for(auto i = 0; i < 16; ++i) { - const auto & robot = their_bots[i]; - if(!robot.visible) { - continue; - } - const auto distance = DistanceToSegment(dst_x, dst_y, src_x, src_y, robot.x, robot.y); - result &= distance > robot_radius; - } - - return result; -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/layers/line_of_sight_ball.cu b/ateam_spatial/src/layers/line_of_sight_ball.cu deleted file mode 100644 index 912185451..000000000 --- a/ateam_spatial/src/layers/line_of_sight_ball.cu +++ /dev/null @@ -1,15 +0,0 @@ -#include "ateam_spatial/layers/line_of_sight_ball.hpp" -#include "ateam_spatial/layers/line_of_sight.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float LineOfSightBall(const float x, const float y, const Ball & ball, const Robot * their_bots, const FieldDimensions & field_dims, const SpatialSettings & settings) { - const auto real_x = x; - const auto real_y = y; - - return LineOfSight(real_x, real_y, ball.x, ball.y, their_bots, field_dims, settings); -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/layers/outside_their_defense_area.cu b/ateam_spatial/src/layers/outside_their_defense_area.cu deleted file mode 100644 index 8cbb90c27..000000000 --- a/ateam_spatial/src/layers/outside_their_defense_area.cu +++ /dev/null @@ -1,17 +0,0 @@ -#include "ateam_spatial/layers/outside_their_defense_area.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float OutsideTheirDefenseArea(const float x, const float y, const FieldDimensions & field_dims, const SpatialSettings & settings) { - const auto robot_diameter = 0.180f; - const auto pos_x_thresh = field_dims.field_length / 2.0f; - const auto neg_x_thresh = (field_dims.field_length / 2.0f) - (field_dims.defense_area_depth + (robot_diameter * 3.0f)); - const auto pos_y_thresh = (field_dims.defense_area_width / 2.0f) + (robot_diameter * 3.0f); - const auto neg_y_thresh = -pos_y_thresh; - - return !((x <= pos_x_thresh) && (x >= neg_x_thresh) && (y <= pos_y_thresh) && (y >= neg_y_thresh)); -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/layers/width_of_shot_on_goal.cu b/ateam_spatial/src/layers/width_of_shot_on_goal.cu deleted file mode 100644 index 34faafc81..000000000 --- a/ateam_spatial/src/layers/width_of_shot_on_goal.cu +++ /dev/null @@ -1,42 +0,0 @@ -#include "ateam_spatial/layers/width_of_shot_on_goal.hpp" -#include "ateam_spatial/layers/line_of_sight.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::layers -{ - -CUDA_HOSTDEV float WidthOfShotOnGoal(const float x, const float y, const Robot * their_robots, const FieldDimensions & field_dims, const SpatialSettings & settings) { - constexpr int kNumSteps = 100; - - const auto real_x = x; - const auto real_y = y; - - const auto goal_x = field_dims.field_length / 2.0f; - const auto goal_y_start = -field_dims.goal_width / 2.0f; - - const auto step_size = field_dims.goal_width / kNumSteps; - - int result = 0; - int counter = 0; - - for(auto step = 0; step < kNumSteps; ++step) { - const auto goal_y = goal_y_start + (step * step_size); - if(LineOfSight(real_x, real_y, goal_x, goal_y, their_robots, field_dims, settings)) { - counter++; - } else { - result = max(result, counter); - counter = 0; - } - } - result = max(result, counter); - - // Avoid shots at shallow angles - const auto angle_to_goal = atan2(fabs(goal_x - real_x), fabs(real_y)); - if (angle_to_goal < 0.7f) { - return 0.0f; - } - - return result * step_size; -} - -} // namespace ateam_spatial::layers diff --git a/ateam_spatial/src/maps/receiver_position_quality.cu b/ateam_spatial/src/maps/receiver_position_quality.cu deleted file mode 100644 index dc31b8661..000000000 --- a/ateam_spatial/src/maps/receiver_position_quality.cu +++ /dev/null @@ -1,31 +0,0 @@ -#include "ateam_spatial/maps/receiver_position_quality.hpp" -#include "ateam_spatial/layers/distance_down_field.hpp" -#include "ateam_spatial/layers/distance_from_field_edge.hpp" -#include "ateam_spatial/layers/distance_to_their_bots.hpp" -#include "ateam_spatial/layers/in_field.hpp" -#include "ateam_spatial/layers/line_of_sight_ball.hpp" -#include "ateam_spatial/layers/outside_their_defense_area.hpp" -#include "ateam_spatial/layers/width_of_shot_on_goal.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" - -namespace ateam_spatial::maps -{ - -CUDA_HOSTDEV float ReceiverPositionQuality(const int spatial_x, const int spatial_y, const Ball & ball, const Robot * their_bots, const FieldDimensions &field_dims, const SpatialSettings &settings) -{ - const auto x = SpatialToRealX(spatial_x, field_dims, settings); - const auto y = SpatialToRealY(spatial_y, field_dims, settings); - const auto max_edge_dist = 1.25f; - const auto edge_dist_multiplier = min(layers::DistanceFromFieldEdge(x, y, field_dims, settings), max_edge_dist) / max_edge_dist; - const auto shot_width_multiplier = layers::WidthOfShotOnGoal(x, y, their_bots, field_dims, settings) / field_dims.goal_width; - const auto robot_distance_multiplier = min(layers::DistanceToTheirBots(x, y, their_bots, field_dims, settings), 1.0f); - return layers::InField(x, y, field_dims, settings) - * layers::OutsideTheirDefenseArea(x, y, field_dims, settings) - * layers::LineOfSightBall(x, y, ball, their_bots, field_dims, settings) - * edge_dist_multiplier - * shot_width_multiplier - * robot_distance_multiplier - * layers::DistanceDownField(x, field_dims, settings); -} - -} // namespace ateam_spatial::maps diff --git a/ateam_spatial/src/render_kernel.cu b/ateam_spatial/src/render_kernel.cu deleted file mode 100644 index 258a8e7f9..000000000 --- a/ateam_spatial/src/render_kernel.cu +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "ateam_spatial/render_kernel.hpp" -#include -#include - -namespace ateam_spatial -{ - -__global__ void render_kernel(const float * input_buffer, const std::size_t buffer_size, - const float min_value, const float max_value, - uint8_t * output_buffer) -{ - const auto index = (blockIdx.x * blockDim.x) + threadIdx.x; - if(index >= buffer_size) { - return; - } - - output_buffer[index] = (input_buffer[index] - min_value) * (255 / (max_value - min_value)); -} - -} // namespace ateam_spatial diff --git a/ateam_spatial/src/spatial_evaluator.cu b/ateam_spatial/src/spatial_evaluator.cu deleted file mode 100644 index 78e500e37..000000000 --- a/ateam_spatial/src/spatial_evaluator.cu +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "ateam_spatial/spatial_evaluator.hpp" -#include -#include -#include "ateam_spatial/render_kernel.hpp" -#include "ateam_spatial/update_maps_kernel.hpp" -#include "ateam_spatial/coordinate_conversions.hpp" -#include "ateam_spatial/gpu_vector.hpp" -#include "ateam_spatial/min_max_loc_kernel.hpp" -#include "ateam_spatial/device_availability.hpp" - -namespace ateam_spatial -{ - -SpatialEvaluator::SpatialEvaluator() -{ - settings_.resolution = 1e-1; // TODO(barulicm): parameterize this - cuda_device_available_ = IsCudaDeviceAvailable(); - if(!cuda_device_available_) { - std::cerr << "Could not detect CUDA device. GPU-accelerated spatial code is disabled." << std::endl; - } -} - -void SpatialEvaluator::UpdateMaps(const FieldDimensions &field, const Ball &ball, const std::array &our_bots, const std::array &their_bots) -{ - UpdateBufferSizes(field); - if(!IsReady()) { - return; - } - gpu_spatial_settings_.CopyToGpu(settings_); - gpu_field_dims_.CopyToGpu(field); - gpu_ball_.CopyToGpu(ball); - gpu_our_bots_.CopyToGpu(our_bots); - gpu_their_bots_.CopyToGpu(their_bots); - - const dim3 block_size(32,32,1); - const auto grid_x = std::ceil(static_cast(settings_.width) / block_size.x); - const auto grid_y = std::ceil(static_cast(settings_.height) / block_size.y); - const auto grid_z = static_cast(MapId::MapCount); - const dim3 grid_size(grid_x, grid_y, grid_z); - update_maps_kernel<<>>(gpu_spatial_settings_.Get(), gpu_field_dims_.Get(), gpu_ball_.Get(), gpu_our_bots_.Get(), gpu_their_bots_.Get(), gpu_map_buffers_.Get(), gpu_map_buffers_.BufferCount(), gpu_map_buffers_.BufferSize()); - if(const auto ret = cudaDeviceSynchronize();ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to launch update maps kernel: ") + cudaGetErrorString(ret)); - } - if(const auto ret = cudaGetLastError(); ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to run update maps kernel: ") + cudaGetErrorString(ret)); - } -} - -void SpatialEvaluator::CopyMapBuffer(const MapId map, std::vector & destination) -{ - gpu_map_buffers_.CopyFromGpu(static_cast(map), destination); -} - - -void SpatialEvaluator::RenderMapBuffer(const MapId map, std::vector & destination) -{ - if(!IsReady()) { - return; - } - const auto max_index = GetMaxLoc(map); - const auto min_index = GetMinLoc(map); - const auto max_value = gpu_map_buffers_.CopyValueFromGpu(static_cast(map), max_index); - const auto min_value = gpu_map_buffers_.CopyValueFromGpu(static_cast(map), min_index); - - const dim3 block_size(64); - const dim3 grid_size(std::ceil(static_cast(gpu_map_buffers_.BufferSize()) / block_size.x)); - render_kernel<<>>(gpu_map_buffers_.Get(static_cast(map)), gpu_map_buffers_.BufferSize(), min_value, max_value, gpu_render_buffer_.Get()); - if(const auto ret = cudaDeviceSynchronize();ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to launch render kernel: ") + cudaGetErrorString(ret)); - } - if(const auto ret = cudaGetLastError(); ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to run render kernel: ") + cudaGetErrorString(ret)); - } - - gpu_render_buffer_.CopyFromGpu(destination); -} - -Point SpatialEvaluator::GetMaxLocation(const MapId map) -{ - if(!IsReady()) { - return Point{0.f,0.f}; - } - const auto max_index = GetMaxLoc(map); - const auto max_y = max_index / settings_.width; - const auto max_x = max_index % settings_.width; - return Point{ - SpatialToRealX(max_x, cached_field_dims_, settings_), - SpatialToRealY(max_y, cached_field_dims_, settings_) - }; -} - -float SpatialEvaluator::GetValueAtLocation(const MapId map, const Point & location) -{ - if(!IsReady()) { - return 0.f; - } - const auto index = (RealToSpatialY(location.y, cached_field_dims_, settings_) * settings_.width) - + RealToSpatialX(location.x, cached_field_dims_, settings_); - return gpu_map_buffers_.CopyValueFromGpu(static_cast(MapId::ReceiverPositionQuality), index); -} - -bool SpatialEvaluator::IsReady() { - return cuda_device_available_ && settings_.height != 0 && settings_.width != 0; -} - -void SpatialEvaluator::UpdateBufferSizes(const FieldDimensions &field) -{ - if(field == cached_field_dims_) { - return; - } - const auto world_width_spatial = WorldWidthSpatial(field, settings_); - const auto world_height_spatial = WorldHeightSpatial(field, settings_); - const auto buffer_size = world_width_spatial * world_height_spatial; - gpu_map_buffers_ = GpuMultibuffer(static_cast(MapId::MapCount), buffer_size); - gpu_render_buffer_ = GpuVector(buffer_size); - settings_.width = world_width_spatial; - settings_.height = world_height_spatial; - cached_field_dims_ = field; -} - -std::size_t SpatialEvaluator::GetMaxLoc(const MapId map) -{ - const auto gpu_buffer = gpu_map_buffers_.Get(static_cast(map)); - constexpr auto kThreadsPerBlock = 64; - const dim3 block_size(kThreadsPerBlock); - const dim3 grid_size(std::ceil(static_cast(gpu_map_buffers_.BufferSize()) / block_size.x)); - GpuObject gpu_max_index; - max_loc_kernel<<>>(gpu_buffer, gpu_map_buffers_.BufferSize(), gpu_max_index.Get()); - if(const auto ret = cudaDeviceSynchronize();ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to launch max_loc_kernel: ") + cudaGetErrorString(ret)); - } - if(const auto ret = cudaGetLastError(); ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to run max_loc_kernel: ") + cudaGetErrorString(ret)); - } - uint32_t result; - gpu_max_index.CopyFromGpu(result); - return result; -} - -std::size_t SpatialEvaluator::GetMinLoc(const MapId map) -{ - const auto gpu_buffer = gpu_map_buffers_.Get(static_cast(map)); - constexpr auto kThreadsPerBlock = 64; - const dim3 block_size(kThreadsPerBlock); - const dim3 grid_size(std::ceil(static_cast(gpu_map_buffers_.BufferSize()) / block_size.x)); - GpuObject gpu_min_index; - min_loc_kernel<<>>(gpu_buffer, gpu_map_buffers_.BufferSize(), gpu_min_index.Get()); - if(const auto ret = cudaDeviceSynchronize();ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to launch max_loc_kernel: ") + cudaGetErrorString(ret)); - } - if(const auto ret = cudaGetLastError(); ret != cudaSuccess) { - throw std::runtime_error(std::string("Failed to run max_loc_kernel: ") + cudaGetErrorString(ret)); - } - uint32_t result; - gpu_min_index.CopyFromGpu(result); - return result; -} - -} // namespace ateam_spatial diff --git a/ateam_spatial/src/update_maps_kernel.cu b/ateam_spatial/src/update_maps_kernel.cu deleted file mode 100644 index 9deac9377..000000000 --- a/ateam_spatial/src/update_maps_kernel.cu +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "ateam_spatial/update_maps_kernel.hpp" -#include -#include "ateam_spatial/maps/receiver_position_quality.hpp" -#include "ateam_spatial/map_id.hpp" - -namespace ateam_spatial -{ - -__global__ void update_maps_kernel(const SpatialSettings * spatial_settings, - const FieldDimensions * field_dimensions, const Ball * ball, - const Robot * our_bots, const Robot * their_bots, - float * map_buffers, const std::size_t map_count, - const std::size_t map_buffer_size) -{ - if(map_count != static_cast(MapId::MapCount)) { - printf("update_maps_kernel(): map_count does not match expected value.\n"); - return; - } - - const auto map_index = blockIdx.z; - if(map_index >= map_count) { - return; - } - - const auto spatial_x = (blockIdx.x * blockDim.x) + threadIdx.x; - const auto spatial_y = (blockIdx.y * blockDim.y) + threadIdx.y; - if(spatial_x >= spatial_settings->width || spatial_y >= spatial_settings->height) { - return; - } - - const auto map_buffer_index = (spatial_y * spatial_settings->width) + spatial_x; - const auto output_buffer_index = (map_index * map_buffer_size) + map_buffer_index; - - float output = 0.0f; - switch(static_cast(map_index)) { - case MapId::ReceiverPositionQuality: - output = maps::ReceiverPositionQuality(spatial_x, spatial_y, *ball, their_bots, *field_dimensions, *spatial_settings); - break; - } - - map_buffers[output_buffer_index] = output; -} - -} // namespace ateam_spatial diff --git a/ateam_spatial/test/CMakeLists.txt b/ateam_spatial/test/CMakeLists.txt deleted file mode 100644 index 367eb1113..000000000 --- a/ateam_spatial/test/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -find_package(ament_cmake_gmock REQUIRED) - -ament_add_gmock(ateam_spatial_tests - coordinate_conversions_tests.cpp - gpu_array_tests.cpp - gpu_multibuffer_tests.cpp - gpu_object_tests.cpp - gpu_vector_tests.cpp - layers_tests.cpp - maps_tests.cpp - min_max_loc_kernel_tests.cu - spatial_evaluator_tests.cpp -) - -target_include_directories(ateam_spatial_tests PUBLIC - $ -) - -target_link_libraries(ateam_spatial_tests - ${PROJECT_NAME} -) - -set_target_properties(ateam_spatial_tests PROPERTIES - CUDA_SEPARABLE_COMPILATION ON - CUDA_STANDARD 17 - CUDA_ARCHITECTURES native -) diff --git a/ateam_spatial/test/coordinate_conversions_tests.cpp b/ateam_spatial/test/coordinate_conversions_tests.cpp deleted file mode 100644 index 96851cf44..000000000 --- a/ateam_spatial/test/coordinate_conversions_tests.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include - -TEST(CooridinateConversionsTests, BasicTest) -{ - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - EXPECT_FLOAT_EQ(WorldWidthReal(field), 16.6); - EXPECT_FLOAT_EQ(WorldHeightReal(field), 9.6); - EXPECT_EQ(WorldWidthSpatial(field, settings), 16600); - EXPECT_EQ(WorldHeightSpatial(field, settings), 9600); - EXPECT_FLOAT_EQ(SpatialToRealX(1000, field, settings), -7.3); - EXPECT_FLOAT_EQ(SpatialToRealY(2000, field, settings), -2.8); - EXPECT_EQ(RealToSpatialDist(0.1, settings), 100); -} diff --git a/ateam_spatial/test/gpu_array_tests.cpp b/ateam_spatial/test/gpu_array_tests.cpp deleted file mode 100644 index 02fdb51fe..000000000 --- a/ateam_spatial/test/gpu_array_tests.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include -#include "skip_if_no_gpu.hpp" - - -TEST(GpuArrayTests, CopyBackAndForth) { - SKIP_IF_NO_GPU(); - const std::array in{0, 2, 4, 6, 8, 10, 12, 14, 16, 18}; - std::array out; - - ateam_spatial::GpuArray a; - a.CopyToGpu(in); - a.CopyFromGpu(out); - EXPECT_THAT(out, ::testing::ContainerEq(in)); -} diff --git a/ateam_spatial/test/gpu_multibuffer_tests.cpp b/ateam_spatial/test/gpu_multibuffer_tests.cpp deleted file mode 100644 index cb2df90bd..000000000 --- a/ateam_spatial/test/gpu_multibuffer_tests.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include -#include "skip_if_no_gpu.hpp" - - -TEST(GpuMultibufferTests, CopyBackAndForth) { - SKIP_IF_NO_GPU(); - ateam_spatial::GpuMultibuffer mb(2, 10); - - const std::vector in1{1, 3, 5, 7, 9, 11, 13, 15, 17, 19}; - const std::vector in2{0, 2, 4, 6, 8, 10, 12, 14, 16, 18}; - mb.CopyToGpu(0, in1); - mb.CopyToGpu(1, in2); - - std::vector out1; - std::vector out2; - mb.CopyFromGpu(0, out1); - mb.CopyFromGpu(1, out2); - - EXPECT_THAT(out1, ::testing::ContainerEq(in1)); - EXPECT_THAT(out2, ::testing::ContainerEq(in2)); -} diff --git a/ateam_spatial/test/gpu_object_tests.cpp b/ateam_spatial/test/gpu_object_tests.cpp deleted file mode 100644 index 24d052c65..000000000 --- a/ateam_spatial/test/gpu_object_tests.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include "skip_if_no_gpu.hpp" - - -TEST(GpuObjectTests, CopyBackAndForth) { - SKIP_IF_NO_GPU(); - ateam_spatial::GpuObject g(12); - int out = 0; - g.CopyFromGpu(out); - EXPECT_EQ(out, 12); -} diff --git a/ateam_spatial/test/gpu_vector_tests.cpp b/ateam_spatial/test/gpu_vector_tests.cpp deleted file mode 100644 index b8acf2dbe..000000000 --- a/ateam_spatial/test/gpu_vector_tests.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include -#include "skip_if_no_gpu.hpp" - - -TEST(GpuVectorTests, CopyBackAndForth) { - SKIP_IF_NO_GPU(); - const std::vector in{1, 3, 5, 7, 9, 11, 13, 15, 17, 19}; - std::vector out; - - ateam_spatial::GpuVector v; - v.CopyToGpu(in); - v.CopyFromGpu(out); - EXPECT_THAT(out, ::testing::ContainerEq(in)); -} diff --git a/ateam_spatial/test/layers_tests.cpp b/ateam_spatial/test/layers_tests.cpp deleted file mode 100644 index 123c93607..000000000 --- a/ateam_spatial/test/layers_tests.cpp +++ /dev/null @@ -1,248 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include -#include -#include -#include -#include -#include - -TEST(LayersTests, DistanceDownField) -{ - using ateam_spatial::layers::DistanceDownField; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - EXPECT_FLOAT_EQ(DistanceDownField(-8.3, field, settings), 0.0); - EXPECT_FLOAT_EQ(DistanceDownField(0.0, field, settings), 8.3); - EXPECT_FLOAT_EQ(DistanceDownField(8.3, field, settings), 16.6); -} - -TEST(LayersTests, DistanceFromFieldEdge) -{ - using ateam_spatial::layers::DistanceFromFieldEdge; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - const auto kAcceptableError = 1e-4; - EXPECT_NEAR(DistanceFromFieldEdge(-7.8, 0.0, field, settings), 0.5, kAcceptableError); - EXPECT_NEAR(DistanceFromFieldEdge(-7.8, -4.77, field, settings), 0.03, kAcceptableError); - EXPECT_NEAR(DistanceFromFieldEdge(0.0, 0.0, field, settings), 4.8, kAcceptableError); - EXPECT_NEAR(DistanceFromFieldEdge(7.7, 0.0, field, settings), 0.6, kAcceptableError); - EXPECT_NEAR(DistanceFromFieldEdge(0.0, -3.8, field, settings), 1.0, kAcceptableError); -} - -TEST(LayersTests, DistanceToTheirBots) -{ - using ateam_spatial::layers::DistanceToTheirBots; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - std::array their_robots; - their_robots[0] = ateam_spatial::Robot{ - true, - 1.0, - 2.0, - 0.0, - 0.0, - 0.0, - 0.0 - }; - their_robots[1] = ateam_spatial::Robot{ - true, - 4.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - }; - - const auto kAcceptableError = 1e-4; - EXPECT_NEAR(DistanceToTheirBots(0.0, 0.0, their_robots.data(), field, settings), 2.236, kAcceptableError); - EXPECT_NEAR(DistanceToTheirBots(4.0, 0.0, their_robots.data(), field, settings), 0.5, kAcceptableError); -} - -TEST(LayersTests, InField) -{ - using ateam_spatial::layers::InField; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - EXPECT_FLOAT_EQ(InField(-8.3, 0.0, field, settings), 0.0); - EXPECT_FLOAT_EQ(InField(-7.999, 0.0, field, settings), 1.0); - EXPECT_FLOAT_EQ(InField(0.0, 0.0, field, settings), 1.0); - EXPECT_FLOAT_EQ(InField(7.999, 0.0, field, settings), 1.0); - EXPECT_FLOAT_EQ(InField(8.3, 0.0, field, settings), 0.0); - EXPECT_FLOAT_EQ(InField(0.0, -4.8, field, settings), 0.0); - EXPECT_FLOAT_EQ(InField(0.0, -4.499, field, settings), 1.0); - EXPECT_FLOAT_EQ(InField(0.0, 0.0, field, settings), 1.0); - EXPECT_FLOAT_EQ(InField(0.0, 4.499, field, settings), 1.0); - EXPECT_FLOAT_EQ(InField(0.0, 4.8, field, settings), 0.0); -} - -TEST(LayersTests, LineOfSightBall) -{ - using ateam_spatial::layers::LineOfSightBall; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - ateam_spatial::Ball ball{ - 1.0, - 1.0, - 0.0, - 0.0 - }; - - std::array their_robots; - their_robots[0] = ateam_spatial::Robot{ - true, - 1.0, - 2.0, - 0.0, - 0.0, - 0.0, - 0.0 - }; - - EXPECT_FLOAT_EQ(LineOfSightBall(0.0, 0.0, ball, their_robots.data(), field, settings), 1.0); - EXPECT_FLOAT_EQ(LineOfSightBall(1.0, 2.2, ball, their_robots.data(), field, settings), 0.0); - EXPECT_FLOAT_EQ(LineOfSightBall(1.0, 0.2, ball, their_robots.data(), field, settings), 1.0); -} - -TEST(LayersTests, OutsideTheirDefenseArea) -{ - using ateam_spatial::layers::OutsideTheirDefenseArea; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - EXPECT_FLOAT_EQ(OutsideTheirDefenseArea(-8.3, -4.8, field, settings), 1.0); - EXPECT_FLOAT_EQ(OutsideTheirDefenseArea(7.199, 0.0, field, settings), 0.0); - EXPECT_FLOAT_EQ(OutsideTheirDefenseArea(7.199, -3.8, field, settings), 1.0); - EXPECT_FLOAT_EQ(OutsideTheirDefenseArea(7.199, 1.6, field, settings), 1.0); - EXPECT_FLOAT_EQ(OutsideTheirDefenseArea(-7.8, 0.0, field, settings), 1.0); -} - - -TEST(LayersTests, WidthOfShotOnGoal) -{ - using ateam_spatial::layers::WidthOfShotOnGoal; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - std::array their_robots; - their_robots[0] = ateam_spatial::Robot{ - true, - -1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - }; - - const auto kAcceptableError = 1e-2; - EXPECT_NEAR(WidthOfShotOnGoal(0.0, 0.0, their_robots.data(), field, settings), 1.0, kAcceptableError); - their_robots[0].x = 0.2; - their_robots[0].x = 0.0; - EXPECT_NEAR(WidthOfShotOnGoal(0.0, 0.0, their_robots.data(), field, settings), 0.0, kAcceptableError); - their_robots[0].x = 0.2; - their_robots[0].y = 0.09; - EXPECT_NEAR(WidthOfShotOnGoal(0.0, 0.0, their_robots.data(), field, settings), 0.5, kAcceptableError); - their_robots[0].x = 0.2; - their_robots[0].y = -0.09; - EXPECT_NEAR(WidthOfShotOnGoal(0.0, 0.0, their_robots.data(), field, settings), 0.49, kAcceptableError); - their_robots[0].x = 4.5; - their_robots[0].y = 0.18; - EXPECT_NEAR(WidthOfShotOnGoal(0.0, 0.0, their_robots.data(), field, settings), 0.65, kAcceptableError); -} diff --git a/ateam_spatial/test/maps_tests.cpp b/ateam_spatial/test/maps_tests.cpp deleted file mode 100644 index 8d55bdd38..000000000 --- a/ateam_spatial/test/maps_tests.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include - -TEST(MapsTests, ReceiverPositionQuality) -{ - using ateam_spatial::maps::ReceiverPositionQuality; - - ateam_spatial::SpatialSettings settings; - settings.resolution = 0.001; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - ateam_spatial::Ball ball{ - 1.0, - 1.0, - 0.0, - 0.0 - }; - - std::array their_robots; - their_robots[0] = ateam_spatial::Robot{ - true, - 1.0, - 2.0, - 0.0, - 0.0, - 0.0, - 0.0 - }; - - const auto kAcceptableError = 1e-4; - EXPECT_FLOAT_EQ(ReceiverPositionQuality(0, 4800, ball, their_robots.data(), field, settings), 0.0); - EXPECT_NEAR(ReceiverPositionQuality(600, 4800, ball, their_robots.data(), field, settings), 0.288, kAcceptableError); - EXPECT_NEAR(ReceiverPositionQuality(8300, 4800, ball, their_robots.data(), field, settings), 8.3, kAcceptableError); - EXPECT_NEAR(ReceiverPositionQuality(14500, 4800, ball, their_robots.data(), field, settings), 14.5, kAcceptableError); - EXPECT_FLOAT_EQ(ReceiverPositionQuality(16600, 4800, ball, their_robots.data(), field, settings), 0.0); - EXPECT_FLOAT_EQ(ReceiverPositionQuality(9300, 7000, ball, their_robots.data(), field, settings), 0.0); -} diff --git a/ateam_spatial/test/min_max_loc_kernel_tests.cu b/ateam_spatial/test/min_max_loc_kernel_tests.cu deleted file mode 100644 index c8dbf4b71..000000000 --- a/ateam_spatial/test/min_max_loc_kernel_tests.cu +++ /dev/null @@ -1,141 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include -#include -#include - -TEST(MinMaxLocKernelTests, MinLoc) -{ - const std::array data{0.f, 18.f, 4.f, 14.f, 8.f, 12.f, 10.f, 6.f, 16.f, 2.f}; - ateam_spatial::GpuArray gpu_data; - gpu_data.CopyToGpu(data); - constexpr int threads_per_block = 64; - const dim3 block_size(threads_per_block); - const dim3 grid_size(std::ceil(static_cast(data.size()) / block_size.x)); - ateam_spatial::GpuObject gpu_min_index; - ateam_spatial::min_loc_kernel<<>>(gpu_data.Get(), gpu_data.Size(), gpu_min_index.Get()); - if (const auto ret = cudaDeviceSynchronize(); ret != cudaSuccess) - { - FAIL() << "Failed to launch min_loc_kernel: " << cudaGetErrorString(ret); - } - if (const auto ret = cudaGetLastError(); ret != cudaSuccess) - { - FAIL() << "Failed to run min_loc_kernel: " << cudaGetErrorString(ret); - } - uint32_t min_index = 0; - gpu_min_index.CopyFromGpu(min_index); - EXPECT_EQ(min_index, 0); -} - -TEST(MinMaxLocKernelTests, MaxLoc) -{ - const std::array data{0.f, 18.f, 4.f, 14.f, 8.f, 12.f, 10.f, 6.f, 16.f, 2.f}; - ateam_spatial::GpuArray gpu_data; - gpu_data.CopyToGpu(data); - constexpr int threads_per_block = 64; - const dim3 block_size(threads_per_block); - const dim3 grid_size(std::ceil(static_cast(data.size()) / block_size.x)); - ateam_spatial::GpuObject gpu_max_index; - ateam_spatial::max_loc_kernel<<>>(gpu_data.Get(), gpu_data.Size(), gpu_max_index.Get()); - if (const auto ret = cudaDeviceSynchronize(); ret != cudaSuccess) - { - FAIL() << "Failed to launch max_loc_kernel: " << cudaGetErrorString(ret); - } - if (const auto ret = cudaGetLastError(); ret != cudaSuccess) - { - FAIL() << "Failed to run max_loc_kernel: " << cudaGetErrorString(ret); - } - uint32_t max_index = 0; - gpu_max_index.CopyFromGpu(max_index); - EXPECT_EQ(max_index, 1); -} - -TEST(MinMaxLocKernelTests, MaxLoc_MultipleBlocks) -{ - std::array data{}; - std::random_device rdev; - std::default_random_engine randeng(rdev()); - std::uniform_real_distribution randist(0.0, 200.0); - std::generate(data.begin(), data.end(), [&]() - { return randist(randeng); }); - // Fill the first half with zeros to ensure the max is not in the first block - std::fill_n(data.begin(), data.size() / 2, 0.f); - ateam_spatial::GpuArray gpu_data; - gpu_data.CopyToGpu(data); - constexpr int threads_per_block = 64; - const dim3 block_size(threads_per_block); - const dim3 grid_size(std::ceil(static_cast(data.size()) / block_size.x)); - ateam_spatial::GpuObject gpu_max_index; - ateam_spatial::max_loc_kernel<<>>(gpu_data.Get(), gpu_data.Size(), gpu_max_index.Get()); - if (const auto ret = cudaDeviceSynchronize(); ret != cudaSuccess) - { - FAIL() << "Failed to launch max_loc_kernel: " << cudaGetErrorString(ret); - } - if (const auto ret = cudaGetLastError(); ret != cudaSuccess) - { - FAIL() << "Failed to run max_loc_kernel: " << cudaGetErrorString(ret); - } - - const auto max_iter = std::max_element(data.begin(), data.end()); - - ASSERT_NE(max_iter, data.end()) << "Failed to find a max value."; - const auto expected_max_index = std::distance(data.begin(), max_iter); - - uint32_t max_index = 0; - gpu_max_index.CopyFromGpu(max_index); - EXPECT_EQ(max_index, expected_max_index); -} - -TEST(MinMaxLocKernelTests, MaxLoc_UnalignedDataSize) -{ - std::array data{}; - std::random_device rdev; - std::default_random_engine randeng(rdev()); - std::uniform_real_distribution randist(0.0, 200.0); - std::generate(data.begin(), data.end(), [&]() - { return randist(randeng); }); - ateam_spatial::GpuArray gpu_data; - gpu_data.CopyToGpu(data); - constexpr int threads_per_block = 64; - const dim3 block_size(threads_per_block); - const dim3 grid_size(std::ceil(static_cast(data.size()) / block_size.x)); - ateam_spatial::GpuObject gpu_max_index; - ateam_spatial::max_loc_kernel<<>>(gpu_data.Get(), gpu_data.Size(), gpu_max_index.Get()); - if (const auto ret = cudaDeviceSynchronize(); ret != cudaSuccess) - { - FAIL() << "Failed to launch max_loc_kernel: " << cudaGetErrorString(ret); - } - if (const auto ret = cudaGetLastError(); ret != cudaSuccess) - { - FAIL() << "Failed to run max_loc_kernel: " << cudaGetErrorString(ret); - } - - const auto max_iter = std::max_element(data.begin(), data.end()); - - ASSERT_NE(max_iter, data.end()) << "Failed to find a max value."; - const auto expected_max_index = std::distance(data.begin(), max_iter); - - uint32_t max_index = 0; - gpu_max_index.CopyFromGpu(max_index); - EXPECT_EQ(max_index, expected_max_index); -} diff --git a/ateam_spatial/test/skip_if_no_gpu.hpp b/ateam_spatial/test/skip_if_no_gpu.hpp deleted file mode 100644 index 72a469ccd..000000000 --- a/ateam_spatial/test/skip_if_no_gpu.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef SKIP_IF_NO_GPU_HPP__ -#define SKIP_IF_NO_GPU_HPP__ -#include -#include - -#define SKIP_IF_NO_GPU() { if(!ateam_spatial::IsCudaDeviceAvailable()) { GTEST_SKIP(); } } - -#endif // SKIP_IF_NO_GPU_HPP__ diff --git a/ateam_spatial/test/spatial_evaluator_tests.cpp b/ateam_spatial/test/spatial_evaluator_tests.cpp deleted file mode 100644 index 322c3c40f..000000000 --- a/ateam_spatial/test/spatial_evaluator_tests.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2025 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include "skip_if_no_gpu.hpp" - -TEST(SpatialEvaluatorTests, Basic) -{ - SKIP_IF_NO_GPU(); - using ateam_spatial::SpatialEvaluator; - using ateam_spatial::MapId; - SpatialEvaluator eval; - - ateam_spatial::FieldDimensions field; - field.field_length = 16.0; - field.field_width = 9.0; - field.boundary_width = 0.3; - field.defense_area_width = 2.0; - field.defense_area_depth = 1.0; - field.goal_width = 1.0; - field.goal_depth = 0.1; - - ateam_spatial::Ball ball; - - std::array our_bots; - - std::array their_bots; - - eval.UpdateMaps(field, ball, our_bots, their_bots); - - std::vector buffer_out; - - eval.CopyMapBuffer(MapId::ReceiverPositionQuality, buffer_out); - - const auto resolution = eval.GetSettings().resolution; - const auto expected_width = (field.field_width + (2 * field.boundary_width)) / resolution; - const auto expected_length = (field.field_length + (2 * field.boundary_width)) / resolution; - const auto expected_size = expected_length * expected_width; - - EXPECT_EQ(buffer_out.size(), expected_size); - - std::vector rendered_buffer; - eval.RenderMapBuffer(MapId::ReceiverPositionQuality, rendered_buffer); - - EXPECT_EQ(rendered_buffer.size(), expected_size); -}