From 3d43db2225b7ecdc0d24a79216d0c8027ec154c9 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 29 Sep 2025 02:09:23 -0400 Subject: [PATCH 01/19] Adds ateam_game_state package with in progress game_state_tracker implementation. --- .../include/ateam_common/topic_names.hpp | 2 + .../ateam_game_state/CMakeLists.txt | 79 ++++++ state_tracking/ateam_game_state/LICENSE | 17 ++ .../include/ateam_game_state/ball.hpp | 39 +++ .../include/ateam_game_state/field.hpp | 51 ++++ .../include/ateam_game_state/referee_info.hpp | 45 ++++ .../include/ateam_game_state/robot.hpp | 56 ++++ .../ateam_game_state/type_adapters.hpp | 104 ++++++++ .../include/ateam_game_state/world.hpp | 54 ++++ state_tracking/ateam_game_state/package.xml | 27 ++ .../game_state_tracker/game_state_tracker.cpp | 200 ++++++++++++++ .../ateam_game_state/src/type_adapters.cpp | 246 ++++++++++++++++++ 12 files changed, 920 insertions(+) create mode 100644 state_tracking/ateam_game_state/CMakeLists.txt create mode 100644 state_tracking/ateam_game_state/LICENSE create mode 100644 state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp create mode 100644 state_tracking/ateam_game_state/include/ateam_game_state/field.hpp create mode 100644 state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp create mode 100644 state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp create mode 100644 state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp create mode 100644 state_tracking/ateam_game_state/include/ateam_game_state/world.hpp create mode 100644 state_tracking/ateam_game_state/package.xml create mode 100644 state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp create mode 100644 state_tracking/ateam_game_state/src/type_adapters.cpp diff --git a/ateam_common/include/ateam_common/topic_names.hpp b/ateam_common/include/ateam_common/topic_names.hpp index af7a71a2f..65861ec84 100644 --- a/ateam_common/include/ateam_common/topic_names.hpp +++ b/ateam_common/include/ateam_common/topic_names.hpp @@ -49,6 +49,8 @@ constexpr std::string_view kJoystickControlStatus = "/joystick_control_status"; // Output from AI constexpr std::string_view kRobotMotionCommandPrefix = "/robot_motion_commands/robot"; + +constexpr std::string_view kWorld = "/world"; } // namespace Topics #endif // ATEAM_COMMON__TOPIC_NAMES_HPP_ diff --git a/state_tracking/ateam_game_state/CMakeLists.txt b/state_tracking/ateam_game_state/CMakeLists.txt new file mode 100644 index 000000000..7598545db --- /dev/null +++ b/state_tracking/ateam_game_state/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.8) +project(ateam_game_state) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(ateam_common REQUIRED) +find_package(ateam_geometry REQUIRED) +find_package(ateam_msgs REQUIRED) +find_package(ssl_league_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/type_adapters.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_20) +target_compile_options(${PROJECT_NAME} PRIVATE -Werror -Wall -Wextra -Wpedantic) +ament_target_dependencies( + ${PROJECT_NAME} + ateam_common + ateam_geometry + ateam_msgs + rclcpp + tf2 + tf2_geometry_msgs +) + + +add_library(game_state_tracker_component SHARED + src/game_state_tracker/game_state_tracker.cpp +) +target_include_directories(game_state_tracker_component PUBLIC + $ +) +target_compile_features(game_state_tracker_component PUBLIC cxx_std_20) +target_compile_options(game_state_tracker_component PRIVATE -Werror -Wall -Wextra -Wpedantic) +ament_target_dependencies( + game_state_tracker_component + rclcpp + rclcpp_components + ateam_common + ateam_geometry + ateam_msgs + ssl_league_msgs +) +target_link_libraries(game_state_tracker_component ${PROJECT_NAME}) +rclcpp_components_register_node( + game_state_tracker_component + PLUGIN "ateam_game_state::GameStateTracker" + EXECUTABLE game_state_tracker_node +) + +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) +install(TARGETS game_state_tracker_component DESTINATION lib) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/state_tracking/ateam_game_state/LICENSE b/state_tracking/ateam_game_state/LICENSE new file mode 100644 index 000000000..30e8e2ece --- /dev/null +++ b/state_tracking/ateam_game_state/LICENSE @@ -0,0 +1,17 @@ +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. diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp new file mode 100644 index 000000000..9a1516df6 --- /dev/null +++ b/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp @@ -0,0 +1,39 @@ +// 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_GAME_STATE__BALL_HPP_ +#define ATEAM_GAME_STATE__BALL_HPP_ + +#include +#include + +namespace ateam_game_state +{ +struct Ball +{ + ateam_geometry::Point pos; + ateam_geometry::Vector vel; + bool visible = false; + std::chrono::steady_clock::time_point last_visible_time; +}; +} // namespace ateam_kenobi + +#endif // ATEAM_GAME_STATE__BALL_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp new file mode 100644 index 000000000..4a8921fc2 --- /dev/null +++ b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp @@ -0,0 +1,51 @@ +// 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_GAME_STATE__FIELD_HPP_ +#define ATEAM_GAME_STATE__FIELD_HPP_ + +#include +#include + +namespace ateam_game_state +{ +struct FieldSidedInfo +{ + std::array defense_area_corners; + std::array goal_corners; + std::array goal_posts; +}; +struct Field +{ + float field_length = 0.f; + float field_width = 0.f; + float goal_width = 0.f; + float goal_depth = 0.f; + float boundary_width = 0.f; + float defense_area_width = 0.f; + float defense_area_depth = 0.f; + std::array field_corners; + int ignore_side = 0; + FieldSidedInfo ours; + FieldSidedInfo theirs; +}; +} // namespace ateam_kenobi + +#endif // ATEAM_GAME_STATE__FIELD_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp new file mode 100644 index 000000000..c23d3dabd --- /dev/null +++ b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp @@ -0,0 +1,45 @@ +// 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_GAME_STATE__REFEREE_INFO_HPP_ +#define ATEAM_GAME_STATE__REFEREE_INFO_HPP_ + +#include +#include +#include +#include + +namespace ateam_game_state +{ +struct RefereeInfo +{ + int our_goalie_id = -1; + int their_goalie_id = -1; + ateam_common::GameStage current_game_stage = ateam_common::GameStage::Unknown; + ateam_common::GameCommand running_command = ateam_common::GameCommand::Halt; + ateam_common::GameCommand prev_command = ateam_common::GameCommand::Halt; + std::optional designated_position; + std::optional next_command = std::nullopt; + std::chrono::system_clock::time_point command_time; +}; +} // namespace ateam_kenobi + +#endif // ATEAM_GAME_STATE__REFEREE_INFO_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp new file mode 100644 index 000000000..308bad66d --- /dev/null +++ b/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp @@ -0,0 +1,56 @@ +// 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_GAME_STATE__ROBOT_HPP_ +#define ATEAM_GAME_STATE__ROBOT_HPP_ + +#include + +namespace ateam_game_state +{ +struct Robot +{ + int id; + bool visible = false; + bool radio_connected = false; + + ateam_geometry::Point pos; + double theta = 0.0; + ateam_geometry::Vector vel; + double omega = 0.0; + + ateam_geometry::Vector prev_command_vel; + double prev_command_omega = 0.0; + + bool breakbeam_ball_detected = false; + bool breakbeam_ball_detected_filtered = false; + + bool kicker_available = true; + bool chipper_available = false; + + bool IsAvailable() const + { + return visible && radio_connected; + } +}; +} // namespace ateam_kenobi + +#endif // ATEAM_GAME_STATE__ROBOT_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp new file mode 100644 index 000000000..e395efc7b --- /dev/null +++ b/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp @@ -0,0 +1,104 @@ +// 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_GAME_STATE__TYPE_ADAPTERS_HPP_ +#define ATEAM_GAME_STATE__TYPE_ADAPTERS_HPP_ + +#include +#include +#include +#include +#include +#include +#include "world.hpp" +#include "ball.hpp" +#include "robot.hpp" +#include "field.hpp" +#include "referee_info.hpp" + + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = ateam_game_state::World; + using ros_message_type = ateam_msgs::msg::World; + + static void convert_to_ros_message(const custom_type & world, ros_message_type & ros_msg); + + static void convert_to_custom(const ros_message_type & ros_msg, custom_type & world); +}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = ateam_game_state::Ball; + using ros_message_type = ateam_msgs::msg::BallState; + + static void convert_to_ros_message(const custom_type & ball, ros_message_type & ros_msg); + + static void convert_to_custom(const ros_message_type & ros_msg, custom_type & ball); +}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = ateam_game_state::Robot; + using ros_message_type = ateam_msgs::msg::RobotState; + + static void convert_to_ros_message(const custom_type & robot, ros_message_type & ros_msg); + + static void convert_to_custom(const ros_message_type & ros_msg, custom_type & robot); +}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = ateam_game_state::Field; + using ros_message_type = ateam_msgs::msg::FieldInfo; + + static void convert_to_ros_message(const custom_type & field, ros_message_type & ros_msg); + + static void convert_to_custom(const ros_message_type & ros_msg, custom_type & field); +}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = ateam_game_state::RefereeInfo; + using ros_message_type = ateam_msgs::msg::RefereeInfo; + + static void convert_to_ros_message(const custom_type & ref_info, ros_message_type & ros_msg); + + static void convert_to_custom(const ros_message_type & ros_msg, custom_type & ref_info); +}; + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(ateam_game_state::World, ateam_msgs::msg::World); +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(ateam_game_state::Ball, ateam_msgs::msg::BallState); +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(ateam_game_state::Robot, ateam_msgs::msg::RobotState); +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(ateam_game_state::Field, ateam_msgs::msg::FieldInfo); +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(ateam_game_state::RefereeInfo, ateam_msgs::msg::RefereeInfo); + +#endif // ATEAM_GAME_STATE__TYPE_ADAPTERS_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/world.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/world.hpp new file mode 100644 index 000000000..d619336d4 --- /dev/null +++ b/state_tracking/ateam_game_state/include/ateam_game_state/world.hpp @@ -0,0 +1,54 @@ +// 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_GAME_STATE__WORLD_HPP_ +#define ATEAM_GAME_STATE__WORLD_HPP_ + +#include +#include +#include + +#include "ball.hpp" +#include "field.hpp" +#include "referee_info.hpp" +#include "robot.hpp" + +namespace ateam_game_state +{ +struct World +{ + std::chrono::steady_clock::time_point current_time; + + Field field; + RefereeInfo referee_info; + + Ball ball; + std::array our_robots; + std::array their_robots; + + bool in_play = false; + + // Holds the ID of the robot not allowed to touch the ball, if any + std::optional double_touch_forbidden_id_; +}; +} // namespace ateam_kenobi + +#endif // ATEAM_GAME_STATE__WORLD_HPP_ diff --git a/state_tracking/ateam_game_state/package.xml b/state_tracking/ateam_game_state/package.xml new file mode 100644 index 000000000..f40b1500b --- /dev/null +++ b/state_tracking/ateam_game_state/package.xml @@ -0,0 +1,27 @@ + + + + ateam_game_state + 0.0.0 + Game state tracking code for the A-Team + Matt Barulic + MIT + + ament_cmake + + rclcpp + rclcpp_components + ateam_common + ateam_geometry + ateam_msgs + ssl_league_msgs + tf2 + tf2_geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp new file mode 100644 index 000000000..82bd1dbc4 --- /dev/null +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -0,0 +1,200 @@ +// 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 +#include +#include "ateam_game_state/world.hpp" +#include "ateam_game_state/type_adapters.hpp" + + +using ateam_common::indexed_topic_helpers::create_indexed_publishers; +using ateam_common::indexed_topic_helpers::create_indexed_subscribers; + +namespace ateam_game_state +{ + +class GameStateTracker : public rclcpp::Node { +public: + explicit GameStateTracker(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : rclcpp::Node("game_state_tracker", options), + gc_listener_(*this) + { + world_pub_ = + create_publisher(std::string(Topics::kWorld), + rclcpp::QoS(1).best_effort().durability_volatile()); + + field_sub_ = create_subscription( + std::string(Topics::kField), + 1, + std::bind(&GameStateTracker::FieldCallback, this, std::placeholders::_1)); + + ball_sub_ = create_subscription( + std::string(Topics::kBall), + 1, + std::bind(&GameStateTracker::BallCallback, this, std::placeholders::_1)); // TODO(barulicm): need to split vision output messages from world rep messages + + create_indexed_subscribers( + blue_bot_subs_, + Topics::kBlueTeamRobotPrefix, + 1, + &GameStateTracker::BlueVisionBotCallback, + this); + + create_indexed_subscribers( + yellow_bot_subs_, + Topics::kYellowTeamRobotPrefix, + 1, + &GameStateTracker::YellowVisionBotCallback, + this); + + timer_ = create_wall_timer(std::chrono::duration(1.0 / + declare_parameter("update_frequency")), + std::bind(&GameStateTracker::TimerCallback, this)); + } + +private: + rclcpp::Publisher::SharedPtr world_pub_; + rclcpp::Subscription::SharedPtr field_sub_; + rclcpp::Subscription::SharedPtr ball_sub_; + std::array::SharedPtr, + ateam_common::indexed_topic_helpers::kRobotCount> blue_bot_subs_; + std::array::SharedPtr, + ateam_common::indexed_topic_helpers::kRobotCount> yellow_bot_subs_; + ateam_common::GameControllerListener gc_listener_; // TODO(barulicm): Move GCListener type to ateam_game_state package + World world_; + rclcpp::TimerBase::SharedPtr timer_; + + void InitializeRobotIds() + { + for(auto i = 0u; i < world_.our_robots.size(); ++i) { + world_.our_robots[i].id = i; + } + for(auto i = 0u; i < world_.their_robots.size(); ++i) { + world_.their_robots[i].id = i; + } + } + + void FieldCallback(const std::unique_ptr & field) + { + world_.field = *field; + } + + void BallCallback(const std::unique_ptr & ball) + { + world_.ball = *ball; + } + + void UpdateBotFromVision(Robot & robot, const ateam_msgs::msg::RobotState::SharedPtr msg) + { + robot.visible = msg->visible; + if (msg->visible) { + robot.pos = ateam_geometry::Point( + msg->pose.position.x, + msg->pose.position.y); + tf2::Quaternion tf2_quat; + tf2::fromMsg(msg->pose.orientation, tf2_quat); + robot.theta = tf2::getYaw(tf2_quat); + robot.vel = ateam_geometry::Vector( + msg->twist.linear.x, + msg->twist.linear.y); + robot.omega = msg->twist.angular.z; + } + } + + void BlueVisionBotCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int id) + { + const auto our_color = gc_listener_.GetTeamColor(); + if(our_color == ateam_common::TeamColor::Unknown) { + return; + } + const auto are_we_blue = our_color == ateam_common::TeamColor::Blue; + auto & robot_state_array = are_we_blue ? world_.our_robots : world_.their_robots; + UpdateBotFromVision(robot_state_array[id], msg); + } + + void YellowVisionBotCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int id) + { + const auto our_color = gc_listener_.GetTeamColor(); + if(our_color == ateam_common::TeamColor::Unknown) { + return; + } + const auto are_we_yellow = our_color == ateam_common::TeamColor::Yellow; + auto & robot_state_array = are_we_yellow ? world_.our_robots : world_.their_robots; + UpdateBotFromVision(robot_state_array[id], msg); + } + + void UpdateRefInfo() + { + world_.referee_info.our_goalie_id = gc_listener_.GetOurGoalieID().value_or(-1); + world_.referee_info.their_goalie_id = gc_listener_.GetTheirGoalieID().value_or(-1); + world_.referee_info.current_game_stage = gc_listener_.GetGameStage(); + world_.referee_info.running_command = gc_listener_.GetGameCommand(); + world_.referee_info.prev_command = gc_listener_.GetPreviousGameCommand(); + + const auto & gc_designated_position = + gc_listener_.GetDesignatedPosition(); + if (gc_designated_position.has_value()) { + if (gc_listener_.GetTeamSide() == ateam_common::TeamSide::PositiveHalf) { + world_.referee_info.designated_position = ateam_geometry::Point( + -gc_designated_position->x, + -gc_designated_position->y); + } else { + world_.referee_info.designated_position = ateam_geometry::Point( + gc_designated_position->x, + gc_designated_position->y); + } + } else { + world_.referee_info.designated_position = std::nullopt; + } + + world_.referee_info.next_command = gc_listener_.GetNextGameCommand(); + + const auto & ref_msg = gc_listener_.GetLatestRefereeMessage(); + world_.referee_info.command_time = + std::chrono::system_clock::time_point( + std::chrono::nanoseconds( + rclcpp::Time(ref_msg.command_timestamp).nanoseconds())); + } + + void TimerCallback() + { + UpdateRefInfo(); + + // TODO(barulicm): Move ateam_kenobi::InPlayEval to this package + // world_.in_play = false; + + // TODO(barulicm): Move ateam_kenobi::DoubleTouchEval to this package + // world_.double_touch_forbidden_id_ = std::nullopt; + + world_pub_->publish(world_); + } +}; + +} // namespace ateam_game_state + +RCLCPP_COMPONENTS_REGISTER_NODE(ateam_game_state::GameStateTracker) diff --git a/state_tracking/ateam_game_state/src/type_adapters.cpp b/state_tracking/ateam_game_state/src/type_adapters.cpp new file mode 100644 index 000000000..9db6e8af8 --- /dev/null +++ b/state_tracking/ateam_game_state/src/type_adapters.cpp @@ -0,0 +1,246 @@ +// 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_game_state/type_adapters.hpp" + +#include +#include +#include + +void rclcpp::TypeAdapter::convert_to_ros_message( + const custom_type & world, ros_message_type & ros_msg) +{ + using FieldTA = rclcpp::adapt_type::as; + using RobotTA = rclcpp::adapt_type::as; + using BallTA = rclcpp::adapt_type::as; + using RefTA = rclcpp::adapt_type::as; + + ros_msg.current_time = + rclcpp::Time( + std::chrono::duration_cast( + world.current_time.time_since_epoch()).count()); + + FieldTA::convert_to_ros_message(world.field, ros_msg.field); + + RefTA::convert_to_ros_message(world.referee_info, ros_msg.referee_info); + + BallTA::convert_to_ros_message(world.ball, ros_msg.balls.emplace_back()); + + ros_msg.our_robots.reserve(world.our_robots.size()); + for (const ateam_game_state::Robot & robot : world.our_robots) { + if (robot.visible || robot.radio_connected) { + RobotTA::convert_to_ros_message(robot, ros_msg.our_robots.emplace_back()); + } else { + ros_msg.our_robots.push_back(ateam_msgs::msg::RobotState()); + } + } + + ros_msg.their_robots.reserve(world.their_robots.size()); + for (const ateam_game_state::Robot & robot : world.their_robots) { + if (robot.visible) { + RobotTA::convert_to_ros_message(robot, ros_msg.their_robots.emplace_back()); + } else { + ros_msg.their_robots.push_back(ateam_msgs::msg::RobotState()); + } + } + + ros_msg.ball_in_play = world.in_play; + ros_msg.double_touch_enforced = world.double_touch_forbidden_id_.has_value(); + ros_msg.double_touch_id = world.double_touch_forbidden_id_.value_or(-1); +} + +void rclcpp::TypeAdapter::convert_to_custom( + const ros_message_type & ros_msg, custom_type & world) +{ + using FieldTA = rclcpp::adapt_type::as; + using RobotTA = rclcpp::adapt_type::as; + using BallTA = rclcpp::adapt_type::as; + using RefTA = rclcpp::adapt_type::as; + + world.current_time = + std::chrono::steady_clock::time_point(std::chrono::seconds(ros_msg.current_time.sec) + + std::chrono::nanoseconds(ros_msg.current_time.nanosec)); + + FieldTA::convert_to_custom(ros_msg.field, world.field); + + RefTA::convert_to_custom(ros_msg.referee_info, world.referee_info); + + BallTA::convert_to_custom(ros_msg.balls.front(), world.ball); + + const auto convert_bot = [](const auto & ros_robot) { + ateam_game_state::Robot robot; + RobotTA::convert_to_custom(ros_robot, robot); + return robot; + }; + + const auto our_robots_count = std::min(ros_msg.our_robots.size(), world.our_robots.size()); + std::transform(ros_msg.our_robots.begin(), ros_msg.our_robots.begin() + our_robots_count, + world.our_robots.begin(), convert_bot); + + const auto their_robots_count = std::min(ros_msg.their_robots.size(), world.their_robots.size()); + std::transform(ros_msg.their_robots.begin(), ros_msg.their_robots.begin() + their_robots_count, + world.their_robots.begin(), convert_bot); + + world.in_play = ros_msg.ball_in_play; + if(ros_msg.double_touch_enforced) { + world.double_touch_forbidden_id_ = ros_msg.double_touch_id; + } +} + +void rclcpp::TypeAdapter::convert_to_ros_message( + const custom_type & ball, ros_message_type & ros_msg) +{ + ros_msg.pose.position.x = ball.pos.x(); + ros_msg.pose.position.y = ball.pos.y(); + ros_msg.twist.linear.x = ball.vel.x(); + ros_msg.twist.linear.y = ball.vel.y(); + ros_msg.visible = ball.visible; +} + +void rclcpp::TypeAdapter::convert_to_custom( + const ros_message_type & ros_msg, custom_type & ball) +{ + ball.pos = ateam_geometry::Point(ros_msg.pose.position.x, ros_msg.pose.position.y); + ball.vel = ateam_geometry::Vector(ros_msg.twist.linear.x, ros_msg.twist.linear.y); + ball.visible = ros_msg.visible; +} + +void rclcpp::TypeAdapter::convert_to_ros_message( + const custom_type & robot, ros_message_type & ros_msg) +{ + ros_msg.pose.position.x = robot.pos.x(); + ros_msg.pose.position.y = robot.pos.y(); + ros_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), robot.theta)); + ros_msg.twist.linear.x = robot.vel.x(); + ros_msg.twist.linear.y = robot.vel.y(); + ros_msg.twist.angular.z = robot.omega; + ros_msg.visible = robot.visible; +} + +void rclcpp::TypeAdapter::convert_to_custom( + const ros_message_type & ros_msg, custom_type & robot) +{ + robot.pos = ateam_geometry::Point(ros_msg.pose.position.x, ros_msg.pose.position.y); + tf2::Quaternion quat; + tf2::fromMsg(ros_msg.pose.orientation, quat); + robot.theta = tf2::getYaw(quat); + robot.vel = ateam_geometry::Vector(ros_msg.twist.linear.x, ros_msg.twist.linear.y); + robot.omega = ros_msg.twist.angular.z; + robot.visible = ros_msg.visible; +} + +void rclcpp::TypeAdapter::convert_to_ros_message( + const custom_type & field, ros_message_type & ros_msg) +{ + ros_msg.field_length = field.field_length; + ros_msg.field_width = field.field_width; + ros_msg.goal_width = field.goal_width; + ros_msg.goal_depth = field.goal_depth; + ros_msg.boundary_width = field.boundary_width; + ros_msg.ignore_side = field.ignore_side; + + auto convert_point_array = [&](auto & in_array, auto out_array_iter) { + std::transform( + in_array.begin(), in_array.end(), out_array_iter, + [&](auto & val)->geometry_msgs::msg::Point32 { + return geometry_msgs::build().x(val.x()).y(val.y()).z(0); + }); + }; + + convert_point_array(field.field_corners, std::back_inserter(ros_msg.field_corners.points)); + convert_point_array( + field.ours.defense_area_corners, + std::back_inserter(ros_msg.ours.defense_area_corners.points)); + convert_point_array( + field.ours.goal_corners, + std::back_inserter(ros_msg.ours.goal_corners.points)); + convert_point_array( + field.theirs.defense_area_corners, + std::back_inserter(ros_msg.theirs.defense_area_corners.points)); + convert_point_array( + field.theirs.goal_corners, + std::back_inserter(ros_msg.theirs.goal_corners.points)); +} + +void rclcpp::TypeAdapter::convert_to_custom( + const ros_message_type & ros_msg, custom_type & field) +{ + field.field_length = ros_msg.field_length; + field.field_width = ros_msg.field_width; + field.goal_width = ros_msg.goal_width; + field.goal_depth = ros_msg.goal_depth; + field.boundary_width = ros_msg.boundary_width; + field.ignore_side = ros_msg.ignore_side; + + auto convert_point_array = [&](auto & in_vector, auto out_array) { + const auto count = std::min(in_vector.size(), out_array.size()); + std::transform( + in_vector.begin(), in_vector.begin() + count, out_array.begin(), + [&](auto & val)->ateam_geometry::Point { + return ateam_geometry::Point(val.x, val.y); + }); + }; + + convert_point_array(ros_msg.field_corners.points, field.field_corners); + convert_point_array(ros_msg.ours.defense_area_corners.points, field.ours.defense_area_corners); + convert_point_array(ros_msg.ours.goal_corners.points, field.ours.goal_corners); + convert_point_array(ros_msg.theirs.defense_area_corners.points, + field.theirs.defense_area_corners); + convert_point_array(ros_msg.theirs.goal_corners.points, field.theirs.goal_corners); +} + +void rclcpp::TypeAdapter::convert_to_ros_message(const custom_type & ref_info, + ros_message_type & ros_msg) +{ + ros_msg.our_goalie_id = ref_info.our_goalie_id; + ros_msg.their_goalie_id = ref_info.their_goalie_id; + ros_msg.game_stage = static_cast(ref_info.current_game_stage); + ros_msg.game_command = static_cast(ref_info.running_command); + ros_msg.prev_command = static_cast(ref_info.prev_command); + + if (ref_info.designated_position.has_value()) { + ros_msg.designated_position.x = ref_info.designated_position->x(); + ros_msg.designated_position.y = ref_info.designated_position->y(); + } else { + ros_msg.designated_position.x = 0.0f; + ros_msg.designated_position.y = 0.0f; + } +} + +void rclcpp::TypeAdapter::convert_to_custom(const ros_message_type & ros_msg, + custom_type & ref_info) +{ + ref_info.our_goalie_id = ros_msg.our_goalie_id; + ref_info.their_goalie_id = ros_msg.their_goalie_id; + + ref_info.current_game_stage = static_cast(ros_msg.game_stage); + ref_info.running_command = static_cast(ros_msg.game_command); + ref_info.prev_command = static_cast(ros_msg.prev_command); + + // TODO(barulicm): add 'has_designated_position' field + ref_info.designated_position = ateam_geometry::Point(ros_msg.designated_position.x, + ros_msg.designated_position.y); +} From a49dec078ff1872e53b8f6c205ab25ba66d32566 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 29 Sep 2025 02:11:25 -0400 Subject: [PATCH 02/19] Moves ateam_field_manager to state_tracking folder. --- .../ateam_field_manager}/CMakeLists.txt | 0 .../ateam_field_manager}/package.xml | 0 .../ateam_field_manager}/src/ateam_field_manager_node.cpp | 0 .../ateam_field_manager}/src/message_conversions.cpp | 0 .../ateam_field_manager}/src/message_conversions.hpp | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename {ateam_field_manager => state_tracking/ateam_field_manager}/CMakeLists.txt (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/package.xml (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/src/ateam_field_manager_node.cpp (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/src/message_conversions.cpp (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/src/message_conversions.hpp (100%) diff --git a/ateam_field_manager/CMakeLists.txt b/state_tracking/ateam_field_manager/CMakeLists.txt similarity index 100% rename from ateam_field_manager/CMakeLists.txt rename to state_tracking/ateam_field_manager/CMakeLists.txt diff --git a/ateam_field_manager/package.xml b/state_tracking/ateam_field_manager/package.xml similarity index 100% rename from ateam_field_manager/package.xml rename to state_tracking/ateam_field_manager/package.xml diff --git a/ateam_field_manager/src/ateam_field_manager_node.cpp b/state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp similarity index 100% rename from ateam_field_manager/src/ateam_field_manager_node.cpp rename to state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp diff --git a/ateam_field_manager/src/message_conversions.cpp b/state_tracking/ateam_field_manager/src/message_conversions.cpp similarity index 100% rename from ateam_field_manager/src/message_conversions.cpp rename to state_tracking/ateam_field_manager/src/message_conversions.cpp diff --git a/ateam_field_manager/src/message_conversions.hpp b/state_tracking/ateam_field_manager/src/message_conversions.hpp similarity index 100% rename from ateam_field_manager/src/message_conversions.hpp rename to state_tracking/ateam_field_manager/src/message_conversions.hpp From 7fd83c7cd83e648e6595fa2c4839b03268d4fa32 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 29 Sep 2025 02:12:48 -0400 Subject: [PATCH 03/19] Moves ateam_vision_filter to state_tracking folder. --- .../ateam_vision_filter}/CMakeLists.txt | 0 .../ateam_vision_filter}/package.xml | 0 .../ateam_vision_filter}/scripts/calibrate_field.py | 0 .../ateam_vision_filter}/src/ateam_vision_filter_node.cpp | 0 .../ateam_vision_filter}/src/camera.cpp | 0 .../ateam_vision_filter}/src/camera.hpp | 0 .../src/filters/interacting_multiple_model_filter.cpp | 0 .../src/filters/interacting_multiple_model_filter.hpp | 0 .../ateam_vision_filter}/src/filters/kalman_filter.cpp | 0 .../ateam_vision_filter}/src/filters/kalman_filter.hpp | 0 .../src/filters/multiple_hypothesis_tracker.cpp | 0 .../src/filters/multiple_hypothesis_tracker.hpp | 0 .../ateam_vision_filter}/src/generators/generator_util.cpp | 0 .../ateam_vision_filter}/src/generators/generator_util.hpp | 0 .../ateam_vision_filter}/src/generators/model_input_generator.cpp | 0 .../ateam_vision_filter}/src/generators/model_input_generator.hpp | 0 .../src/generators/transmission_probability_generator.cpp | 0 .../src/generators/transmission_probability_generator.hpp | 0 .../ateam_vision_filter}/src/message_conversions.cpp | 0 .../ateam_vision_filter}/src/message_conversions.hpp | 0 .../ateam_vision_filter}/src/types/ball.hpp | 0 .../ateam_vision_filter}/src/types/ball_measurement.hpp | 0 .../ateam_vision_filter}/src/types/camera_measurement.hpp | 0 .../ateam_vision_filter}/src/types/models.hpp | 0 .../ateam_vision_filter}/src/types/robot.hpp | 0 .../ateam_vision_filter}/src/types/robot_measurement.hpp | 0 .../ateam_vision_filter}/src/world.cpp | 0 .../ateam_vision_filter}/src/world.hpp | 0 .../ateam_vision_filter}/test/camera_test.cpp | 0 .../test/filters/interacting_multiple_model_filter_test.cpp | 0 .../ateam_vision_filter}/test/filters/kalman_filter_test.cpp | 0 .../test/filters/multiple_hypothesis_tracker_test.cpp | 0 .../test/generators/model_input_generator_test.cpp | 0 .../test/generators/transmission_probability_generator_test.cpp | 0 .../ateam_vision_filter}/test/world_test.cpp | 0 35 files changed, 0 insertions(+), 0 deletions(-) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/CMakeLists.txt (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/package.xml (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/scripts/calibrate_field.py (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/ateam_vision_filter_node.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/camera.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/camera.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/interacting_multiple_model_filter.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/interacting_multiple_model_filter.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/kalman_filter.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/kalman_filter.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/multiple_hypothesis_tracker.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/multiple_hypothesis_tracker.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/generator_util.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/generator_util.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/model_input_generator.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/model_input_generator.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/transmission_probability_generator.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/transmission_probability_generator.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/message_conversions.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/message_conversions.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/ball.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/ball_measurement.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/camera_measurement.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/models.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/robot.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/robot_measurement.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/world.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/world.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/camera_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/filters/interacting_multiple_model_filter_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/filters/kalman_filter_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/filters/multiple_hypothesis_tracker_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/generators/model_input_generator_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/generators/transmission_probability_generator_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/world_test.cpp (100%) diff --git a/ateam_vision_filter/CMakeLists.txt b/state_tracking/ateam_vision_filter/CMakeLists.txt similarity index 100% rename from ateam_vision_filter/CMakeLists.txt rename to state_tracking/ateam_vision_filter/CMakeLists.txt diff --git a/ateam_vision_filter/package.xml b/state_tracking/ateam_vision_filter/package.xml similarity index 100% rename from ateam_vision_filter/package.xml rename to state_tracking/ateam_vision_filter/package.xml diff --git a/ateam_vision_filter/scripts/calibrate_field.py b/state_tracking/ateam_vision_filter/scripts/calibrate_field.py similarity index 100% rename from ateam_vision_filter/scripts/calibrate_field.py rename to state_tracking/ateam_vision_filter/scripts/calibrate_field.py diff --git a/ateam_vision_filter/src/ateam_vision_filter_node.cpp b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp similarity index 100% rename from ateam_vision_filter/src/ateam_vision_filter_node.cpp rename to state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp diff --git a/ateam_vision_filter/src/camera.cpp b/state_tracking/ateam_vision_filter/src/camera.cpp similarity index 100% rename from ateam_vision_filter/src/camera.cpp rename to state_tracking/ateam_vision_filter/src/camera.cpp diff --git a/ateam_vision_filter/src/camera.hpp b/state_tracking/ateam_vision_filter/src/camera.hpp similarity index 100% rename from ateam_vision_filter/src/camera.hpp rename to state_tracking/ateam_vision_filter/src/camera.hpp diff --git a/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp similarity index 100% rename from ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp rename to state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp diff --git a/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp similarity index 100% rename from ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp rename to state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp diff --git a/ateam_vision_filter/src/filters/kalman_filter.cpp b/state_tracking/ateam_vision_filter/src/filters/kalman_filter.cpp similarity index 100% rename from ateam_vision_filter/src/filters/kalman_filter.cpp rename to state_tracking/ateam_vision_filter/src/filters/kalman_filter.cpp diff --git a/ateam_vision_filter/src/filters/kalman_filter.hpp b/state_tracking/ateam_vision_filter/src/filters/kalman_filter.hpp similarity index 100% rename from ateam_vision_filter/src/filters/kalman_filter.hpp rename to state_tracking/ateam_vision_filter/src/filters/kalman_filter.hpp diff --git a/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp similarity index 100% rename from ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp rename to state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp diff --git a/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp similarity index 100% rename from ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp rename to state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp diff --git a/ateam_vision_filter/src/generators/generator_util.cpp b/state_tracking/ateam_vision_filter/src/generators/generator_util.cpp similarity index 100% rename from ateam_vision_filter/src/generators/generator_util.cpp rename to state_tracking/ateam_vision_filter/src/generators/generator_util.cpp diff --git a/ateam_vision_filter/src/generators/generator_util.hpp b/state_tracking/ateam_vision_filter/src/generators/generator_util.hpp similarity index 100% rename from ateam_vision_filter/src/generators/generator_util.hpp rename to state_tracking/ateam_vision_filter/src/generators/generator_util.hpp diff --git a/ateam_vision_filter/src/generators/model_input_generator.cpp b/state_tracking/ateam_vision_filter/src/generators/model_input_generator.cpp similarity index 100% rename from ateam_vision_filter/src/generators/model_input_generator.cpp rename to state_tracking/ateam_vision_filter/src/generators/model_input_generator.cpp diff --git a/ateam_vision_filter/src/generators/model_input_generator.hpp b/state_tracking/ateam_vision_filter/src/generators/model_input_generator.hpp similarity index 100% rename from ateam_vision_filter/src/generators/model_input_generator.hpp rename to state_tracking/ateam_vision_filter/src/generators/model_input_generator.hpp diff --git a/ateam_vision_filter/src/generators/transmission_probability_generator.cpp b/state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.cpp similarity index 100% rename from ateam_vision_filter/src/generators/transmission_probability_generator.cpp rename to state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.cpp diff --git a/ateam_vision_filter/src/generators/transmission_probability_generator.hpp b/state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.hpp similarity index 100% rename from ateam_vision_filter/src/generators/transmission_probability_generator.hpp rename to state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.hpp diff --git a/ateam_vision_filter/src/message_conversions.cpp b/state_tracking/ateam_vision_filter/src/message_conversions.cpp similarity index 100% rename from ateam_vision_filter/src/message_conversions.cpp rename to state_tracking/ateam_vision_filter/src/message_conversions.cpp diff --git a/ateam_vision_filter/src/message_conversions.hpp b/state_tracking/ateam_vision_filter/src/message_conversions.hpp similarity index 100% rename from ateam_vision_filter/src/message_conversions.hpp rename to state_tracking/ateam_vision_filter/src/message_conversions.hpp diff --git a/ateam_vision_filter/src/types/ball.hpp b/state_tracking/ateam_vision_filter/src/types/ball.hpp similarity index 100% rename from ateam_vision_filter/src/types/ball.hpp rename to state_tracking/ateam_vision_filter/src/types/ball.hpp diff --git a/ateam_vision_filter/src/types/ball_measurement.hpp b/state_tracking/ateam_vision_filter/src/types/ball_measurement.hpp similarity index 100% rename from ateam_vision_filter/src/types/ball_measurement.hpp rename to state_tracking/ateam_vision_filter/src/types/ball_measurement.hpp diff --git a/ateam_vision_filter/src/types/camera_measurement.hpp b/state_tracking/ateam_vision_filter/src/types/camera_measurement.hpp similarity index 100% rename from ateam_vision_filter/src/types/camera_measurement.hpp rename to state_tracking/ateam_vision_filter/src/types/camera_measurement.hpp diff --git a/ateam_vision_filter/src/types/models.hpp b/state_tracking/ateam_vision_filter/src/types/models.hpp similarity index 100% rename from ateam_vision_filter/src/types/models.hpp rename to state_tracking/ateam_vision_filter/src/types/models.hpp diff --git a/ateam_vision_filter/src/types/robot.hpp b/state_tracking/ateam_vision_filter/src/types/robot.hpp similarity index 100% rename from ateam_vision_filter/src/types/robot.hpp rename to state_tracking/ateam_vision_filter/src/types/robot.hpp diff --git a/ateam_vision_filter/src/types/robot_measurement.hpp b/state_tracking/ateam_vision_filter/src/types/robot_measurement.hpp similarity index 100% rename from ateam_vision_filter/src/types/robot_measurement.hpp rename to state_tracking/ateam_vision_filter/src/types/robot_measurement.hpp diff --git a/ateam_vision_filter/src/world.cpp b/state_tracking/ateam_vision_filter/src/world.cpp similarity index 100% rename from ateam_vision_filter/src/world.cpp rename to state_tracking/ateam_vision_filter/src/world.cpp diff --git a/ateam_vision_filter/src/world.hpp b/state_tracking/ateam_vision_filter/src/world.hpp similarity index 100% rename from ateam_vision_filter/src/world.hpp rename to state_tracking/ateam_vision_filter/src/world.hpp diff --git a/ateam_vision_filter/test/camera_test.cpp b/state_tracking/ateam_vision_filter/test/camera_test.cpp similarity index 100% rename from ateam_vision_filter/test/camera_test.cpp rename to state_tracking/ateam_vision_filter/test/camera_test.cpp diff --git a/ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp b/state_tracking/ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp similarity index 100% rename from ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp rename to state_tracking/ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp diff --git a/ateam_vision_filter/test/filters/kalman_filter_test.cpp b/state_tracking/ateam_vision_filter/test/filters/kalman_filter_test.cpp similarity index 100% rename from ateam_vision_filter/test/filters/kalman_filter_test.cpp rename to state_tracking/ateam_vision_filter/test/filters/kalman_filter_test.cpp diff --git a/ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp b/state_tracking/ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp similarity index 100% rename from ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp rename to state_tracking/ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp diff --git a/ateam_vision_filter/test/generators/model_input_generator_test.cpp b/state_tracking/ateam_vision_filter/test/generators/model_input_generator_test.cpp similarity index 100% rename from ateam_vision_filter/test/generators/model_input_generator_test.cpp rename to state_tracking/ateam_vision_filter/test/generators/model_input_generator_test.cpp diff --git a/ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp b/state_tracking/ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp similarity index 100% rename from ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp rename to state_tracking/ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp diff --git a/ateam_vision_filter/test/world_test.cpp b/state_tracking/ateam_vision_filter/test/world_test.cpp similarity index 100% rename from ateam_vision_filter/test/world_test.cpp rename to state_tracking/ateam_vision_filter/test/world_test.cpp From a132ddbc5e58101949208363c65c8ed83904acf7 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 29 Sep 2025 23:59:17 -0400 Subject: [PATCH 04/19] Copies in play and double touch evaluators to game_state_tracker. --- .../ateam_game_state/CMakeLists.txt | 2 + .../double_touch_evaluator.cpp | 114 +++++++++++++ .../double_touch_evaluator.hpp | 54 ++++++ .../game_state_tracker/game_state_tracker.cpp | 13 +- .../game_state_tracker/in_play_evaluator.cpp | 156 ++++++++++++++++++ .../game_state_tracker/in_play_evaluator.hpp | 64 +++++++ 6 files changed, 396 insertions(+), 7 deletions(-) create mode 100644 state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.cpp create mode 100644 state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp create mode 100644 state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp create mode 100644 state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp diff --git a/state_tracking/ateam_game_state/CMakeLists.txt b/state_tracking/ateam_game_state/CMakeLists.txt index 7598545db..c9f950b2a 100644 --- a/state_tracking/ateam_game_state/CMakeLists.txt +++ b/state_tracking/ateam_game_state/CMakeLists.txt @@ -33,6 +33,8 @@ ament_target_dependencies( add_library(game_state_tracker_component SHARED src/game_state_tracker/game_state_tracker.cpp + src/game_state_tracker/double_touch_evaluator.cpp + src/game_state_tracker/in_play_evaluator.cpp ) target_include_directories(game_state_tracker_component PUBLIC $ diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.cpp new file mode 100644 index 000000000..c3244a01c --- /dev/null +++ b/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.cpp @@ -0,0 +1,114 @@ +// 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 "double_touch_evaluator.hpp" +#include + +namespace ateam_game_state +{ + +void DoubleTouchEvaluator::Update(World & world) +{ + const auto running_command = world.referee_info.running_command; + BOOST_SCOPE_EXIT(&running_command, this_) { + this_->prev_game_command_ = running_command; + } BOOST_SCOPE_EXIT_END + + if (running_command != ateam_common::GameCommand::NormalStart && + running_command != ateam_common::GameCommand::PrepareKickoffOurs && + running_command != ateam_common::GameCommand::DirectFreeOurs) + { + // Game is in a state where the double-touch rule does not apply. + double_touch_rule_applies_ = false; + } + + if (running_command != prev_game_command_ && + (running_command == ateam_common::GameCommand::PrepareKickoffOurs || + running_command == ateam_common::GameCommand::DirectFreeOurs)) + { + // Entering state where double-touch rule applies + forbidden_id_.reset(); + prev_touching_id_.reset(); + double_touch_rule_applies_ = true; + } + + if (!double_touch_rule_applies_) { + world.double_touch_forbidden_id_.reset(); + return; + } + + const auto maybe_toucher = GetRobotTouchingBall(world); + + if (!prev_touching_id_ && maybe_toucher) { + // A robot has started touching the ball + const auto toucher_id = maybe_toucher.value().id; + if (forbidden_id_ && toucher_id != forbidden_id_.value()) { + // This is the second robot to touch the ball, double-touch hold is released + forbidden_id_.reset(); + double_touch_rule_applies_ = false; + world.double_touch_forbidden_id_.reset(); + return; + } + } + + if (prev_touching_id_ && (!maybe_toucher || world.in_play)) { + // A robot has stopped touching the ball, or moved the ball enough to put it in play + if (!forbidden_id_) { + // This was the first robot to touch the ball, it can't touch it again + forbidden_id_ = prev_touching_id_; + } + } + + world.double_touch_forbidden_id_ = forbidden_id_; + + if (!maybe_toucher) { + prev_touching_id_.reset(); + } else { + prev_touching_id_ = maybe_toucher.value().id; + } +} + +std::optional DoubleTouchEvaluator::GetRobotTouchingBall(const World & world) +{ + auto found_iter = std::ranges::find_if( + world.our_robots, [this, &world](const Robot & robot) { + if (!robot.visible) { + return false; + } + const auto ball_bot_distance = std::sqrt( + CGAL::squared_distance(robot.pos, world.ball.pos)); + if (robot.breakbeam_ball_detected_filtered && + ball_bot_distance <= kStartTouchBallsenseThreshold) + { + return true; + } + if (prev_touching_id_.value_or(-1) == robot.id) { + return ball_bot_distance <= kEndTouchVisionThreshold; + } else { + return ball_bot_distance <= kStartTouchVisionThreshold; + } + }); + if (found_iter == world.our_robots.end()) { + return {}; + } + return *found_iter; +} + +} // namespace ateam_game_state diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp b/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp new file mode 100644 index 000000000..18071b8aa --- /dev/null +++ b/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp @@ -0,0 +1,54 @@ +// 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 GAME_STATE_TRACKER__DOUBLE_TOUCH_EVAL_HPP_ +#define GAME_STATE_TRACKER__DOUBLE_TOUCH_EVAL_HPP_ + +#include +#include +#include +#include +#include "ateam_game_state/world.hpp" + +namespace ateam_game_state +{ + +class DoubleTouchEvaluator +{ +public: + void Update(World & world); + +private: + static constexpr double kStartTouchBallsenseThreshold = kRobotRadius + 0.1; + static constexpr double kStartTouchVisionThreshold = kRobotRadius + kBallRadius + 0.01; + static constexpr double kEndTouchVisionThreshold = kRobotRadius + kBallRadius + 0.045; + bool double_touch_rule_applies_ = false; + std::optional forbidden_id_; + // tracking the frame when command changes, different from RefereeInfo.prev_command + ateam_common::GameCommand prev_game_command_{ateam_common::GameCommand::Halt}; + std::optional prev_touching_id_; + + std::optional GetRobotTouchingBall(const World & world); +}; + +} // namespace ateam_game_state + +#endif // GAME_STATE_TRACKER__DOUBLE_TOUCH_EVAL_HPP_ diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index 82bd1dbc4..c83acae79 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -30,6 +30,8 @@ #include #include "ateam_game_state/world.hpp" #include "ateam_game_state/type_adapters.hpp" +#include "double_touch_evaluator.hpp" +#include "in_play_evaluator.hpp" using ateam_common::indexed_topic_helpers::create_indexed_publishers; @@ -78,6 +80,8 @@ class GameStateTracker : public rclcpp::Node { } private: + DoubleTouchEvaluator double_touch_evaluator_; + InPlayEvaluator in_play_evaluator_; rclcpp::Publisher::SharedPtr world_pub_; rclcpp::Subscription::SharedPtr field_sub_; rclcpp::Subscription::SharedPtr ball_sub_; @@ -184,13 +188,8 @@ class GameStateTracker : public rclcpp::Node { void TimerCallback() { UpdateRefInfo(); - - // TODO(barulicm): Move ateam_kenobi::InPlayEval to this package - // world_.in_play = false; - - // TODO(barulicm): Move ateam_kenobi::DoubleTouchEval to this package - // world_.double_touch_forbidden_id_ = std::nullopt; - + double_touch_evaluator_.Update(world_); + in_play_evaluator_.Update(world_); world_pub_->publish(world_); } }; diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp new file mode 100644 index 000000000..4ff712db7 --- /dev/null +++ b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp @@ -0,0 +1,156 @@ +// 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 "in_play_evaluator.hpp" + +#include +#include +#include + +namespace ateam_game_state +{ + +void InPlayEvaluator::Update(World & world) +{ + const auto game_command = world.referee_info.running_command; + if (game_command == ateam_common::GameCommand::ForceStart) { + in_play_ = true; + } + if (IsGameStopping(world)) { + in_play_ = false; + ball_start_pos_.reset(); + timeout_duration_.reset(); + timeout_start_ = std::chrono::steady_clock::time_point::max(); + } + if (IsStopCommandEnding(world)) { + SetTimeout(world); + SetDistanceThreshold(world); + } + if (IsGameResuming(world)) { + ball_start_pos_ = world.ball.pos; + timeout_start_ = std::chrono::steady_clock::now(); + } + if (!in_play_ && HasTimeoutExpired()) { + in_play_ = true; + } + if (!in_play_ && HasBallMoved(world)) { + in_play_ = true; + } + + /* If none of the above conditions have hit, our in play status hasn't changed and we keep the + * existing value. + */ + + world.in_play = in_play_; + prev_game_command_ = game_command; +} + +void InPlayEvaluator::SetTimeout(const World & world) +{ + switch (world.referee_info.running_command) { + case ateam_common::GameCommand::PrepareKickoffOurs: + case ateam_common::GameCommand::PrepareKickoffTheirs: + timeout_duration_ = std::chrono::seconds(10); + break; + case ateam_common::GameCommand::DirectFreeOurs: + case ateam_common::GameCommand::DirectFreeTheirs: + // Division A + // timeout_duration_ = std::chrono::seconds(5); + // Division B + timeout_duration_ = std::chrono::seconds(10); + break; + default: + timeout_duration_.reset(); + break; + } +} + +void InPlayEvaluator::SetDistanceThreshold(const World & world) +{ + switch (world.referee_info.running_command) { + case ateam_common::GameCommand::PrepareKickoffOurs: + case ateam_common::GameCommand::PrepareKickoffTheirs: + case ateam_common::GameCommand::DirectFreeOurs: + case ateam_common::GameCommand::DirectFreeTheirs: + case ateam_common::GameCommand::PreparePenaltyOurs: + case ateam_common::GameCommand::PreparePenaltyTheirs: + ball_moved_threshold_ = 0.05; + break; + default: + ball_moved_threshold_ = std::numeric_limits::infinity(); + break; + } +} + + +bool InPlayEvaluator::IsGameStopping(const World & world) +{ + if (world.referee_info.running_command == prev_game_command_) { + return false; + } + return world.referee_info.running_command == ateam_common::GameCommand::Stop || + world.referee_info.running_command == ateam_common::GameCommand::Halt; +} + +bool InPlayEvaluator::IsStopCommandEnding(const World & world) +{ + return world.referee_info.running_command != prev_game_command_ && + prev_game_command_ == ateam_common::GameCommand::Stop; +} + + +bool InPlayEvaluator::IsGameResuming(const World & world) +{ + if (world.referee_info.running_command == prev_game_command_) { + return false; + } + + if (world.referee_info.running_command == ateam_common::GameCommand::NormalStart || + world.referee_info.running_command == ateam_common::GameCommand::DirectFreeOurs || + world.referee_info.running_command == ateam_common::GameCommand::DirectFreeTheirs) + { + return true; + } + + return false; +} + +bool InPlayEvaluator::HasBallMoved(const World & world) +{ + if (!ball_start_pos_) { + return false; + } + + return CGAL::approximate_sqrt( + CGAL::squared_distance( + world.ball.pos, + ball_start_pos_.value())) > ball_moved_threshold_; +} + +bool InPlayEvaluator::HasTimeoutExpired() +{ + if (!timeout_duration_) { + return false; + } + + return (std::chrono::steady_clock::now() - timeout_duration_.value()) > timeout_start_; +} + +} // namespace ateam_kenobi diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp new file mode 100644 index 000000000..e286a29f1 --- /dev/null +++ b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp @@ -0,0 +1,64 @@ +// 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 GAME_STATE_TRACKER__IN_PLAY_EVAL_HPP_ +#define GAME_STATE_TRACKER__IN_PLAY_EVAL_HPP_ + +#include +#include +#include +#include +#include "ateam_game_state/world.hpp" + +namespace ateam_game_state +{ + +class InPlayEvaluator +{ +public: + void Update(World & world); + +private: + bool in_play_ = false; + std::optional ball_start_pos_; + ateam_common::GameCommand prev_game_command_; + std::optional timeout_duration_; + std::chrono::steady_clock::time_point timeout_start_; + double ball_moved_threshold_ = 0; + + void SetTimeout(const World & world); + + void SetDistanceThreshold(const World & world); + + bool IsGameStopping(const World & world); + + bool IsStopCommandEnding(const World & world); + + bool IsGameResuming(const World & world); + + bool HasBallMoved(const World & world); + + bool HasTimeoutExpired(); +}; + +} // namespace ateam_game_state + +#endif // GAME_STATE_TRACKER__IN_PLAY_EVAL_HPP_ From 7cdd9d7bbfd9b6260951fd62a67b71bfb681a007 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:17:15 -0400 Subject: [PATCH 05/19] Splits vision/game state messages and removes unused message types. --- ateam_msgs/CMakeLists.txt | 24 ++++++++++--------- ateam_msgs/msg/BallState.msg | 8 ------- ateam_msgs/msg/BehaviorExecutorState.msg | 1 - ateam_msgs/msg/GameStateBall.msg | 4 ++++ ateam_msgs/msg/GameStateRobot.msg | 6 +++++ ateam_msgs/msg/GameStateWorld.msg | 12 ++++++++++ ateam_msgs/msg/KenobiStatus.msg | 1 + ateam_msgs/msg/RobotState.msg | 10 -------- ateam_msgs/msg/Sample3d.msg | 7 ------ ateam_msgs/msg/Trajectory.msg | 1 - ateam_msgs/msg/VisionCameraState.msg | 4 ---- ateam_msgs/msg/VisionIMMState.msg | 2 -- ateam_msgs/msg/VisionMHTState.msg | 1 - ateam_msgs/msg/VisionStateBall.msg | 4 ++++ ateam_msgs/msg/VisionStateCamera.msg | 4 ++++ ateam_msgs/msg/VisionStateCameraArray.msg | 1 + ateam_msgs/msg/VisionStateIMM.msg | 2 ++ ateam_msgs/msg/VisionStateMHT.msg | 1 + ...ionModelState.msg => VisionStateModel.msg} | 2 +- ateam_msgs/msg/VisionStateRobot.msg | 5 ++++ ateam_msgs/msg/VisionWorldState.msg | 1 - ateam_msgs/msg/World.msg | 18 -------------- 22 files changed, 54 insertions(+), 65 deletions(-) delete mode 100644 ateam_msgs/msg/BallState.msg delete mode 100644 ateam_msgs/msg/BehaviorExecutorState.msg create mode 100644 ateam_msgs/msg/GameStateBall.msg create mode 100644 ateam_msgs/msg/GameStateRobot.msg create mode 100644 ateam_msgs/msg/GameStateWorld.msg create mode 100644 ateam_msgs/msg/KenobiStatus.msg delete mode 100644 ateam_msgs/msg/RobotState.msg delete mode 100644 ateam_msgs/msg/Sample3d.msg delete mode 100644 ateam_msgs/msg/Trajectory.msg delete mode 100644 ateam_msgs/msg/VisionCameraState.msg delete mode 100644 ateam_msgs/msg/VisionIMMState.msg delete mode 100644 ateam_msgs/msg/VisionMHTState.msg create mode 100644 ateam_msgs/msg/VisionStateBall.msg create mode 100644 ateam_msgs/msg/VisionStateCamera.msg create mode 100644 ateam_msgs/msg/VisionStateCameraArray.msg create mode 100644 ateam_msgs/msg/VisionStateIMM.msg create mode 100644 ateam_msgs/msg/VisionStateMHT.msg rename ateam_msgs/msg/{VisionModelState.msg => VisionStateModel.msg} (60%) create mode 100644 ateam_msgs/msg/VisionStateRobot.msg delete mode 100644 ateam_msgs/msg/VisionWorldState.msg delete mode 100644 ateam_msgs/msg/World.msg diff --git a/ateam_msgs/CMakeLists.txt b/ateam_msgs/CMakeLists.txt index b9da1e8d9..139cc0d92 100755 --- a/ateam_msgs/CMakeLists.txt +++ b/ateam_msgs/CMakeLists.txt @@ -10,23 +10,25 @@ find_package(sensor_msgs REQUIRED) find_package(ssl_league_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - msg/BallState.msg - msg/BehaviorExecutorState.msg msg/JoystickControlStatus.msg + msg/KenobiStatus.msg msg/Overlay.msg msg/OverlayArray.msg msg/RefereeInfo.msg msg/RobotMotionCommand.msg - msg/RobotState.msg - msg/Sample3d.msg msg/TeamClientConnectionStatus.msg - msg/Trajectory.msg - msg/VisionCameraState.msg - msg/VisionIMMState.msg - msg/VisionMHTState.msg - msg/VisionModelState.msg - msg/VisionWorldState.msg - msg/World.msg + + msg/GameStateBall.msg + msg/GameStateRobot.msg + msg/GameStateWorld.msg + + msg/VisionStateBall.msg + msg/VisionStateCamera.msg + msg/VisionStateCameraArray.msg + msg/VisionStateIMM.msg + msg/VisionStateMHT.msg + msg/VisionStateModel.msg + msg/VisionStateRobot.msg msg/FieldInfo.msg msg/FieldSidedInfo.msg diff --git a/ateam_msgs/msg/BallState.msg b/ateam_msgs/msg/BallState.msg deleted file mode 100644 index 12bc9cd82..000000000 --- a/ateam_msgs/msg/BallState.msg +++ /dev/null @@ -1,8 +0,0 @@ -# XY in linear -# Velocity/Accel in field frame -# All units in meters and seconds -geometry_msgs/Pose pose -geometry_msgs/Twist twist -geometry_msgs/Accel accel - -bool visible \ No newline at end of file diff --git a/ateam_msgs/msg/BehaviorExecutorState.msg b/ateam_msgs/msg/BehaviorExecutorState.msg deleted file mode 100644 index 4635ed68e..000000000 --- a/ateam_msgs/msg/BehaviorExecutorState.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/Trajectory[] previous_trajectories \ No newline at end of file diff --git a/ateam_msgs/msg/GameStateBall.msg b/ateam_msgs/msg/GameStateBall.msg new file mode 100644 index 000000000..8fb831044 --- /dev/null +++ b/ateam_msgs/msg/GameStateBall.msg @@ -0,0 +1,4 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Accel accel +bool visible diff --git a/ateam_msgs/msg/GameStateRobot.msg b/ateam_msgs/msg/GameStateRobot.msg new file mode 100644 index 000000000..839f1ed76 --- /dev/null +++ b/ateam_msgs/msg/GameStateRobot.msg @@ -0,0 +1,6 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Twist twist_body +geometry_msgs/Accel accel +bool visible +bool connected diff --git a/ateam_msgs/msg/GameStateWorld.msg b/ateam_msgs/msg/GameStateWorld.msg new file mode 100644 index 000000000..88a3ed4ff --- /dev/null +++ b/ateam_msgs/msg/GameStateWorld.msg @@ -0,0 +1,12 @@ +builtin_interfaces/Time current_time + +FieldInfo field +RefereeInfo referee_info + +GameStateBall ball +GameStateRobot[] our_robots +GameStateRobot[] their_robots + +bool ball_in_play +bool double_touch_enforced +int32 double_touch_id diff --git a/ateam_msgs/msg/KenobiStatus.msg b/ateam_msgs/msg/KenobiStatus.msg new file mode 100644 index 000000000..0ffd5d67f --- /dev/null +++ b/ateam_msgs/msg/KenobiStatus.msg @@ -0,0 +1 @@ +float64 fps diff --git a/ateam_msgs/msg/RobotState.msg b/ateam_msgs/msg/RobotState.msg deleted file mode 100644 index cc1b00090..000000000 --- a/ateam_msgs/msg/RobotState.msg +++ /dev/null @@ -1,10 +0,0 @@ -# XY in linear, Z in angular -# Velocity/Accel in field frame -# All units in meters and seconds -geometry_msgs/Pose pose -geometry_msgs/Twist twist -geometry_msgs/Accel accel - -geometry_msgs/Twist twist_body - -bool visible \ No newline at end of file diff --git a/ateam_msgs/msg/Sample3d.msg b/ateam_msgs/msg/Sample3d.msg deleted file mode 100644 index 7680226e9..000000000 --- a/ateam_msgs/msg/Sample3d.msg +++ /dev/null @@ -1,7 +0,0 @@ -# XY in linear, Z in angular -# Velocity/Accel in field frame -# All units in meters and seconds -builtin_interfaces/Time time -geometry_msgs/Pose pose -geometry_msgs/Twist twist -geometry_msgs/Accel accel \ No newline at end of file diff --git a/ateam_msgs/msg/Trajectory.msg b/ateam_msgs/msg/Trajectory.msg deleted file mode 100644 index 15edeb3a9..000000000 --- a/ateam_msgs/msg/Trajectory.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/Sample3d[] samples \ No newline at end of file diff --git a/ateam_msgs/msg/VisionCameraState.msg b/ateam_msgs/msg/VisionCameraState.msg deleted file mode 100644 index 0e1dcab25..000000000 --- a/ateam_msgs/msg/VisionCameraState.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint8 camera_id -ateam_msgs/VisionMHTState[] yellow_robots -ateam_msgs/VisionMHTState[] blue_robots -ateam_msgs/VisionMHTState ball \ No newline at end of file diff --git a/ateam_msgs/msg/VisionIMMState.msg b/ateam_msgs/msg/VisionIMMState.msg deleted file mode 100644 index a16992b39..000000000 --- a/ateam_msgs/msg/VisionIMMState.msg +++ /dev/null @@ -1,2 +0,0 @@ -ateam_msgs/VisionModelState[] models -uint32 updates_until_valid_track \ No newline at end of file diff --git a/ateam_msgs/msg/VisionMHTState.msg b/ateam_msgs/msg/VisionMHTState.msg deleted file mode 100644 index 325e719cb..000000000 --- a/ateam_msgs/msg/VisionMHTState.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/VisionIMMState[] tracks \ No newline at end of file diff --git a/ateam_msgs/msg/VisionStateBall.msg b/ateam_msgs/msg/VisionStateBall.msg new file mode 100644 index 000000000..8fb831044 --- /dev/null +++ b/ateam_msgs/msg/VisionStateBall.msg @@ -0,0 +1,4 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Accel accel +bool visible diff --git a/ateam_msgs/msg/VisionStateCamera.msg b/ateam_msgs/msg/VisionStateCamera.msg new file mode 100644 index 000000000..1e62903d4 --- /dev/null +++ b/ateam_msgs/msg/VisionStateCamera.msg @@ -0,0 +1,4 @@ +uint8 camera_id +VisionStateMHT[] yellow_robots +VisionStateMHT[] blue_robots +VisionStateMHT ball diff --git a/ateam_msgs/msg/VisionStateCameraArray.msg b/ateam_msgs/msg/VisionStateCameraArray.msg new file mode 100644 index 000000000..2d95c5662 --- /dev/null +++ b/ateam_msgs/msg/VisionStateCameraArray.msg @@ -0,0 +1 @@ +VisionStateCamera[] camera_states diff --git a/ateam_msgs/msg/VisionStateIMM.msg b/ateam_msgs/msg/VisionStateIMM.msg new file mode 100644 index 000000000..b9a300ffe --- /dev/null +++ b/ateam_msgs/msg/VisionStateIMM.msg @@ -0,0 +1,2 @@ +VisionStateModel[] models +uint32 updates_until_valid_track diff --git a/ateam_msgs/msg/VisionStateMHT.msg b/ateam_msgs/msg/VisionStateMHT.msg new file mode 100644 index 000000000..c3c289119 --- /dev/null +++ b/ateam_msgs/msg/VisionStateMHT.msg @@ -0,0 +1 @@ +VisionStateIMM[] tracks diff --git a/ateam_msgs/msg/VisionModelState.msg b/ateam_msgs/msg/VisionStateModel.msg similarity index 60% rename from ateam_msgs/msg/VisionModelState.msg rename to ateam_msgs/msg/VisionStateModel.msg index 7c0e05647..4d2d4d415 100644 --- a/ateam_msgs/msg/VisionModelState.msg +++ b/ateam_msgs/msg/VisionStateModel.msg @@ -1,2 +1,2 @@ int32 model_type -float32 mu \ No newline at end of file +float32 mu diff --git a/ateam_msgs/msg/VisionStateRobot.msg b/ateam_msgs/msg/VisionStateRobot.msg new file mode 100644 index 000000000..a462624d4 --- /dev/null +++ b/ateam_msgs/msg/VisionStateRobot.msg @@ -0,0 +1,5 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Twist twist_body +geometry_msgs/Accel accel +bool visible diff --git a/ateam_msgs/msg/VisionWorldState.msg b/ateam_msgs/msg/VisionWorldState.msg deleted file mode 100644 index 81cc37884..000000000 --- a/ateam_msgs/msg/VisionWorldState.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/VisionCameraState[] camera_states \ No newline at end of file diff --git a/ateam_msgs/msg/World.msg b/ateam_msgs/msg/World.msg deleted file mode 100644 index 9de809e2d..000000000 --- a/ateam_msgs/msg/World.msg +++ /dev/null @@ -1,18 +0,0 @@ -builtin_interfaces/Time current_time - -FieldInfo field -RefereeInfo referee_info - -BallState[] balls -RobotState[] our_robots -RobotState[] their_robots - -RobotState[] plan_from_our_robots - -ateam_msgs/BehaviorExecutorState behavior_executor_state - -bool ball_in_play -bool double_touch_enforced -int32 double_touch_id - -float64 fps From 03f27275a51b1b45333d0e2a0c508fd8046fe9f1 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:34:48 -0400 Subject: [PATCH 06/19] Switches vision filter to new vision state message types --- .../src/ateam_vision_filter_node.cpp | 17 +++++++++-------- .../ateam_vision_filter/src/camera.cpp | 10 +++++----- .../ateam_vision_filter/src/camera.hpp | 4 ++-- .../interacting_multiple_model_filter.cpp | 6 +++--- .../interacting_multiple_model_filter.hpp | 4 ++-- .../src/filters/multiple_hypothesis_tracker.cpp | 4 ++-- .../src/filters/multiple_hypothesis_tracker.hpp | 4 ++-- .../src/message_conversions.cpp | 8 ++++---- .../src/message_conversions.hpp | 8 ++++---- .../ateam_vision_filter/src/world.cpp | 6 +++--- .../ateam_vision_filter/src/world.hpp | 4 ++-- 11 files changed, 38 insertions(+), 37 deletions(-) diff --git a/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp index 26266641d..3db6dba90 100644 --- a/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp +++ b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp @@ -53,26 +53,26 @@ class VisionFilterNode : public rclcpp::Node }); timer_ = create_wall_timer(10ms, std::bind(&VisionFilterNode::timer_callback, this)); - ball_publisher_ = create_publisher( + ball_publisher_ = create_publisher( std::string(Topics::kBall), rclcpp::SystemDefaultsQoS()); ateam_common::indexed_topic_helpers::create_indexed_publishers - ( + ( blue_robots_publisher_, Topics::kBlueTeamRobotPrefix, rclcpp::SystemDefaultsQoS(), this ); ateam_common::indexed_topic_helpers::create_indexed_publishers - ( + ( yellow_robots_publisher_, Topics::kYellowTeamRobotPrefix, rclcpp::SystemDefaultsQoS(), this ); - vision_state_publisher_ = create_publisher( + vision_state_publisher_ = create_publisher( std::string(Topics::kVisionState), rclcpp::SystemDefaultsQoS()); @@ -140,11 +140,12 @@ class VisionFilterNode : public rclcpp::Node private: rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr ball_publisher_; - std::array::SharedPtr, 16> blue_robots_publisher_; - std::array::SharedPtr, + rclcpp::Publisher::SharedPtr ball_publisher_; + std::array::SharedPtr, + 16> blue_robots_publisher_; + std::array::SharedPtr, 16> yellow_robots_publisher_; - rclcpp::Publisher::SharedPtr vision_state_publisher_; + rclcpp::Publisher::SharedPtr vision_state_publisher_; rclcpp::Subscription::SharedPtr ssl_vision_subscription_; rclcpp::Subscription::SharedPtr ssl_vision_subs_; rclcpp::Subscription::SharedPtr field_subscription_; diff --git a/state_tracking/ateam_vision_filter/src/camera.cpp b/state_tracking/ateam_vision_filter/src/camera.cpp index af45d17c8..51594773f 100644 --- a/state_tracking/ateam_vision_filter/src/camera.cpp +++ b/state_tracking/ateam_vision_filter/src/camera.cpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include "filters/kalman_filter.hpp" #include "filters/interacting_multiple_model_filter.hpp" @@ -116,14 +116,14 @@ std::array, 16> Camera::get_blue_robot_est return get_robot_estimates_with_score(blue_team); } -ateam_msgs::msg::VisionCameraState Camera::get_vision_camera_state() const +ateam_msgs::msg::VisionStateCamera Camera::get_vision_camera_state() const { - ateam_msgs::msg::VisionCameraState camera_state; + ateam_msgs::msg::VisionStateCamera camera_state; // Prefill up to team size for (std::size_t i = 0; i < yellow_team.size(); i++) { - camera_state.yellow_robots.push_back(ateam_msgs::msg::VisionMHTState()); - camera_state.blue_robots.push_back(ateam_msgs::msg::VisionMHTState()); + camera_state.yellow_robots.push_back(ateam_msgs::msg::VisionStateMHT()); + camera_state.blue_robots.push_back(ateam_msgs::msg::VisionStateMHT()); } for (std::size_t i = 0; i < yellow_team.size(); i++) { diff --git a/state_tracking/ateam_vision_filter/src/camera.hpp b/state_tracking/ateam_vision_filter/src/camera.hpp index 7902df26d..8282af9d8 100644 --- a/state_tracking/ateam_vision_filter/src/camera.hpp +++ b/state_tracking/ateam_vision_filter/src/camera.hpp @@ -30,7 +30,7 @@ #include -#include +#include #include "filters/multiple_hypothesis_tracker.hpp" #include "generators/model_input_generator.hpp" @@ -81,7 +81,7 @@ class Camera /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionCameraState get_vision_camera_state() const; + ateam_msgs::msg::VisionStateCamera get_vision_camera_state() const; void set_ignored_half(int ignored_half) { diff --git a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp index b85ea668e..b5f7326f5 100644 --- a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp +++ b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp @@ -164,11 +164,11 @@ double InteractingMultipleModelFilter::get_potential_measurement_error( return potential_measurement_error.norm(); } -ateam_msgs::msg::VisionIMMState InteractingMultipleModelFilter::get_vision_imm_state() const +ateam_msgs::msg::VisionStateIMM InteractingMultipleModelFilter::get_vision_imm_state() const { - ateam_msgs::msg::VisionIMMState imm_state; + ateam_msgs::msg::VisionStateIMM imm_state; for (const auto & model_type : model_types) { - ateam_msgs::msg::VisionModelState model_state; + ateam_msgs::msg::VisionStateModel model_state; model_state.model_type = model_type; model_state.mu = mu.at(model_type); imm_state.models.push_back(model_state); diff --git a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp index 44da44c46..97f637165 100644 --- a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp +++ b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include "filters/kalman_filter.hpp" #include "types/models.hpp" @@ -101,7 +101,7 @@ class InteractingMultipleModelFilter /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionIMMState get_vision_imm_state() const; + ateam_msgs::msg::VisionStateIMM get_vision_imm_state() const; private: void update_mu(const Eigen::VectorXd & zt); diff --git a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp index 6fec90ffc..3f38451f9 100644 --- a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp +++ b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp @@ -124,9 +124,9 @@ get_state_estimate() const return std::make_pair(best_track->get_state_estimate(), best_track->get_validity_score()); } -ateam_msgs::msg::VisionMHTState MultipleHypothesisTracker::get_vision_mht_state() const +ateam_msgs::msg::VisionStateMHT MultipleHypothesisTracker::get_vision_mht_state() const { - ateam_msgs::msg::VisionMHTState mht_state; + ateam_msgs::msg::VisionStateMHT mht_state; for (const auto & track : tracks) { mht_state.tracks.push_back(track.get_vision_imm_state()); } diff --git a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp index 69aca2655..8203d0beb 100644 --- a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp +++ b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp @@ -32,7 +32,7 @@ #include #include -#include +#include #include "filters/interacting_multiple_model_filter.hpp" @@ -54,7 +54,7 @@ class MultipleHypothesisTracker /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionMHTState get_vision_mht_state() const; + ateam_msgs::msg::VisionStateMHT get_vision_mht_state() const; private: void remove_expired_tracks(); diff --git a/state_tracking/ateam_vision_filter/src/message_conversions.cpp b/state_tracking/ateam_vision_filter/src/message_conversions.cpp index b1886ff1a..d071ff580 100644 --- a/state_tracking/ateam_vision_filter/src/message_conversions.cpp +++ b/state_tracking/ateam_vision_filter/src/message_conversions.cpp @@ -33,9 +33,9 @@ namespace ateam_vision_filter::message_conversions { -ateam_msgs::msg::BallState toMsg(const std::optional & maybe_ball) +ateam_msgs::msg::VisionStateBall toMsg(const std::optional & maybe_ball) { - ateam_msgs::msg::BallState ball_state_msg; + ateam_msgs::msg::VisionStateBall ball_state_msg; ball_state_msg.visible = maybe_ball.has_value(); if (maybe_ball.has_value()) { auto obj = maybe_ball.value(); @@ -51,9 +51,9 @@ ateam_msgs::msg::BallState toMsg(const std::optional & maybe_ball) return ball_state_msg; } -ateam_msgs::msg::RobotState toMsg(const std::optional & maybe_robot) +ateam_msgs::msg::VisionStateRobot toMsg(const std::optional & maybe_robot) { - ateam_msgs::msg::RobotState robot_state_msg; + ateam_msgs::msg::VisionStateRobot robot_state_msg; robot_state_msg.visible = maybe_robot.has_value(); if (maybe_robot.has_value()) { diff --git a/state_tracking/ateam_vision_filter/src/message_conversions.hpp b/state_tracking/ateam_vision_filter/src/message_conversions.hpp index 15dd705a3..0ad9e5c14 100644 --- a/state_tracking/ateam_vision_filter/src/message_conversions.hpp +++ b/state_tracking/ateam_vision_filter/src/message_conversions.hpp @@ -24,8 +24,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -43,8 +43,8 @@ namespace ateam_vision_filter::message_conversions { -ateam_msgs::msg::BallState toMsg(const std::optional & maybe_ball); -ateam_msgs::msg::RobotState toMsg(const std::optional & maybe_robot); +ateam_msgs::msg::VisionStateBall toMsg(const std::optional & maybe_ball); +ateam_msgs::msg::VisionStateRobot toMsg(const std::optional & maybe_robot); CameraMeasurement fromMsg( const ssl_league_msgs::msg::VisionDetectionFrame & ros_msg, diff --git a/state_tracking/ateam_vision_filter/src/world.cpp b/state_tracking/ateam_vision_filter/src/world.cpp index 689cf105d..2e6109a15 100644 --- a/state_tracking/ateam_vision_filter/src/world.cpp +++ b/state_tracking/ateam_vision_filter/src/world.cpp @@ -227,11 +227,11 @@ std::array, 16> World::get_blue_robots_estimate() return blue_robots_estimates; } -ateam_msgs::msg::VisionWorldState World::get_vision_world_state() const +ateam_msgs::msg::VisionStateCameraArray World::get_vision_world_state() const { - ateam_msgs::msg::VisionWorldState world_state; + ateam_msgs::msg::VisionStateCameraArray world_state; for (const auto & camera_pair : cameras) { - ateam_msgs::msg::VisionCameraState camera_state = camera_pair.second.get_vision_camera_state(); + ateam_msgs::msg::VisionStateCamera camera_state = camera_pair.second.get_vision_camera_state(); camera_state.camera_id = camera_pair.first; world_state.camera_states.push_back(camera_state); } diff --git a/state_tracking/ateam_vision_filter/src/world.hpp b/state_tracking/ateam_vision_filter/src/world.hpp index 2b55fea6c..5f7464148 100644 --- a/state_tracking/ateam_vision_filter/src/world.hpp +++ b/state_tracking/ateam_vision_filter/src/world.hpp @@ -29,7 +29,7 @@ #include -#include +#include #include "camera.hpp" #include "generators/model_input_generator.hpp" @@ -80,7 +80,7 @@ class World /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionWorldState get_vision_world_state() const; + ateam_msgs::msg::VisionStateCameraArray get_vision_world_state() const; private: rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; From bef9b436003f37a27433f10e310c909a4b46bd09 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:36:37 -0400 Subject: [PATCH 07/19] Switches UI to new vision state messages --- ateam_ui/src/src/rosManager.ts | 4 ++-- ateam_ui/test/launch_tests/ateam_ui_test.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ateam_ui/src/src/rosManager.ts b/ateam_ui/src/src/rosManager.ts index d62289f8f..ad958ed24 100644 --- a/ateam_ui/src/src/rosManager.ts +++ b/ateam_ui/src/src/rosManager.ts @@ -204,7 +204,7 @@ export class RosManager { ballTopic = new ROSLIB.Topic({ ros: this.ros, name: '/ball', - messageType: 'ateam_msgs/msg/BallState' + messageType: 'ateam_msgs/msg/VisionStateBall' }); this.subscriptions.set("ball", ballTopic); } @@ -217,7 +217,7 @@ export class RosManager { robotTopic = new ROSLIB.Topic({ ros: this.ros, name: '/' + team + '_team/robot' + i, - messageType: 'ateam_msgs/msg/RobotState' + messageType: 'ateam_msgs/msg/VisionStateRobot' }); this.subscriptions.set("/" + team + "_team/robot" + i, robotTopic); } diff --git a/ateam_ui/test/launch_tests/ateam_ui_test.py b/ateam_ui/test/launch_tests/ateam_ui_test.py index f2c10f7b0..6d96ab634 100755 --- a/ateam_ui/test/launch_tests/ateam_ui_test.py +++ b/ateam_ui/test/launch_tests/ateam_ui_test.py @@ -103,12 +103,12 @@ def test_0_subscribers_created(self): self.assertIsNotNone(subscriptions) # Check subscription to /ball - self.assertIn(("/ball", ['ateam_msgs/msg/BallState']), subscriptions) + self.assertIn(("/ball", ['ateam_msgs/msg/VisionStateBall']), subscriptions) # Check robot subscriptions for team in ["yellow", "blue"]: for id in range(0, 16): - topic = (f"/{team}_team/robot{id}", ['ateam_msgs/msg/RobotState']) + topic = (f"/{team}_team/robot{id}", ['ateam_msgs/msg/VisionStateRobot']) self.assertIn(topic, subscriptions) # Check subscription to /overlay From 56c87c0951f3ff48cf45f0bbfc4df136bb4057fa Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:42:10 -0400 Subject: [PATCH 08/19] Switches motion benchmarks to new vision state messages --- .../src/angular_velocity_benchmark.cpp | 16 ++++++++++------ .../src/velocity_benchmark.cpp | 16 ++++++++++------ 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp index b229a8bad..37bedaf34 100644 --- a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -125,7 +125,7 @@ struct Options struct Robot { bool connected = false; - std::optional state; + std::optional state; std::optional feedback; std::optional motion_feedback; }; @@ -156,8 +156,9 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node const auto state_topic_prefix = options_.team_color == ateam_common::TeamColor::Blue ? Topics::kBlueTeamRobotPrefix : Topics::kYellowTeamRobotPrefix; - create_indexed_subscribers( - robot_state_subs_, state_topic_prefix, 1, &AngularVelocityBenchmarkNode::RobotStateCallback, + create_indexed_subscribers( + robot_state_subs_, state_topic_prefix, 1, + &AngularVelocityBenchmarkNode::VisionStateRobotCallback, this); create_indexed_subscribers(robot_feedback_subs_, @@ -196,7 +197,8 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node rclcpp::Publisher::SharedPtr command_pub_; rclcpp::Subscription::SharedPtr field_sub_; - std::array::SharedPtr, 16> robot_state_subs_; + std::array::SharedPtr, + 16> robot_state_subs_; std::array::SharedPtr, 16> robot_feedback_subs_; std::array::SharedPtr, @@ -210,7 +212,9 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node field_ = *msg; } - void RobotStateCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int robot_id) + void VisionStateRobotCallback( + const ateam_msgs::msg::VisionStateRobot::SharedPtr msg, + int robot_id) { robots_[robot_id].state = *msg; } diff --git a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp index 7b1e18f30..4d5f7db2b 100644 --- a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -120,7 +120,7 @@ struct Options struct Robot { bool connected = false; - std::optional state; + std::optional state; std::optional feedback; std::optional motion_feedback; }; @@ -151,8 +151,9 @@ class VelocityBenchmarkNode : public rclcpp::Node const auto state_topic_prefix = options_.team_color == ateam_common::TeamColor::Blue ? Topics::kBlueTeamRobotPrefix : Topics::kYellowTeamRobotPrefix; - create_indexed_subscribers( - robot_state_subs_, state_topic_prefix, 1, &VelocityBenchmarkNode::RobotStateCallback, this); + create_indexed_subscribers( + robot_state_subs_, state_topic_prefix, 1, &VelocityBenchmarkNode::VisionStateRobotCallback, + this); create_indexed_subscribers(robot_feedback_subs_, Topics::kRobotFeedbackPrefix, 1, &VelocityBenchmarkNode::RobotFeedbackCallback, this); @@ -187,7 +188,8 @@ class VelocityBenchmarkNode : public rclcpp::Node rclcpp::Publisher::SharedPtr command_pub_; rclcpp::Subscription::SharedPtr field_sub_; - std::array::SharedPtr, 16> robot_state_subs_; + std::array::SharedPtr, + 16> robot_state_subs_; std::array::SharedPtr, 16> robot_feedback_subs_; std::array::SharedPtr, @@ -201,7 +203,9 @@ class VelocityBenchmarkNode : public rclcpp::Node field_ = *msg; } - void RobotStateCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int robot_id) + void VisionStateRobotCallback( + const ateam_msgs::msg::VisionStateRobot::SharedPtr msg, + int robot_id) { robots_[robot_id].state = *msg; } From a1a741b02980db6737bcfea611588c67825753ca Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 01:03:42 -0400 Subject: [PATCH 09/19] Switches Kenobi to new vision/game state types. --- ateam_kenobi/src/core/fps_tracker.hpp | 4 +- .../src/core/motion/motion_controller.cpp | 1 - .../src/core/types/message_conversions.cpp | 24 +++++------ .../src/core/types/message_conversions.hpp | 23 ++++------- ateam_kenobi/src/kenobi_node.cpp | 41 +++++++++++-------- 5 files changed, 47 insertions(+), 46 deletions(-) diff --git a/ateam_kenobi/src/core/fps_tracker.hpp b/ateam_kenobi/src/core/fps_tracker.hpp index bc0c5af1a..ecbafa25c 100644 --- a/ateam_kenobi/src/core/fps_tracker.hpp +++ b/ateam_kenobi/src/core/fps_tracker.hpp @@ -36,7 +36,7 @@ class FpsTracker std::fill(buffer_.begin(), buffer_.end(), average_fps_ / kWindowWidth); } - void update(World & world) + double Update(const World & world) { const auto dt = ateam_common::TimeDiffSeconds(world.current_time, prev_time_); prev_time_ = world.current_time; @@ -45,7 +45,7 @@ class FpsTracker buffer_[buffer_index_] = current_fps / kWindowWidth; average_fps_ += buffer_[buffer_index_]; buffer_index_ = (buffer_index_ + 1) % kWindowWidth; - world.fps = average_fps_; + return average_fps_; } private: diff --git a/ateam_kenobi/src/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index 7f062a378..2a70aedbe 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include "ateam_geometry/ateam_geometry.hpp" #include "ateam_common/robot_constants.hpp" #include "pid.hpp" diff --git a/ateam_kenobi/src/core/types/message_conversions.cpp b/ateam_kenobi/src/core/types/message_conversions.cpp index 8e20f28b3..90d57f676 100644 --- a/ateam_kenobi/src/core/types/message_conversions.cpp +++ b/ateam_kenobi/src/core/types/message_conversions.cpp @@ -21,9 +21,11 @@ #include "core/types/message_conversions.hpp" #include - #include +#include "core/types/field.hpp" +#include "core/types/referee_info.hpp" + namespace ateam_kenobi::message_conversions { ateam_msgs::msg::FieldInfo toMsg(const Field & obj) @@ -81,9 +83,9 @@ ateam_msgs::msg::RefereeInfo toMsg(const RefereeInfo & obj) } -ateam_msgs::msg::BallState toMsg(const Ball & obj) +ateam_msgs::msg::GameStateBall toMsg(const Ball & obj) { - ateam_msgs::msg::BallState ball_state_msg; + ateam_msgs::msg::GameStateBall ball_state_msg; ball_state_msg.pose.position.x = obj.pos.x(); ball_state_msg.pose.position.y = obj.pos.y(); ball_state_msg.twist.linear.x = obj.vel.x(); @@ -93,9 +95,9 @@ ateam_msgs::msg::BallState toMsg(const Ball & obj) return ball_state_msg; } -ateam_msgs::msg::RobotState toMsg(const Robot & obj) +ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj) { - ateam_msgs::msg::RobotState robot_state_msg; + ateam_msgs::msg::GameStateRobot robot_state_msg; robot_state_msg.pose.position.x = obj.pos.x(); robot_state_msg.pose.position.y = obj.pos.y(); robot_state_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), obj.theta)); @@ -107,9 +109,9 @@ ateam_msgs::msg::RobotState toMsg(const Robot & obj) return robot_state_msg; } -ateam_msgs::msg::World toMsg(const World & obj) +ateam_msgs::msg::GameStateWorld toMsg(const World & obj) { - ateam_msgs::msg::World world_msg; + ateam_msgs::msg::GameStateWorld world_msg; world_msg.current_time = rclcpp::Time( @@ -119,13 +121,13 @@ ateam_msgs::msg::World toMsg(const World & obj) world_msg.field = toMsg(obj.field); world_msg.referee_info = toMsg(obj.referee_info); - world_msg.balls.push_back(toMsg(obj.ball)); + world_msg.ball = toMsg(obj.ball); for (const Robot & robot : obj.our_robots) { if (robot.visible || robot.radio_connected) { world_msg.our_robots.push_back(toMsg(robot)); } else { - world_msg.our_robots.push_back(ateam_msgs::msg::RobotState()); + world_msg.our_robots.push_back(ateam_msgs::msg::GameStateRobot()); } } @@ -133,7 +135,7 @@ ateam_msgs::msg::World toMsg(const World & obj) if (robot.visible) { world_msg.their_robots.push_back(toMsg(robot)); } else { - world_msg.their_robots.push_back(ateam_msgs::msg::RobotState()); + world_msg.their_robots.push_back(ateam_msgs::msg::GameStateRobot()); } } @@ -141,8 +143,6 @@ ateam_msgs::msg::World toMsg(const World & obj) world_msg.double_touch_enforced = obj.double_touch_forbidden_id_.has_value(); world_msg.double_touch_id = obj.double_touch_forbidden_id_.value_or(-1); - world_msg.fps = obj.fps; - return world_msg; } diff --git a/ateam_kenobi/src/core/types/message_conversions.hpp b/ateam_kenobi/src/core/types/message_conversions.hpp index a5cd7a696..5413438de 100644 --- a/ateam_kenobi/src/core/types/message_conversions.hpp +++ b/ateam_kenobi/src/core/types/message_conversions.hpp @@ -22,25 +22,20 @@ #ifndef CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ #define CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include "core/types/ball.hpp" +#include "core/types/robot.hpp" #include "core/types/world.hpp" -#include "core/types/field.hpp" -#include "core/types/referee_info.hpp" + namespace ateam_kenobi::message_conversions { -ateam_msgs::msg::BallState toMsg(const Ball & obj); -ateam_msgs::msg::RobotState toMsg(const Robot & obj); - -ateam_msgs::msg::World toMsg(const World & obj); +ateam_msgs::msg::GameStateBall toMsg(const Ball & obj); +ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj); +ateam_msgs::msg::GameStateWorld toMsg(const World & obj); } // namespace ateam_kenobi::message_conversions diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 44035069a..85db050b1 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -25,18 +25,19 @@ #include #include #include -#include #include #include -#include +#include +#include #include #include #include #include -#include +#include #include #include #include +#include #include #include #include @@ -86,14 +87,14 @@ class KenobiNode : public rclcpp::Node "/play_info", rclcpp::SystemDefaultsQoS()); - create_indexed_subscribers( + create_indexed_subscribers( blue_robots_subscriptions_, Topics::kBlueTeamRobotPrefix, 10, &KenobiNode::blue_robot_state_callback, this); - create_indexed_subscribers( + create_indexed_subscribers( yellow_robots_subscriptions_, Topics::kYellowTeamRobotPrefix, 10, @@ -118,12 +119,12 @@ class KenobiNode : public rclcpp::Node robot_commands_publishers_, Topics::kRobotMotionCommandPrefix, rclcpp::SystemDefaultsQoS(), this); - ball_subscription_ = create_subscription( + ball_subscription_ = create_subscription( std::string(Topics::kBall), 10, std::bind(&KenobiNode::ball_state_callback, this, std::placeholders::_1)); - world_publisher_ = create_publisher( + world_publisher_ = create_publisher( "~/world", rclcpp::SystemDefaultsQoS()); @@ -146,6 +147,9 @@ class KenobiNode : public rclcpp::Node "~/playbook_state", rclcpp::SystemDefaultsQoS()); + status_publisher_ = create_publisher("~/status", + rclcpp::SystemDefaultsQoS()); + timer_ = create_wall_timer(10ms, std::bind(&KenobiNode::timer_callback, this)); const auto playbook_path = declare_parameter("playbook", ""); @@ -182,10 +186,10 @@ class KenobiNode : public rclcpp::Node FpsTracker fps_tracker_; rclcpp::Publisher::SharedPtr overlay_publisher_; rclcpp::Publisher::SharedPtr play_info_publisher_; - rclcpp::Subscription::SharedPtr ball_subscription_; - std::array::SharedPtr, + rclcpp::Subscription::SharedPtr ball_subscription_; + std::array::SharedPtr, 16> blue_robots_subscriptions_; - std::array::SharedPtr, + std::array::SharedPtr, 16> yellow_robots_subscriptions_; std::array::SharedPtr, 16> robot_feedback_subscriptions_; @@ -198,10 +202,11 @@ class KenobiNode : public rclcpp::Node rclcpp::Service::SharedPtr override_service_; rclcpp::Service::SharedPtr play_enabled_service_; rclcpp::Publisher::SharedPtr playbook_state_publisher_; + rclcpp::Publisher::SharedPtr status_publisher_; ateam_common::GameControllerListener game_controller_listener_; - rclcpp::Publisher::SharedPtr world_publisher_; + rclcpp::Publisher::SharedPtr world_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -216,7 +221,7 @@ class KenobiNode : public rclcpp::Node } void blue_robot_state_callback( - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg, + const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg, int id) { const auto our_color = game_controller_listener_.GetTeamColor(); @@ -229,7 +234,7 @@ class KenobiNode : public rclcpp::Node } void yellow_robot_state_callback( - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg, + const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg, int id) { const auto our_color = game_controller_listener_.GetTeamColor(); @@ -244,7 +249,7 @@ class KenobiNode : public rclcpp::Node void robot_state_callback( std::array & robot_states, std::size_t id, - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg) + const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg) { robot_states.at(id).visible = robot_state_msg->visible; if (robot_state_msg->visible) { @@ -278,7 +283,7 @@ class KenobiNode : public rclcpp::Node world_.our_robots.at(id).radio_connected = robot_connection_msg->radio_connected; } - void ball_state_callback(const ateam_msgs::msg::BallState::SharedPtr ball_state_msg) + void ball_state_callback(const ateam_msgs::msg::VisionStateBall::SharedPtr ball_state_msg) { world_.ball.visible = ball_state_msg->visible; const auto ball_visibility_timed_out = ateam_common::TimeDiffSeconds(world_.current_time, @@ -427,10 +432,12 @@ class KenobiNode : public rclcpp::Node "DETECTED TEAM SIDE WAS UNKNOWN"); } - fps_tracker_.update(world_); - world_publisher_->publish(ateam_kenobi::message_conversions::toMsg(world_)); + ateam_msgs::msg::KenobiStatus status_msg; + status_msg.fps = fps_tracker_.Update(world_); + status_publisher_->publish(status_msg); + auto motion_commands = runPlayFrame(world_); defense_area_enforcement::EnforceDefenseAreaKeepout(world_, motion_commands); From 6e9f46b5b13e14ffab8f5f8ccb185d8b1dab78fe Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 01:20:52 -0400 Subject: [PATCH 10/19] Switches ateam_game_state to new state message types. --- .../ateam_game_state/type_adapters.hpp | 24 +++++++-------- .../game_state_tracker/game_state_tracker.cpp | 30 +++++++++++-------- .../ateam_game_state/src/type_adapters.cpp | 30 ++++++++++--------- 3 files changed, 45 insertions(+), 39 deletions(-) diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp index e395efc7b..f5c7e84df 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp @@ -22,9 +22,9 @@ #ifndef ATEAM_GAME_STATE__TYPE_ADAPTERS_HPP_ #define ATEAM_GAME_STATE__TYPE_ADAPTERS_HPP_ -#include -#include -#include +#include +#include +#include #include #include #include @@ -36,11 +36,11 @@ template<> -struct rclcpp::TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = ateam_game_state::World; - using ros_message_type = ateam_msgs::msg::World; + using ros_message_type = ateam_msgs::msg::GameStateWorld; static void convert_to_ros_message(const custom_type & world, ros_message_type & ros_msg); @@ -48,11 +48,11 @@ struct rclcpp::TypeAdapter }; template<> -struct rclcpp::TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = ateam_game_state::Ball; - using ros_message_type = ateam_msgs::msg::BallState; + using ros_message_type = ateam_msgs::msg::GameStateBall; static void convert_to_ros_message(const custom_type & ball, ros_message_type & ros_msg); @@ -60,11 +60,11 @@ struct rclcpp::TypeAdapter }; template<> -struct rclcpp::TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = ateam_game_state::Robot; - using ros_message_type = ateam_msgs::msg::RobotState; + using ros_message_type = ateam_msgs::msg::GameStateRobot; static void convert_to_ros_message(const custom_type & robot, ros_message_type & ros_msg); @@ -95,9 +95,9 @@ struct rclcpp::TypeAdapter #include #include -#include +#include +#include #include #include #include @@ -55,19 +56,19 @@ class GameStateTracker : public rclcpp::Node { 1, std::bind(&GameStateTracker::FieldCallback, this, std::placeholders::_1)); - ball_sub_ = create_subscription( + ball_sub_ = create_subscription( std::string(Topics::kBall), 1, - std::bind(&GameStateTracker::BallCallback, this, std::placeholders::_1)); // TODO(barulicm): need to split vision output messages from world rep messages + std::bind(&GameStateTracker::BallCallback, this, std::placeholders::_1)); - create_indexed_subscribers( + create_indexed_subscribers( blue_bot_subs_, Topics::kBlueTeamRobotPrefix, 1, &GameStateTracker::BlueVisionBotCallback, this); - create_indexed_subscribers( + create_indexed_subscribers( yellow_bot_subs_, Topics::kYellowTeamRobotPrefix, 1, @@ -84,10 +85,10 @@ class GameStateTracker : public rclcpp::Node { InPlayEvaluator in_play_evaluator_; rclcpp::Publisher::SharedPtr world_pub_; rclcpp::Subscription::SharedPtr field_sub_; - rclcpp::Subscription::SharedPtr ball_sub_; - std::array::SharedPtr, + rclcpp::Subscription::SharedPtr ball_sub_; + std::array::SharedPtr, ateam_common::indexed_topic_helpers::kRobotCount> blue_bot_subs_; - std::array::SharedPtr, + std::array::SharedPtr, ateam_common::indexed_topic_helpers::kRobotCount> yellow_bot_subs_; ateam_common::GameControllerListener gc_listener_; // TODO(barulicm): Move GCListener type to ateam_game_state package World world_; @@ -108,12 +109,15 @@ class GameStateTracker : public rclcpp::Node { world_.field = *field; } - void BallCallback(const std::unique_ptr & ball) + void BallCallback(const std::unique_ptr & msg) { - world_.ball = *ball; + world_.ball.pos = ateam_geometry::Point(msg->pose.position.x, msg->pose.position.y); + world_.ball.vel = ateam_geometry::Vector(msg->twist.linear.x, msg->twist.linear.y); + world_.ball.visible = msg->visible; + // TODO(barulicm): Update remaining Ball fields } - void UpdateBotFromVision(Robot & robot, const ateam_msgs::msg::RobotState::SharedPtr msg) + void UpdateBotFromVision(Robot & robot, const ateam_msgs::msg::VisionStateRobot::SharedPtr msg) { robot.visible = msg->visible; if (msg->visible) { @@ -130,7 +134,7 @@ class GameStateTracker : public rclcpp::Node { } } - void BlueVisionBotCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int id) + void BlueVisionBotCallback(const ateam_msgs::msg::VisionStateRobot::SharedPtr msg, int id) { const auto our_color = gc_listener_.GetTeamColor(); if(our_color == ateam_common::TeamColor::Unknown) { @@ -141,7 +145,7 @@ class GameStateTracker : public rclcpp::Node { UpdateBotFromVision(robot_state_array[id], msg); } - void YellowVisionBotCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int id) + void YellowVisionBotCallback(const ateam_msgs::msg::VisionStateRobot::SharedPtr msg, int id) { const auto our_color = gc_listener_.GetTeamColor(); if(our_color == ateam_common::TeamColor::Unknown) { diff --git a/state_tracking/ateam_game_state/src/type_adapters.cpp b/state_tracking/ateam_game_state/src/type_adapters.cpp index 9db6e8af8..c0a7e3cc1 100644 --- a/state_tracking/ateam_game_state/src/type_adapters.cpp +++ b/state_tracking/ateam_game_state/src/type_adapters.cpp @@ -25,12 +25,14 @@ #include #include -void rclcpp::TypeAdapter::convert_to_ros_message( +// TODO(barulicm): Check that all fields are adapted + +void rclcpp::TypeAdapter::convert_to_ros_message( const custom_type & world, ros_message_type & ros_msg) { using FieldTA = rclcpp::adapt_type::as; - using RobotTA = rclcpp::adapt_type::as; - using BallTA = rclcpp::adapt_type::as; + using RobotTA = rclcpp::adapt_type::as; + using BallTA = rclcpp::adapt_type::as; using RefTA = rclcpp::adapt_type::as; ros_msg.current_time = @@ -42,14 +44,14 @@ void rclcpp::TypeAdapter::conve RefTA::convert_to_ros_message(world.referee_info, ros_msg.referee_info); - BallTA::convert_to_ros_message(world.ball, ros_msg.balls.emplace_back()); + BallTA::convert_to_ros_message(world.ball, ros_msg.ball); ros_msg.our_robots.reserve(world.our_robots.size()); for (const ateam_game_state::Robot & robot : world.our_robots) { if (robot.visible || robot.radio_connected) { RobotTA::convert_to_ros_message(robot, ros_msg.our_robots.emplace_back()); } else { - ros_msg.our_robots.push_back(ateam_msgs::msg::RobotState()); + ros_msg.our_robots.push_back(ateam_msgs::msg::GameStateRobot()); } } @@ -58,7 +60,7 @@ void rclcpp::TypeAdapter::conve if (robot.visible) { RobotTA::convert_to_ros_message(robot, ros_msg.their_robots.emplace_back()); } else { - ros_msg.their_robots.push_back(ateam_msgs::msg::RobotState()); + ros_msg.their_robots.push_back(ateam_msgs::msg::GameStateRobot()); } } @@ -67,12 +69,12 @@ void rclcpp::TypeAdapter::conve ros_msg.double_touch_id = world.double_touch_forbidden_id_.value_or(-1); } -void rclcpp::TypeAdapter::convert_to_custom( +void rclcpp::TypeAdapter::convert_to_custom( const ros_message_type & ros_msg, custom_type & world) { using FieldTA = rclcpp::adapt_type::as; - using RobotTA = rclcpp::adapt_type::as; - using BallTA = rclcpp::adapt_type::as; + using RobotTA = rclcpp::adapt_type::as; + using BallTA = rclcpp::adapt_type::as; using RefTA = rclcpp::adapt_type::as; world.current_time = @@ -83,7 +85,7 @@ void rclcpp::TypeAdapter::conve RefTA::convert_to_custom(ros_msg.referee_info, world.referee_info); - BallTA::convert_to_custom(ros_msg.balls.front(), world.ball); + BallTA::convert_to_custom(ros_msg.ball, world.ball); const auto convert_bot = [](const auto & ros_robot) { ateam_game_state::Robot robot; @@ -106,7 +108,7 @@ void rclcpp::TypeAdapter::conve } void rclcpp::TypeAdapter::convert_to_ros_message( + ateam_msgs::msg::GameStateBall>::convert_to_ros_message( const custom_type & ball, ros_message_type & ros_msg) { ros_msg.pose.position.x = ball.pos.x(); @@ -116,7 +118,7 @@ void rclcpp::TypeAdapter::convert_to_custom( +void rclcpp::TypeAdapter::convert_to_custom( const ros_message_type & ros_msg, custom_type & ball) { ball.pos = ateam_geometry::Point(ros_msg.pose.position.x, ros_msg.pose.position.y); @@ -125,7 +127,7 @@ void rclcpp::TypeAdapter::co } void rclcpp::TypeAdapter::convert_to_ros_message( + ateam_msgs::msg::GameStateRobot>::convert_to_ros_message( const custom_type & robot, ros_message_type & ros_msg) { ros_msg.pose.position.x = robot.pos.x(); @@ -137,7 +139,7 @@ void rclcpp::TypeAdapter::convert_to_custom( +void rclcpp::TypeAdapter::convert_to_custom( const ros_message_type & ros_msg, custom_type & robot) { robot.pos = ateam_geometry::Point(ros_msg.pose.position.x, ros_msg.pose.position.y); From 2b26a3762a7bd2763cb5cb0a9e000a47e292c4ab Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 22:21:31 -0400 Subject: [PATCH 11/19] Synchronizes message and c++ fields in game state types --- ateam_msgs/msg/GameStateBall.msg | 1 + ateam_msgs/msg/GameStateRobot.msg | 14 ++- ateam_msgs/msg/RefereeInfo.msg | 9 +- .../include/ateam_game_state/field.hpp | 1 - .../include/ateam_game_state/referee_info.hpp | 2 +- .../game_state_tracker/game_state_tracker.cpp | 2 +- .../ateam_game_state/src/type_adapters.cpp | 105 +++++++++++++----- 7 files changed, 95 insertions(+), 39 deletions(-) diff --git a/ateam_msgs/msg/GameStateBall.msg b/ateam_msgs/msg/GameStateBall.msg index 8fb831044..3dc602c67 100644 --- a/ateam_msgs/msg/GameStateBall.msg +++ b/ateam_msgs/msg/GameStateBall.msg @@ -2,3 +2,4 @@ geometry_msgs/Pose pose geometry_msgs/Twist twist geometry_msgs/Accel accel bool visible +builtin_interfaces/Time last_visible_time diff --git a/ateam_msgs/msg/GameStateRobot.msg b/ateam_msgs/msg/GameStateRobot.msg index 839f1ed76..081c11bf9 100644 --- a/ateam_msgs/msg/GameStateRobot.msg +++ b/ateam_msgs/msg/GameStateRobot.msg @@ -1,6 +1,10 @@ -geometry_msgs/Pose pose -geometry_msgs/Twist twist -geometry_msgs/Twist twist_body -geometry_msgs/Accel accel +int32 id bool visible -bool connected +bool radio_connected +geometry_msgs/Pose pose +geometry_msgs/Twist velocity +geometry_msgs/Twist prev_command_velocity +bool breakbeam_ball_detected +bool breakbeam_ball_detected_filtered +bool kicker_available +bool chipper_available diff --git a/ateam_msgs/msg/RefereeInfo.msg b/ateam_msgs/msg/RefereeInfo.msg index 47f4becd8..2b709c355 100644 --- a/ateam_msgs/msg/RefereeInfo.msg +++ b/ateam_msgs/msg/RefereeInfo.msg @@ -1,7 +1,10 @@ uint8 our_goalie_id uint8 their_goalie_id uint8 game_stage -uint8 game_command +uint8 running_command uint8 prev_command - -geometry_msgs/Point32 designated_position \ No newline at end of file +bool has_designated_position +geometry_msgs/Point32 designated_position +bool has_next_command +uint8 next_command +builtin_interfaces/Time command_time diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp index 4a8921fc2..e4e446caf 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp @@ -30,7 +30,6 @@ struct FieldSidedInfo { std::array defense_area_corners; std::array goal_corners; - std::array goal_posts; }; struct Field { diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp index c23d3dabd..d46750392 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp @@ -33,7 +33,7 @@ struct RefereeInfo { int our_goalie_id = -1; int their_goalie_id = -1; - ateam_common::GameStage current_game_stage = ateam_common::GameStage::Unknown; + ateam_common::GameStage game_stage = ateam_common::GameStage::Unknown; ateam_common::GameCommand running_command = ateam_common::GameCommand::Halt; ateam_common::GameCommand prev_command = ateam_common::GameCommand::Halt; std::optional designated_position; diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index 26911d0bb..189a8cfa8 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -160,7 +160,7 @@ class GameStateTracker : public rclcpp::Node { { world_.referee_info.our_goalie_id = gc_listener_.GetOurGoalieID().value_or(-1); world_.referee_info.their_goalie_id = gc_listener_.GetTheirGoalieID().value_or(-1); - world_.referee_info.current_game_stage = gc_listener_.GetGameStage(); + world_.referee_info.game_stage = gc_listener_.GetGameStage(); world_.referee_info.running_command = gc_listener_.GetGameCommand(); world_.referee_info.prev_command = gc_listener_.GetPreviousGameCommand(); diff --git a/state_tracking/ateam_game_state/src/type_adapters.cpp b/state_tracking/ateam_game_state/src/type_adapters.cpp index c0a7e3cc1..26486147e 100644 --- a/state_tracking/ateam_game_state/src/type_adapters.cpp +++ b/state_tracking/ateam_game_state/src/type_adapters.cpp @@ -25,9 +25,8 @@ #include #include -// TODO(barulicm): Check that all fields are adapted - -void rclcpp::TypeAdapter::convert_to_ros_message( +void rclcpp::TypeAdapter::convert_to_ros_message( const custom_type & world, ros_message_type & ros_msg) { using FieldTA = rclcpp::adapt_type::as; @@ -69,7 +68,8 @@ void rclcpp::TypeAdapter::convert_to_custom( +void rclcpp::TypeAdapter::convert_to_custom( const ros_message_type & ros_msg, custom_type & world) { using FieldTA = rclcpp::adapt_type::as; @@ -116,6 +116,10 @@ void rclcpp::TypeAdapter( + ball.last_visible_time.time_since_epoch()).count()); } void rclcpp::TypeAdapter::convert_to_custom( @@ -124,31 +128,60 @@ void rclcpp::TypeAdapter ball.pos = ateam_geometry::Point(ros_msg.pose.position.x, ros_msg.pose.position.y); ball.vel = ateam_geometry::Vector(ros_msg.twist.linear.x, ros_msg.twist.linear.y); ball.visible = ros_msg.visible; + ball.last_visible_time = + std::chrono::steady_clock::time_point(std::chrono::seconds(ros_msg.last_visible_time.sec) + + std::chrono::nanoseconds(ros_msg.last_visible_time.nanosec)); } void rclcpp::TypeAdapter::convert_to_ros_message( const custom_type & robot, ros_message_type & ros_msg) { + ros_msg.id = robot.id; + ros_msg.visible = robot.visible; + ros_msg.radio_connected = robot.radio_connected; + ros_msg.pose.position.x = robot.pos.x(); ros_msg.pose.position.y = robot.pos.y(); ros_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), robot.theta)); - ros_msg.twist.linear.x = robot.vel.x(); - ros_msg.twist.linear.y = robot.vel.y(); - ros_msg.twist.angular.z = robot.omega; - ros_msg.visible = robot.visible; + + ros_msg.velocity.linear.x = robot.vel.x(); + ros_msg.velocity.linear.y = robot.vel.y(); + ros_msg.velocity.angular.z = robot.omega; + + ros_msg.prev_command_velocity.linear.x = robot.prev_command_vel.x(); + ros_msg.prev_command_velocity.linear.y = robot.prev_command_vel.y(); + ros_msg.prev_command_velocity.angular.z = robot.prev_command_omega; + + ros_msg.breakbeam_ball_detected = robot.breakbeam_ball_detected; + ros_msg.breakbeam_ball_detected_filtered = robot.breakbeam_ball_detected_filtered; + + ros_msg.kicker_available = robot.kicker_available; + ros_msg.chipper_available = robot.chipper_available; } -void rclcpp::TypeAdapter::convert_to_custom( +void rclcpp::TypeAdapter::convert_to_custom( const ros_message_type & ros_msg, custom_type & robot) { + robot.id = ros_msg.id; + robot.visible = ros_msg.visible; + robot.radio_connected = ros_msg.radio_connected; + robot.pos = ateam_geometry::Point(ros_msg.pose.position.x, ros_msg.pose.position.y); tf2::Quaternion quat; tf2::fromMsg(ros_msg.pose.orientation, quat); robot.theta = tf2::getYaw(quat); - robot.vel = ateam_geometry::Vector(ros_msg.twist.linear.x, ros_msg.twist.linear.y); - robot.omega = ros_msg.twist.angular.z; - robot.visible = ros_msg.visible; + + robot.vel = ateam_geometry::Vector(ros_msg.velocity.linear.x, ros_msg.velocity.linear.y); + robot.omega = ros_msg.velocity.angular.z; + + robot.prev_command_vel = ateam_geometry::Vector(ros_msg.prev_command_velocity.linear.x, + ros_msg.prev_command_velocity.linear.y); + robot.prev_command_omega = ros_msg.prev_command_velocity.angular.z; + + robot.kicker_available = ros_msg.kicker_available; + robot.chipper_available = ros_msg.chipper_available; } void rclcpp::TypeAdapter::c } void rclcpp::TypeAdapter::convert_to_ros_message(const custom_type & ref_info, + ateam_msgs::msg::RefereeInfo>::convert_to_ros_message( + const custom_type & ref_info, ros_message_type & ros_msg) { ros_msg.our_goalie_id = ref_info.our_goalie_id; ros_msg.their_goalie_id = ref_info.their_goalie_id; - ros_msg.game_stage = static_cast(ref_info.current_game_stage); - ros_msg.game_command = static_cast(ref_info.running_command); + ros_msg.game_stage = static_cast(ref_info.game_stage); + ros_msg.running_command = static_cast(ref_info.running_command); ros_msg.prev_command = static_cast(ref_info.prev_command); - if (ref_info.designated_position.has_value()) { - ros_msg.designated_position.x = ref_info.designated_position->x(); - ros_msg.designated_position.y = ref_info.designated_position->y(); - } else { - ros_msg.designated_position.x = 0.0f; - ros_msg.designated_position.y = 0.0f; - } + ros_msg.has_designated_position = ref_info.designated_position.has_value(); + const auto & designated_position = ref_info.designated_position.value_or(ateam_geometry::Point(0, + 0)); + ros_msg.designated_position.x = designated_position.x(); + ros_msg.designated_position.y = designated_position.y(); + + ros_msg.has_next_command = ref_info.next_command.has_value(); + ros_msg.next_command = + static_cast(ref_info.next_command.value_or(ateam_common::GameCommand::Halt)); + + ros_msg.command_time = + rclcpp::Time( + std::chrono::duration_cast( + ref_info.command_time.time_since_epoch()).count()); } void rclcpp::TypeAdapter::convert_to_custom(const ros_message_type & ros_msg, + ateam_msgs::msg::RefereeInfo>::convert_to_custom( + const ros_message_type & ros_msg, custom_type & ref_info) { ref_info.our_goalie_id = ros_msg.our_goalie_id; ref_info.their_goalie_id = ros_msg.their_goalie_id; - ref_info.current_game_stage = static_cast(ros_msg.game_stage); - ref_info.running_command = static_cast(ros_msg.game_command); + ref_info.game_stage = static_cast(ros_msg.game_stage); + ref_info.running_command = static_cast(ros_msg.running_command); ref_info.prev_command = static_cast(ros_msg.prev_command); - // TODO(barulicm): add 'has_designated_position' field - ref_info.designated_position = ateam_geometry::Point(ros_msg.designated_position.x, - ros_msg.designated_position.y); + if(ros_msg.has_designated_position) { + ref_info.designated_position = ateam_geometry::Point(ros_msg.designated_position.x, + ros_msg.designated_position.y); + } + if(ros_msg.has_next_command) { + ref_info.next_command = static_cast(ros_msg.next_command); + } + ref_info.command_time = + std::chrono::system_clock::time_point(std::chrono::seconds(ros_msg.command_time.sec) + + std::chrono::nanoseconds(ros_msg.command_time.nanosec)); } From 4029c4bc963df82027fecfad88ccef0acc83f80f Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 22:24:12 -0400 Subject: [PATCH 12/19] Adds missing field to game state tracker ball update function. --- .../src/game_state_tracker/game_state_tracker.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index 189a8cfa8..1fa915603 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -114,7 +114,9 @@ class GameStateTracker : public rclcpp::Node { world_.ball.pos = ateam_geometry::Point(msg->pose.position.x, msg->pose.position.y); world_.ball.vel = ateam_geometry::Vector(msg->twist.linear.x, msg->twist.linear.y); world_.ball.visible = msg->visible; - // TODO(barulicm): Update remaining Ball fields + if(world_.ball.visible) { + world_.ball.last_visible_time = std::chrono::steady_clock::now(); + } } void UpdateBotFromVision(Robot & robot, const ateam_msgs::msg::VisionStateRobot::SharedPtr msg) From baa5e3f06f0503ff92fe63467f840f72765760d8 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 22:47:02 -0400 Subject: [PATCH 13/19] Adds more robot state subscribers to game_state_tracker --- .../ateam_game_state/CMakeLists.txt | 2 + state_tracking/ateam_game_state/package.xml | 1 + .../game_state_tracker/game_state_tracker.cpp | 49 +++++++++++++++++++ 3 files changed, 52 insertions(+) diff --git a/state_tracking/ateam_game_state/CMakeLists.txt b/state_tracking/ateam_game_state/CMakeLists.txt index c9f950b2a..d9c9d4254 100644 --- a/state_tracking/ateam_game_state/CMakeLists.txt +++ b/state_tracking/ateam_game_state/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(rclcpp_components REQUIRED) find_package(ateam_common REQUIRED) find_package(ateam_geometry REQUIRED) find_package(ateam_msgs REQUIRED) +find_package(ateam_radio_msgs REQUIRED) find_package(ssl_league_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -48,6 +49,7 @@ ament_target_dependencies( ateam_common ateam_geometry ateam_msgs + ateam_radio_msgs ssl_league_msgs ) target_link_libraries(game_state_tracker_component ${PROJECT_NAME}) diff --git a/state_tracking/ateam_game_state/package.xml b/state_tracking/ateam_game_state/package.xml index f40b1500b..c5ee69d45 100644 --- a/state_tracking/ateam_game_state/package.xml +++ b/state_tracking/ateam_game_state/package.xml @@ -14,6 +14,7 @@ ateam_common ateam_geometry ateam_msgs + ateam_radio_msgs ssl_league_msgs tf2 tf2_geometry_msgs diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index 1fa915603..b5fcc7cee 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -22,8 +22,11 @@ #include #include #include +#include #include #include +#include +#include #include #include #include @@ -75,6 +78,27 @@ class GameStateTracker : public rclcpp::Node { &GameStateTracker::YellowVisionBotCallback, this); + create_indexed_subscribers( + robot_feedback_subs_, + Topics::kRobotFeedbackPrefix, + 1, + &GameStateTracker::RobotFeedbackCallback, + this); + + create_indexed_subscribers( + robot_connection_status_subs_, + Topics::kRobotConnectionStatusPrefix, + 1, + &GameStateTracker::RobotConnectionCallback, + this); + + create_indexed_subscribers( + robot_command_subs_, + Topics::kRobotMotionCommandPrefix, + 1, + &GameStateTracker::RobotCommandCallback, + this); + timer_ = create_wall_timer(std::chrono::duration(1.0 / declare_parameter("update_frequency")), std::bind(&GameStateTracker::TimerCallback, this)); @@ -90,6 +114,12 @@ class GameStateTracker : public rclcpp::Node { ateam_common::indexed_topic_helpers::kRobotCount> blue_bot_subs_; std::array::SharedPtr, ateam_common::indexed_topic_helpers::kRobotCount> yellow_bot_subs_; + std::array::SharedPtr, + 16> robot_feedback_subs_; + std::array::SharedPtr, + 16> robot_connection_status_subs_; + std::array::SharedPtr, + 16> robot_command_subs_; ateam_common::GameControllerListener gc_listener_; // TODO(barulicm): Move GCListener type to ateam_game_state package World world_; rclcpp::TimerBase::SharedPtr timer_; @@ -158,6 +188,25 @@ class GameStateTracker : public rclcpp::Node { UpdateBotFromVision(robot_state_array[id], msg); } + void RobotFeedbackCallback(const ateam_radio_msgs::msg::BasicTelemetry::SharedPtr msg, int id) + { + world_.our_robots[id].breakbeam_ball_detected = msg->breakbeam_ball_detected; + world_.our_robots[id].kicker_available = msg->kicker_available; + world_.our_robots[id].chipper_available = msg->chipper_available; + } + + void RobotConnectionCallback(const ateam_radio_msgs::msg::ConnectionStatus::SharedPtr msg, int id) + { + world_.our_robots[id].radio_connected = msg->radio_connected; + } + + void RobotCommandCallback(const ateam_msgs::msg::RobotMotionCommand::SharedPtr msg, int id) + { + world_.our_robots[id].prev_command_vel = ateam_geometry::Vector(msg->twist.linear.x, + msg->twist.linear.y); + world_.our_robots[id].prev_command_omega = msg->twist.angular.z; + } + void UpdateRefInfo() { world_.referee_info.our_goalie_id = gc_listener_.GetOurGoalieID().value_or(-1); From 5b534c526ad0d3606cd1afa62438052bf41be38b Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 23:20:36 -0400 Subject: [PATCH 14/19] Swaps out kenobi state tracking code for a subscription to the new world topic. --- ateam_kenobi/CMakeLists.txt | 3 +- ateam_kenobi/package.xml | 1 + ateam_kenobi/src/core/ballsense_emulator.hpp | 2 +- ateam_kenobi/src/core/ballsense_filter.hpp | 2 +- .../src/core/defense_area_enforcement.hpp | 2 +- ateam_kenobi/src/core/double_touch_eval.hpp | 2 +- ateam_kenobi/src/core/fps_tracker.hpp | 2 +- ateam_kenobi/src/core/in_play_eval.hpp | 2 +- .../src/core/motion/motion_controller.hpp | 4 +- .../src/core/motion/world_to_body_vel.hpp | 2 +- .../core/path_planning/escape_velocity.hpp | 2 +- .../src/core/path_planning/obstacles.cpp | 4 +- .../src/core/path_planning/obstacles.hpp | 2 +- .../src/core/path_planning/path_planner.hpp | 2 +- .../core/play_helpers/available_robots.hpp | 4 +- .../src/core/play_helpers/easy_move_to.hpp | 4 +- ateam_kenobi/src/core/play_helpers/lanes.hpp | 2 +- .../src/core/play_helpers/possession.cpp | 2 +- .../src/core/play_helpers/possession.hpp | 2 +- .../core/play_helpers/robot_assignment.hpp | 2 +- .../src/core/play_helpers/shot_evaluation.hpp | 2 +- .../core/play_helpers/window_evaluation.hpp | 2 +- ateam_kenobi/src/core/play_selector.hpp | 2 +- ateam_kenobi/src/core/stp/play.hpp | 2 +- .../src/core/{types/ball.hpp => types.hpp} | 27 +- ateam_kenobi/src/core/types/field.hpp | 50 --- .../src/core/types/message_conversions.cpp | 149 --------- .../src/core/types/message_conversions.hpp | 42 --- ateam_kenobi/src/core/types/referee_info.hpp | 45 --- ateam_kenobi/src/core/types/robot.hpp | 56 ---- ateam_kenobi/src/core/types/world.hpp | 60 ---- ateam_kenobi/src/kenobi_node.cpp | 284 +----------------- .../offense/kickoff_pass_play.cpp | 2 +- .../segment_passing_target_funcs.hpp | 2 +- .../src/plays/stop_plays/stop_helpers.hpp | 2 +- .../plays/test_plays/controls_test_play.cpp | 2 +- .../plays/test_plays/test_intercept_play.cpp | 2 +- .../src/plays/test_plays/test_pass_play.cpp | 2 +- .../src/plays/test_plays/test_play.cpp | 2 +- .../plays/util_plays/corner_lineup_play.cpp | 2 +- .../plays/util_plays/corner_lineup_play.hpp | 2 +- ateam_kenobi/src/plays/wall_play.cpp | 2 +- ateam_kenobi/src/plays/wall_play.hpp | 2 +- ateam_kenobi/src/skills/capture.hpp | 2 +- ateam_kenobi/src/skills/dribble.hpp | 2 +- ateam_kenobi/src/skills/goalie.hpp | 2 +- ateam_kenobi/src/skills/intercept.hpp | 2 +- ateam_kenobi/src/skills/line_kick.hpp | 2 +- ateam_kenobi/src/skills/pass_receiver.hpp | 2 +- ateam_kenobi/src/skills/pivot_kick.hpp | 2 +- ateam_kenobi/src/tactics/blockers.hpp | 2 +- ateam_kenobi/src/tactics/defenders.hpp | 4 +- ateam_kenobi/src/tactics/pass.hpp | 2 +- ateam_kenobi/src/tactics/standard_defense.hpp | 4 +- ateam_kenobi/test/path_planner_test.cpp | 4 +- ateam_kenobi/test/robot_assignment_test.cpp | 19 +- 56 files changed, 90 insertions(+), 750 deletions(-) rename ateam_kenobi/src/core/{types/ball.hpp => types.hpp} (69%) delete mode 100644 ateam_kenobi/src/core/types/field.hpp delete mode 100644 ateam_kenobi/src/core/types/message_conversions.cpp delete mode 100644 ateam_kenobi/src/core/types/message_conversions.hpp delete mode 100644 ateam_kenobi/src/core/types/referee_info.hpp delete mode 100644 ateam_kenobi/src/core/types/robot.hpp delete mode 100644 ateam_kenobi/src/core/types/world.hpp diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index f5ceebb31..68c6557b8 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp_components REQUIRED) find_package(ateam_msgs REQUIRED) find_package(ateam_radio_msgs REQUIRED) find_package(ateam_common REQUIRED) +find_package(ateam_game_state REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(ateam_geometry REQUIRED) @@ -34,7 +35,6 @@ target_sources(kenobi_node_component PRIVATE src/core/path_planning/escape_velocity.cpp src/core/path_planning/obstacles.cpp src/core/path_planning/path_planner.cpp - src/core/types/message_conversions.cpp src/core/motion/motion_controller.cpp src/core/visualization/overlays.cpp src/core/play_helpers/easy_move_to.cpp @@ -122,6 +122,7 @@ ament_target_dependencies( "ateam_radio_msgs" "ateam_msgs" "ateam_common" + "ateam_game_state" "tf2" "tf2_geometry_msgs" "ateam_geometry" diff --git a/ateam_kenobi/package.xml b/ateam_kenobi/package.xml index 935055cce..ecae1cc27 100644 --- a/ateam_kenobi/package.xml +++ b/ateam_kenobi/package.xml @@ -14,6 +14,7 @@ ateam_msgs ateam_radio_msgs ateam_common + ateam_game_state tf2 tf2_geometry_msgs ateam_geometry diff --git a/ateam_kenobi/src/core/ballsense_emulator.hpp b/ateam_kenobi/src/core/ballsense_emulator.hpp index 68670378b..c73710fd3 100644 --- a/ateam_kenobi/src/core/ballsense_emulator.hpp +++ b/ateam_kenobi/src/core/ballsense_emulator.hpp @@ -27,7 +27,7 @@ #include #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/ballsense_filter.hpp b/ateam_kenobi/src/core/ballsense_filter.hpp index acb7b4184..70d83e218 100644 --- a/ateam_kenobi/src/core/ballsense_filter.hpp +++ b/ateam_kenobi/src/core/ballsense_filter.hpp @@ -24,7 +24,7 @@ #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/defense_area_enforcement.hpp b/ateam_kenobi/src/core/defense_area_enforcement.hpp index bc913eac7..4dd68f543 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.hpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.hpp @@ -22,7 +22,7 @@ #define CORE__DEFENSE_AREA_ENFORCEMENT_HPP_ #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::defense_area_enforcement { diff --git a/ateam_kenobi/src/core/double_touch_eval.hpp b/ateam_kenobi/src/core/double_touch_eval.hpp index f65cb4a3f..cfacd863e 100644 --- a/ateam_kenobi/src/core/double_touch_eval.hpp +++ b/ateam_kenobi/src/core/double_touch_eval.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/visualization/overlays.hpp" namespace ateam_kenobi diff --git a/ateam_kenobi/src/core/fps_tracker.hpp b/ateam_kenobi/src/core/fps_tracker.hpp index ecbafa25c..18ab9a472 100644 --- a/ateam_kenobi/src/core/fps_tracker.hpp +++ b/ateam_kenobi/src/core/fps_tracker.hpp @@ -23,7 +23,7 @@ #define CORE__FPS_TRACKER_HPP_ #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/in_play_eval.hpp b/ateam_kenobi/src/core/in_play_eval.hpp index 7cab8a3af..a2d0aa2ff 100644 --- a/ateam_kenobi/src/core/in_play_eval.hpp +++ b/ateam_kenobi/src/core/in_play_eval.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/motion/motion_controller.hpp b/ateam_kenobi/src/core/motion/motion_controller.hpp index 6dc80f719..5e53ddcef 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -27,8 +27,8 @@ #include #include #include -#include "core/types/robot.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" +#include "core/types.hpp" #include "pid.hpp" // cause the robot to: always face a point, face in the direction of travel, or stay facing the diff --git a/ateam_kenobi/src/core/motion/world_to_body_vel.hpp b/ateam_kenobi/src/core/motion/world_to_body_vel.hpp index f4ab7ea75..c0cc6ce83 100644 --- a/ateam_kenobi/src/core/motion/world_to_body_vel.hpp +++ b/ateam_kenobi/src/core/motion/world_to_body_vel.hpp @@ -25,7 +25,7 @@ #include #include #include -#include "core/types/robot.hpp" +#include "core/types.hpp" namespace ateam_kenobi::motion { diff --git a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp index 6045002fd..c910879dc 100644 --- a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp +++ b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp @@ -25,7 +25,7 @@ #include #include #include -#include "core/types/robot.hpp" +#include "core/types.hpp" namespace ateam_kenobi::path_planning { diff --git a/ateam_kenobi/src/core/path_planning/obstacles.cpp b/ateam_kenobi/src/core/path_planning/obstacles.cpp index 7d202fe82..c10718bd5 100644 --- a/ateam_kenobi/src/core/path_planning/obstacles.cpp +++ b/ateam_kenobi/src/core/path_planning/obstacles.cpp @@ -171,11 +171,11 @@ bool IsPointInBounds( ateam_geometry::Rectangle pathable_region(ateam_geometry::Point(-x_bound, -y_bound), ateam_geometry::Point(x_bound, y_bound)); - if (world.ignore_side > 0) { + if (world.field.ignore_side > 0) { pathable_region = ateam_geometry::Rectangle( ateam_geometry::Point(-x_bound, -y_bound), ateam_geometry::Point(0, y_bound)); - } else if (world.ignore_side < 0) { + } else if (world.field.ignore_side < 0) { pathable_region = ateam_geometry::Rectangle( ateam_geometry::Point(0, y_bound), ateam_geometry::Point(x_bound, -y_bound)); diff --git a/ateam_kenobi/src/core/path_planning/obstacles.hpp b/ateam_kenobi/src/core/path_planning/obstacles.hpp index 61b5f0689..dcad2670d 100644 --- a/ateam_kenobi/src/core/path_planning/obstacles.hpp +++ b/ateam_kenobi/src/core/path_planning/obstacles.hpp @@ -24,7 +24,7 @@ #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::path_planning { diff --git a/ateam_kenobi/src/core/path_planning/path_planner.hpp b/ateam_kenobi/src/core/path_planning/path_planner.hpp index 63dba7f01..f1ae42d3b 100644 --- a/ateam_kenobi/src/core/path_planning/path_planner.hpp +++ b/ateam_kenobi/src/core/path_planning/path_planner.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/stp/base.hpp" namespace ateam_kenobi::path_planning diff --git a/ateam_kenobi/src/core/play_helpers/available_robots.hpp b/ateam_kenobi/src/core/play_helpers/available_robots.hpp index 1c3d6673b..e1209b620 100644 --- a/ateam_kenobi/src/core/play_helpers/available_robots.hpp +++ b/ateam_kenobi/src/core/play_helpers/available_robots.hpp @@ -24,8 +24,8 @@ #include #include -#include "core/types/robot.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" +#include "core/types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp index b2c7c68dd..124f7a1b6 100644 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp +++ b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp @@ -30,8 +30,8 @@ #include "core/path_planning/path_planner.hpp" #include "core/motion/motion_controller.hpp" #include "core/stp/base.hpp" -#include "core/types/robot.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" +#include "core/types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/lanes.hpp b/ateam_kenobi/src/core/play_helpers/lanes.hpp index f33440bda..4c42a86c5 100644 --- a/ateam_kenobi/src/core/play_helpers/lanes.hpp +++ b/ateam_kenobi/src/core/play_helpers/lanes.hpp @@ -22,7 +22,7 @@ #define CORE__PLAY_HELPERS__LANES_HPP_ #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::play_helpers::lanes { diff --git a/ateam_kenobi/src/core/play_helpers/possession.cpp b/ateam_kenobi/src/core/play_helpers/possession.cpp index 34675cd77..21a2518ae 100644 --- a/ateam_kenobi/src/core/play_helpers/possession.cpp +++ b/ateam_kenobi/src/core/play_helpers/possession.cpp @@ -23,7 +23,7 @@ #include #include #include -#include "core/types/robot.hpp" +#include "core/types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/possession.hpp b/ateam_kenobi/src/core/play_helpers/possession.hpp index 87dc09abe..242570254 100644 --- a/ateam_kenobi/src/core/play_helpers/possession.hpp +++ b/ateam_kenobi/src/core/play_helpers/possession.hpp @@ -21,7 +21,7 @@ #ifndef CORE__PLAY_HELPERS__POSSESSION_HPP_ #define CORE__PLAY_HELPERS__POSSESSION_HPP_ -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp b/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp index efa161bb3..093fe69fc 100644 --- a/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp +++ b/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types/robot.hpp" +#include "core/types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp b/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp index 8da1d7385..25f391424 100644 --- a/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp +++ b/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp @@ -23,7 +23,7 @@ #define CORE__PLAY_HELPERS__SHOT_EVALUATION_HPP_ #include -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp b/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp index 53c0388c8..7ad60021e 100644 --- a/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp +++ b/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types/robot.hpp" +#include "core/types.hpp" #include "core/visualization/overlays.hpp" namespace ateam_kenobi::play_helpers::window_evaluation diff --git a/ateam_kenobi/src/core/play_selector.hpp b/ateam_kenobi/src/core/play_selector.hpp index a72370a2e..3e6166274 100644 --- a/ateam_kenobi/src/core/play_selector.hpp +++ b/ateam_kenobi/src/core/play_selector.hpp @@ -29,7 +29,7 @@ #include #include #include "core/stp/play.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/stp/play.hpp b/ateam_kenobi/src/core/stp/play.hpp index 47bdde3db..0138e9e93 100644 --- a/ateam_kenobi/src/core/stp/play.hpp +++ b/ateam_kenobi/src/core/stp/play.hpp @@ -28,7 +28,7 @@ #include #include "base.hpp" #include "play_score.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::stp { diff --git a/ateam_kenobi/src/core/types/ball.hpp b/ateam_kenobi/src/core/types.hpp similarity index 69% rename from ateam_kenobi/src/core/types/ball.hpp rename to ateam_kenobi/src/core/types.hpp index ceb70e569..d0fd27eb9 100644 --- a/ateam_kenobi/src/core/types/ball.hpp +++ b/ateam_kenobi/src/core/types.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 A Team +// 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 @@ -19,21 +19,22 @@ // THE SOFTWARE. -#ifndef CORE__TYPES__BALL_HPP_ -#define CORE__TYPES__BALL_HPP_ +#ifndef CORE__TYPES_HPP_ +#define CORE__TYPES_HPP_ -#include -#include +#include +#include +#include +#include +#include namespace ateam_kenobi { -struct Ball -{ - ateam_geometry::Point pos; - ateam_geometry::Vector vel; - bool visible = false; - std::chrono::steady_clock::time_point last_visible_time; -}; +using Ball = ateam_game_state::Ball; +using Field = ateam_game_state::Field; +using RefereeInfo = ateam_game_state::RefereeInfo; +using Robot = ateam_game_state::Robot; +using World = ateam_game_state::World; } // namespace ateam_kenobi -#endif // CORE__TYPES__BALL_HPP_ +#endif // CORE__TYPES_HPP_ diff --git a/ateam_kenobi/src/core/types/field.hpp b/ateam_kenobi/src/core/types/field.hpp deleted file mode 100644 index 6ad998d4e..000000000 --- a/ateam_kenobi/src/core/types/field.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2021 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 CORE__TYPES__FIELD_HPP_ -#define CORE__TYPES__FIELD_HPP_ - -#include -#include - -namespace ateam_kenobi -{ -struct FieldSidedInfo -{ - std::array defense_area_corners; - std::array goal_corners; - std::array goal_posts; -}; -struct Field -{ - float field_length = 0.f; - float field_width = 0.f; - float goal_width = 0.f; - float goal_depth = 0.f; - float boundary_width = 0.f; - float defense_area_width = 0.f; - float defense_area_depth = 0.f; - std::array field_corners; - FieldSidedInfo ours; - FieldSidedInfo theirs; -}; -} // namespace ateam_kenobi - -#endif // CORE__TYPES__FIELD_HPP_ diff --git a/ateam_kenobi/src/core/types/message_conversions.cpp b/ateam_kenobi/src/core/types/message_conversions.cpp deleted file mode 100644 index 90d57f676..000000000 --- a/ateam_kenobi/src/core/types/message_conversions.cpp +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2021 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 "core/types/message_conversions.hpp" - -#include -#include - -#include "core/types/field.hpp" -#include "core/types/referee_info.hpp" - -namespace ateam_kenobi::message_conversions -{ -ateam_msgs::msg::FieldInfo toMsg(const Field & obj) -{ - ateam_msgs::msg::FieldInfo field_msg; - field_msg.field_length = obj.field_length; - field_msg.field_width = obj.field_width; - field_msg.goal_width = obj.goal_width; - field_msg.goal_depth = obj.goal_depth; - field_msg.boundary_width = obj.boundary_width; - - auto convert_point_array = [&](auto & starting_array, auto final_array_iter) { - std::transform( - starting_array.begin(), starting_array.end(), final_array_iter, - [&](auto & val)->geometry_msgs::msg::Point32 { - return geometry_msgs::build().x(val.x()).y(val.y()).z(0); - }); - }; - - convert_point_array(obj.field_corners, std::back_inserter(field_msg.field_corners.points)); - convert_point_array( - obj.ours.defense_area_corners, - std::back_inserter(field_msg.ours.goal_corners.points)); - convert_point_array( - obj.ours.goal_corners, - std::back_inserter(field_msg.ours.defense_area_corners.points)); - convert_point_array( - obj.theirs.defense_area_corners, - std::back_inserter(field_msg.theirs.goal_corners.points)); - convert_point_array( - obj.theirs.goal_corners, - std::back_inserter(field_msg.theirs.defense_area_corners.points)); - - return field_msg; -} - -ateam_msgs::msg::RefereeInfo toMsg(const RefereeInfo & obj) -{ - ateam_msgs::msg::RefereeInfo ref_msg; - ref_msg.our_goalie_id = obj.our_goalie_id; - ref_msg.their_goalie_id = obj.their_goalie_id; - ref_msg.game_stage = static_cast(obj.current_game_stage); - ref_msg.game_command = static_cast(obj.running_command); - ref_msg.prev_command = static_cast(obj.prev_command); - - if (obj.designated_position.has_value()) { - ref_msg.designated_position.x = obj.designated_position->x(); - ref_msg.designated_position.y = obj.designated_position->y(); - } else { - ref_msg.designated_position.x = 0.0f; - ref_msg.designated_position.y = 0.0f; - } - - return ref_msg; -} - - -ateam_msgs::msg::GameStateBall toMsg(const Ball & obj) -{ - ateam_msgs::msg::GameStateBall ball_state_msg; - ball_state_msg.pose.position.x = obj.pos.x(); - ball_state_msg.pose.position.y = obj.pos.y(); - ball_state_msg.twist.linear.x = obj.vel.x(); - ball_state_msg.twist.linear.y = obj.vel.y(); - ball_state_msg.visible = obj.visible; - - return ball_state_msg; -} - -ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj) -{ - ateam_msgs::msg::GameStateRobot robot_state_msg; - robot_state_msg.pose.position.x = obj.pos.x(); - robot_state_msg.pose.position.y = obj.pos.y(); - robot_state_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), obj.theta)); - robot_state_msg.twist.linear.x = obj.vel.x(); - robot_state_msg.twist.linear.y = obj.vel.y(); - robot_state_msg.twist.angular.z = obj.omega; - robot_state_msg.visible = obj.visible; - - return robot_state_msg; -} - -ateam_msgs::msg::GameStateWorld toMsg(const World & obj) -{ - ateam_msgs::msg::GameStateWorld world_msg; - - world_msg.current_time = - rclcpp::Time( - std::chrono::duration_cast( - obj.current_time.time_since_epoch()).count()); - - world_msg.field = toMsg(obj.field); - world_msg.referee_info = toMsg(obj.referee_info); - - world_msg.ball = toMsg(obj.ball); - - for (const Robot & robot : obj.our_robots) { - if (robot.visible || robot.radio_connected) { - world_msg.our_robots.push_back(toMsg(robot)); - } else { - world_msg.our_robots.push_back(ateam_msgs::msg::GameStateRobot()); - } - } - - for (const Robot & robot : obj.their_robots) { - if (robot.visible) { - world_msg.their_robots.push_back(toMsg(robot)); - } else { - world_msg.their_robots.push_back(ateam_msgs::msg::GameStateRobot()); - } - } - - world_msg.ball_in_play = obj.in_play; - world_msg.double_touch_enforced = obj.double_touch_forbidden_id_.has_value(); - world_msg.double_touch_id = obj.double_touch_forbidden_id_.value_or(-1); - - return world_msg; -} - -} // namespace ateam_kenobi::message_conversions diff --git a/ateam_kenobi/src/core/types/message_conversions.hpp b/ateam_kenobi/src/core/types/message_conversions.hpp deleted file mode 100644 index 5413438de..000000000 --- a/ateam_kenobi/src/core/types/message_conversions.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2021 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 CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ -#define CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ - -#include -#include -#include - -#include "core/types/ball.hpp" -#include "core/types/robot.hpp" -#include "core/types/world.hpp" - - -namespace ateam_kenobi::message_conversions -{ -ateam_msgs::msg::GameStateBall toMsg(const Ball & obj); -ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj); -ateam_msgs::msg::GameStateWorld toMsg(const World & obj); - -} // namespace ateam_kenobi::message_conversions - -#endif // CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ diff --git a/ateam_kenobi/src/core/types/referee_info.hpp b/ateam_kenobi/src/core/types/referee_info.hpp deleted file mode 100644 index cc39cd3d0..000000000 --- a/ateam_kenobi/src/core/types/referee_info.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2021 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 CORE__TYPES__REFEREE_INFO_HPP_ -#define CORE__TYPES__REFEREE_INFO_HPP_ - -#include -#include -#include -#include - -namespace ateam_kenobi -{ -struct RefereeInfo -{ - int our_goalie_id = -1; - int their_goalie_id = -1; - ateam_common::GameStage current_game_stage = ateam_common::GameStage::Unknown; - ateam_common::GameCommand running_command = ateam_common::GameCommand::Halt; - ateam_common::GameCommand prev_command = ateam_common::GameCommand::Halt; - std::optional designated_position; - std::optional next_command = std::nullopt; - std::chrono::system_clock::time_point command_time; -}; -} // namespace ateam_kenobi - -#endif // CORE__TYPES__REFEREE_INFO_HPP_ diff --git a/ateam_kenobi/src/core/types/robot.hpp b/ateam_kenobi/src/core/types/robot.hpp deleted file mode 100644 index 1b9690a1b..000000000 --- a/ateam_kenobi/src/core/types/robot.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2021 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 CORE__TYPES__ROBOT_HPP_ -#define CORE__TYPES__ROBOT_HPP_ - -#include - -namespace ateam_kenobi -{ -struct Robot -{ - int id; - bool visible = false; - bool radio_connected = false; - - ateam_geometry::Point pos; - double theta = 0.0; - ateam_geometry::Vector vel; - double omega = 0.0; - - ateam_geometry::Vector prev_command_vel; - double prev_command_omega = 0.0; - - bool breakbeam_ball_detected = false; - bool breakbeam_ball_detected_filtered = false; - - bool kicker_available = true; - bool chipper_available = false; - - bool IsAvailable() const - { - return visible && radio_connected; - } -}; -} // namespace ateam_kenobi - -#endif // CORE__TYPES__ROBOT_HPP_ diff --git a/ateam_kenobi/src/core/types/world.hpp b/ateam_kenobi/src/core/types/world.hpp deleted file mode 100644 index aa5c1e8c4..000000000 --- a/ateam_kenobi/src/core/types/world.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2021 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 CORE__TYPES__WORLD_HPP_ -#define CORE__TYPES__WORLD_HPP_ - -#include -#include -#include - -#include "core/types/ball.hpp" -#include "core/types/field.hpp" -#include "core/types/referee_info.hpp" -#include "core/types/robot.hpp" - -namespace ateam_kenobi -{ -struct World -{ - std::chrono::steady_clock::time_point current_time; - - Field field; - RefereeInfo referee_info; - - Ball ball; - std::array our_robots; - std::array their_robots; - - bool in_play = false; - bool our_penalty = false; - bool their_penalty = false; - - int ignore_side = 0; - - double fps = 100.0; - - // Holds the ID of the robot not allowed to touch the ball, if any - std::optional double_touch_forbidden_id_; -}; -} // namespace ateam_kenobi - -#endif // CORE__TYPES__WORLD_HPP_ diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 85db050b1..e6ea8ea6b 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -25,11 +25,6 @@ #include #include #include -#include -#include -#include -#include -#include #include #include #include @@ -44,8 +39,7 @@ #include #include #include -#include "core/types/world.hpp" -#include "core/types/message_conversions.hpp" +#include "core/types.hpp" #include "core/play_selector.hpp" #include "core/in_play_eval.hpp" #include "core/double_touch_eval.hpp" @@ -71,11 +65,8 @@ class KenobiNode : public rclcpp::Node : rclcpp::Node("kenobi_node", options), play_selector_(*this), joystick_enforcer_(*this), - overlays_(""), - game_controller_listener_(*this) + overlays_("") { - initialize_robot_ids(); - declare_parameter("use_world_velocities", false); declare_parameter("use_emulated_ballsense", false); @@ -87,52 +78,10 @@ class KenobiNode : public rclcpp::Node "/play_info", rclcpp::SystemDefaultsQoS()); - create_indexed_subscribers( - blue_robots_subscriptions_, - Topics::kBlueTeamRobotPrefix, - 10, - &KenobiNode::blue_robot_state_callback, - this); - - create_indexed_subscribers( - yellow_robots_subscriptions_, - Topics::kYellowTeamRobotPrefix, - 10, - &KenobiNode::yellow_robot_state_callback, - this); - - create_indexed_subscribers( - robot_feedback_subscriptions_, - Topics::kRobotFeedbackPrefix, - 10, - &KenobiNode::robot_feedback_callback, - this); - - create_indexed_subscribers( - robot_connection_status_subscriptions_, - Topics::kRobotConnectionStatusPrefix, - 10, - &KenobiNode::robot_connection_callback, - this); - create_indexed_publishers( robot_commands_publishers_, Topics::kRobotMotionCommandPrefix, rclcpp::SystemDefaultsQoS(), this); - ball_subscription_ = create_subscription( - std::string(Topics::kBall), - 10, - std::bind(&KenobiNode::ball_state_callback, this, std::placeholders::_1)); - - world_publisher_ = create_publisher( - "~/world", - rclcpp::SystemDefaultsQoS()); - - field_subscription_ = create_subscription( - std::string(Topics::kField), - 10, - std::bind(&KenobiNode::field_callback, this, std::placeholders::_1)); - override_service_ = create_service( "~/set_override_play", std::bind( &KenobiNode::set_override_play_callback, this, std::placeholders::_1, @@ -150,7 +99,9 @@ class KenobiNode : public rclcpp::Node status_publisher_ = create_publisher("~/status", rclcpp::SystemDefaultsQoS()); - timer_ = create_wall_timer(10ms, std::bind(&KenobiNode::timer_callback, this)); + world_subscription_ = create_subscription( + std::string(Topics::kWorld), rclcpp::SystemDefaultsQoS(), + std::bind(&KenobiNode::WorldCallback, this, std::placeholders::_1)); const auto playbook_path = declare_parameter("playbook", ""); const auto autosave_playbook_path = getCacheDirectory() / "playbook/autosave.json"; @@ -174,7 +125,6 @@ class KenobiNode : public rclcpp::Node } private: - World world_; PlaySelector play_selector_; InPlayEval in_play_eval_; DoubleTouchEval double_touch_eval_; @@ -186,159 +136,13 @@ class KenobiNode : public rclcpp::Node FpsTracker fps_tracker_; rclcpp::Publisher::SharedPtr overlay_publisher_; rclcpp::Publisher::SharedPtr play_info_publisher_; - rclcpp::Subscription::SharedPtr ball_subscription_; - std::array::SharedPtr, - 16> blue_robots_subscriptions_; - std::array::SharedPtr, - 16> yellow_robots_subscriptions_; - std::array::SharedPtr, - 16> robot_feedback_subscriptions_; - std::array::SharedPtr, - 16> robot_connection_status_subscriptions_; - rclcpp::Subscription::SharedPtr - field_subscription_; std::array::SharedPtr, 16> robot_commands_publishers_; rclcpp::Service::SharedPtr override_service_; rclcpp::Service::SharedPtr play_enabled_service_; rclcpp::Publisher::SharedPtr playbook_state_publisher_; rclcpp::Publisher::SharedPtr status_publisher_; - - ateam_common::GameControllerListener game_controller_listener_; - - rclcpp::Publisher::SharedPtr world_publisher_; - - rclcpp::TimerBase::SharedPtr timer_; - - void initialize_robot_ids() - { - for(auto i = 0u; i < world_.our_robots.size(); ++i) { - world_.our_robots[i].id = i; - } - for(auto i = 0u; i < world_.their_robots.size(); ++i) { - world_.their_robots[i].id = i; - } - } - - void blue_robot_state_callback( - const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg, - int id) - { - const auto our_color = game_controller_listener_.GetTeamColor(); - if(our_color == ateam_common::TeamColor::Unknown) { - return; - } - const auto are_we_blue = our_color == ateam_common::TeamColor::Blue; - auto & robot_state_array = are_we_blue ? world_.our_robots : world_.their_robots; - robot_state_callback(robot_state_array, id, robot_state_msg); - } - - void yellow_robot_state_callback( - const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg, - int id) - { - const auto our_color = game_controller_listener_.GetTeamColor(); - if(our_color == ateam_common::TeamColor::Unknown) { - return; - } - const auto are_we_yellow = our_color == ateam_common::TeamColor::Yellow; - auto & robot_state_array = are_we_yellow ? world_.our_robots : world_.their_robots; - robot_state_callback(robot_state_array, id, robot_state_msg); - } - - void robot_state_callback( - std::array & robot_states, - std::size_t id, - const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg) - { - robot_states.at(id).visible = robot_state_msg->visible; - if (robot_state_msg->visible) { - robot_states.at(id).pos = ateam_geometry::Point( - robot_state_msg->pose.position.x, - robot_state_msg->pose.position.y); - tf2::Quaternion tf2_quat; - tf2::fromMsg(robot_state_msg->pose.orientation, tf2_quat); - robot_states.at(id).theta = tf2::getYaw(tf2_quat); - robot_states.at(id).vel = ateam_geometry::Vector( - robot_state_msg->twist.linear.x, - robot_state_msg->twist.linear.y); - robot_states.at(id).omega = robot_state_msg->twist.angular.z; - robot_states.at(id).id = id; - } - } - - void robot_feedback_callback( - const ateam_radio_msgs::msg::BasicTelemetry::SharedPtr robot_feedback_msg, - int id) - { - world_.our_robots.at(id).breakbeam_ball_detected = robot_feedback_msg->breakbeam_ball_detected; - world_.our_robots.at(id).kicker_available = robot_feedback_msg->kicker_available; - world_.our_robots.at(id).chipper_available = robot_feedback_msg->chipper_available; - } - - void robot_connection_callback( - const ateam_radio_msgs::msg::ConnectionStatus::SharedPtr robot_connection_msg, - int id) - { - world_.our_robots.at(id).radio_connected = robot_connection_msg->radio_connected; - } - - void ball_state_callback(const ateam_msgs::msg::VisionStateBall::SharedPtr ball_state_msg) - { - world_.ball.visible = ball_state_msg->visible; - const auto ball_visibility_timed_out = ateam_common::TimeDiffSeconds(world_.current_time, - world_.ball.last_visible_time) > 0.5; - if (ball_state_msg->visible) { - world_.ball.pos = ateam_geometry::Point( - ball_state_msg->pose.position.x, - ball_state_msg->pose.position.y); - world_.ball.vel = ateam_geometry::Vector( - ball_state_msg->twist.linear.x, - ball_state_msg->twist.linear.y); - world_.ball.last_visible_time = world_.current_time; - } else if(ball_visibility_timed_out) { - world_.ball.vel = ateam_geometry::Vector{0.0, 0.0}; - } - } - - void field_callback(const ateam_msgs::msg::FieldInfo::SharedPtr field_msg) - { - Field field; - field.field_length = field_msg->field_length; - field.field_width = field_msg->field_width; - field.goal_width = field_msg->goal_width; - field.goal_depth = field_msg->goal_depth; - field.boundary_width = field_msg->boundary_width; - field.defense_area_depth = field_msg->defense_area_depth; - field.defense_area_width = field_msg->defense_area_width; - - auto convert_point_array = [&](auto & starting_array, auto final_array_iter) { - std::transform( - starting_array.begin(), starting_array.end(), final_array_iter, - [&](auto & val) { - return ateam_geometry::Point(val.x, val.y); - }); - }; - - convert_point_array(field_msg->field_corners.points, field.field_corners.begin()); - convert_point_array( - field_msg->ours.defense_area_corners.points, - field.ours.defense_area_corners.begin()); - convert_point_array(field_msg->ours.goal_corners.points, field.ours.goal_corners.begin()); - convert_point_array( - field_msg->theirs.defense_area_corners.points, - field.theirs.defense_area_corners.begin()); - convert_point_array(field_msg->theirs.goal_corners.points, field.theirs.goal_corners.begin()); - field.ours.goal_posts = {field.ours.goal_corners[0], field.ours.goal_corners[1]}; - field.theirs.goal_posts = {field.theirs.goal_corners[0], field.theirs.goal_corners[1]}; - - world_.field = field; - if (game_controller_listener_.GetTeamSide() == ateam_common::TeamSide::PositiveHalf) { - world_.ignore_side = -field_msg->ignore_side; - } else { - world_.ignore_side = field_msg->ignore_side; - } - } + rclcpp::Subscription::SharedPtr world_subscription_; void set_override_play_callback( const ateam_msgs::srv::SetOverridePlay::Request::SharedPtr request, @@ -377,77 +181,22 @@ class KenobiNode : public rclcpp::Node response->success = true; } - void timer_callback() + void WorldCallback(const ateam_game_state::World & world) { - world_.current_time = std::chrono::steady_clock::now(); - world_.referee_info.running_command = game_controller_listener_.GetGameCommand(); - const auto & ref_msg = game_controller_listener_.GetLatestRefereeMessage(); - world_.referee_info.command_time = - std::chrono::system_clock::time_point( - std::chrono::nanoseconds( - rclcpp::Time(ref_msg.command_timestamp).nanoseconds())); - world_.referee_info.prev_command = game_controller_listener_.GetPreviousGameCommand(); - world_.referee_info.next_command = game_controller_listener_.GetNextGameCommand(); - world_.referee_info.current_game_stage = game_controller_listener_.GetGameStage(); - - const auto & gc_designated_position = - game_controller_listener_.GetDesignatedPosition(); - if (gc_designated_position.has_value()) { - if (game_controller_listener_.GetTeamSide() == ateam_common::TeamSide::PositiveHalf) { - world_.referee_info.designated_position = ateam_geometry::Point( - -gc_designated_position->x, - -gc_designated_position->y); - } else { - world_.referee_info.designated_position = ateam_geometry::Point( - gc_designated_position->x, - gc_designated_position->y); - } - } else { - world_.referee_info.designated_position = std::nullopt; - } - - - if (game_controller_listener_.GetOurGoalieID().has_value()) { - world_.referee_info.our_goalie_id = game_controller_listener_.GetOurGoalieID().value(); - } - if (game_controller_listener_.GetTheirGoalieID().has_value()) { - world_.referee_info.their_goalie_id = game_controller_listener_.GetTheirGoalieID().value(); - } - in_play_eval_.Update(world_); - double_touch_eval_.update(world_, overlays_); - if (get_parameter("use_emulated_ballsense").as_bool()) { - ballsense_emulator_.Update(world_); - } - ballsense_filter_.Update(world_); - if (game_controller_listener_.GetTeamColor() == ateam_common::TeamColor::Unknown) { - auto & clk = *this->get_clock(); - RCLCPP_WARN_THROTTLE( - this->get_logger(), clk, 3000, - "DETECTED TEAM COLOR WAS UNKNOWN"); - } - if (game_controller_listener_.GetTeamSide() == ateam_common::TeamSide::Unknown) { - auto & clk = *this->get_clock(); - RCLCPP_WARN_THROTTLE( - this->get_logger(), clk, 3000, - "DETECTED TEAM SIDE WAS UNKNOWN"); - } - - world_publisher_->publish(ateam_kenobi::message_conversions::toMsg(world_)); - ateam_msgs::msg::KenobiStatus status_msg; - status_msg.fps = fps_tracker_.Update(world_); + status_msg.fps = fps_tracker_.Update(world); status_publisher_->publish(status_msg); - auto motion_commands = runPlayFrame(world_); + auto motion_commands = runPlayFrame(world); - defense_area_enforcement::EnforceDefenseAreaKeepout(world_, motion_commands); + defense_area_enforcement::EnforceDefenseAreaKeepout(world, motion_commands); joystick_enforcer_.RemoveCommandForJoystickBot(motion_commands); if (get_parameter("use_world_velocities").as_bool()) { - motion::ConvertBodyVelsToWorldVels(motion_commands, world_.our_robots); + motion::ConvertBodyVelsToWorldVels(motion_commands, world.our_robots); } else { - motion::ConvertWorldVelsToBodyVels(motion_commands, world_.our_robots); + motion::ConvertWorldVelsToBodyVels(motion_commands, world.our_robots); } send_all_motion_commands(motion_commands); @@ -490,15 +239,6 @@ class KenobiNode : public rclcpp::Node const auto & maybe_motion_command = robot_motion_commands.at(id); if (maybe_motion_command.has_value()) { robot_commands_publishers_.at(id)->publish(maybe_motion_command.value()); - world_.our_robots.at(id).prev_command_vel = ateam_geometry::Vector( - maybe_motion_command.value().twist.linear.x, - maybe_motion_command.value().twist.linear.y - ); - - world_.our_robots.at(id).prev_command_omega = maybe_motion_command.value().twist.angular.z; - } else { - world_.our_robots.at(id).prev_command_vel = ateam_geometry::Vector(0, 0); - world_.our_robots.at(id).prev_command_omega = 0; } } } diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp index 10399fd35..3f7085d38 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp @@ -23,7 +23,7 @@ #include #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" #include "skills/goalie.hpp" #include "core/play_helpers/robot_assignment.hpp" #include "core/play_helpers/available_robots.hpp" diff --git a/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp b/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp index a7e6187db..c722c022b 100644 --- a/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp +++ b/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp @@ -23,7 +23,7 @@ #define PLAYS__PASSING_PLAYS__SEGMENT_PASSING_TARGET_FUNCS_HPP_ #include -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/lanes.hpp" #include "core/stp/base.hpp" diff --git a/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp b/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp index 736ad8d84..55bfec26f 100644 --- a/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp +++ b/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp @@ -29,7 +29,7 @@ #include #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::plays::stop_plays::stop_helpers diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp index 8349f95f7..5848b901e 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -22,7 +22,7 @@ #include #include #include -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/available_robots.hpp" namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp b/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp index 9afd9c2e1..520406aad 100644 --- a/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp @@ -19,7 +19,7 @@ // THE SOFTWARE. #include "test_intercept_play.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/available_robots.hpp" #include "core/play_helpers/robot_assignment.hpp" diff --git a/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp b/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp index f7707eb67..85d025dee 100644 --- a/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp @@ -19,7 +19,7 @@ // THE SOFTWARE. #include "test_pass_play.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/available_robots.hpp" #include "core/play_helpers/robot_assignment.hpp" diff --git a/ateam_kenobi/src/plays/test_plays/test_play.cpp b/ateam_kenobi/src/plays/test_plays/test_play.cpp index 773f2f942..a486ed11a 100644 --- a/ateam_kenobi/src/plays/test_plays/test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_play.cpp @@ -20,7 +20,7 @@ #include "test_play.hpp" #include "ateam_geometry/types.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "skills/goalie.hpp" #include "core/play_helpers/available_robots.hpp" diff --git a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp index 1867dcb06..6710e6d12 100644 --- a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp +++ b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp @@ -21,7 +21,7 @@ #include "corner_lineup_play.hpp" #include -#include "core/types/robot.hpp" +#include "core/types.hpp" #include "core/play_helpers/available_robots.hpp" namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp index 17a983921..26f4bd5b3 100644 --- a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp +++ b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp @@ -26,7 +26,7 @@ #include #include "ateam_geometry/types.hpp" -#include "core/types/robot.hpp" +#include "core/types.hpp" #include "core/stp/play.hpp" #include "core/play_helpers/easy_move_to.hpp" diff --git a/ateam_kenobi/src/plays/wall_play.cpp b/ateam_kenobi/src/plays/wall_play.cpp index 1bead908e..c51f62422 100644 --- a/ateam_kenobi/src/plays/wall_play.cpp +++ b/ateam_kenobi/src/plays/wall_play.cpp @@ -24,7 +24,7 @@ #include #include #include "core/play_helpers/robot_assignment.hpp" -#include "core/types/robot.hpp" +#include "core/types.hpp" #include "skills/goalie.hpp" #include "core/play_helpers/available_robots.hpp" namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/wall_play.hpp b/ateam_kenobi/src/plays/wall_play.hpp index 9eba641d7..6378affa3 100644 --- a/ateam_kenobi/src/plays/wall_play.hpp +++ b/ateam_kenobi/src/plays/wall_play.hpp @@ -26,7 +26,7 @@ #include #include "ateam_geometry/types.hpp" -#include "core/types/robot.hpp" +#include "core/types.hpp" #include "core/stp/play.hpp" #include "core/play_helpers/easy_move_to.hpp" #include "skills/goalie.hpp" diff --git a/ateam_kenobi/src/skills/capture.hpp b/ateam_kenobi/src/skills/capture.hpp index ce3be9b25..a5347da19 100644 --- a/ateam_kenobi/src/skills/capture.hpp +++ b/ateam_kenobi/src/skills/capture.hpp @@ -26,7 +26,7 @@ #include "core/play_helpers/easy_move_to.hpp" #include #include "core/stp/skill.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/dribble.hpp b/ateam_kenobi/src/skills/dribble.hpp index a523b38d2..00f01bec5 100644 --- a/ateam_kenobi/src/skills/dribble.hpp +++ b/ateam_kenobi/src/skills/dribble.hpp @@ -26,7 +26,7 @@ #include "core/play_helpers/easy_move_to.hpp" #include #include "core/stp/skill.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/goalie.hpp b/ateam_kenobi/src/skills/goalie.hpp index 34e77f988..0f96ad4e3 100644 --- a/ateam_kenobi/src/skills/goalie.hpp +++ b/ateam_kenobi/src/skills/goalie.hpp @@ -24,7 +24,7 @@ #include #include #include "core/play_helpers/easy_move_to.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/stp/skill.hpp" #include "pivot_kick.hpp" #include "line_kick.hpp" diff --git a/ateam_kenobi/src/skills/intercept.hpp b/ateam_kenobi/src/skills/intercept.hpp index ef8e6c477..c10a18717 100644 --- a/ateam_kenobi/src/skills/intercept.hpp +++ b/ateam_kenobi/src/skills/intercept.hpp @@ -26,7 +26,7 @@ #include "core/play_helpers/easy_move_to.hpp" #include #include "core/stp/skill.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/line_kick.hpp b/ateam_kenobi/src/skills/line_kick.hpp index 1b031c6dd..bfdf357a3 100644 --- a/ateam_kenobi/src/skills/line_kick.hpp +++ b/ateam_kenobi/src/skills/line_kick.hpp @@ -25,7 +25,7 @@ #include #include #include "kick_skill.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/pass_receiver.hpp b/ateam_kenobi/src/skills/pass_receiver.hpp index 118540e99..022b5b626 100644 --- a/ateam_kenobi/src/skills/pass_receiver.hpp +++ b/ateam_kenobi/src/skills/pass_receiver.hpp @@ -24,7 +24,7 @@ #include #include "core/play_helpers/easy_move_to.hpp" #include "core/stp/skill.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" namespace ateam_kenobi::skills { diff --git a/ateam_kenobi/src/skills/pivot_kick.hpp b/ateam_kenobi/src/skills/pivot_kick.hpp index fa7a63b02..fe98058f2 100644 --- a/ateam_kenobi/src/skills/pivot_kick.hpp +++ b/ateam_kenobi/src/skills/pivot_kick.hpp @@ -25,7 +25,7 @@ #include #include #include "kick_skill.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/easy_move_to.hpp" #include "skills/capture.hpp" diff --git a/ateam_kenobi/src/tactics/blockers.hpp b/ateam_kenobi/src/tactics/blockers.hpp index 6ad3c99a2..58ca6539e 100644 --- a/ateam_kenobi/src/tactics/blockers.hpp +++ b/ateam_kenobi/src/tactics/blockers.hpp @@ -26,7 +26,7 @@ #include #include #include "core/stp/tactic.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::tactics diff --git a/ateam_kenobi/src/tactics/defenders.hpp b/ateam_kenobi/src/tactics/defenders.hpp index 335adeef6..2176d3e9b 100644 --- a/ateam_kenobi/src/tactics/defenders.hpp +++ b/ateam_kenobi/src/tactics/defenders.hpp @@ -25,8 +25,8 @@ #include #include #include -#include "core/types/world.hpp" -#include "core/types/robot.hpp" +#include "core/types.hpp" +#include "core/types.hpp" #include "core/stp/tactic.hpp" #include "core/play_helpers/easy_move_to.hpp" diff --git a/ateam_kenobi/src/tactics/pass.hpp b/ateam_kenobi/src/tactics/pass.hpp index 83d474679..a897b3162 100644 --- a/ateam_kenobi/src/tactics/pass.hpp +++ b/ateam_kenobi/src/tactics/pass.hpp @@ -24,7 +24,7 @@ #include #include "core/stp/tactic.hpp" -#include "core/types/world.hpp" +#include "core/types.hpp" #include "skills/universal_kick.hpp" #include "skills/pass_receiver.hpp" diff --git a/ateam_kenobi/src/tactics/standard_defense.hpp b/ateam_kenobi/src/tactics/standard_defense.hpp index 6e549d734..55c6039e7 100644 --- a/ateam_kenobi/src/tactics/standard_defense.hpp +++ b/ateam_kenobi/src/tactics/standard_defense.hpp @@ -23,8 +23,8 @@ #include #include -#include "core/types/world.hpp" -#include "core/types/robot.hpp" +#include "core/types.hpp" +#include "core/types.hpp" #include "core/stp/tactic.hpp" #include "defenders.hpp" #include "skills/goalie.hpp" diff --git a/ateam_kenobi/test/path_planner_test.cpp b/ateam_kenobi/test/path_planner_test.cpp index dbd620ca2..403514c2e 100644 --- a/ateam_kenobi/test/path_planner_test.cpp +++ b/ateam_kenobi/test/path_planner_test.cpp @@ -24,8 +24,8 @@ #include #include "core/path_planning/path_planner.hpp" -#include "core/types/world.hpp" -#include "core/types/robot.hpp" +#include "core/types.hpp" +#include "core/types.hpp" #include #include diff --git a/ateam_kenobi/test/robot_assignment_test.cpp b/ateam_kenobi/test/robot_assignment_test.cpp index ad3fd42f4..e6927b805 100644 --- a/ateam_kenobi/test/robot_assignment_test.cpp +++ b/ateam_kenobi/test/robot_assignment_test.cpp @@ -28,7 +28,6 @@ using namespace ateam_kenobi::play_helpers; // NOLINT(build/namespaces) using testing::ElementsAre; using testing::Optional; -using testing::Field; using testing::Eq; TEST(RobotAssignmentTest, EmptyRobots) @@ -62,7 +61,7 @@ TEST(RobotAssignmentTest, OneRobotOneGoal) ateam_geometry::Point(3, 4) }; const auto assignments = assignRobots(robots, goals); - EXPECT_THAT(assignments, ElementsAre(Optional(Field(&Robot::id, Eq(1))))); + EXPECT_THAT(assignments, ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))))); } TEST(RobotAssignmentTest, TwoRobotsOneGoal) @@ -79,7 +78,7 @@ TEST(RobotAssignmentTest, TwoRobotsOneGoal) ateam_geometry::Point(3, 4) }; const auto assignments = assignRobots(robots, goals); - EXPECT_THAT(assignments, ElementsAre(Optional(Field(&Robot::id, Eq(2))))); + EXPECT_THAT(assignments, ElementsAre(Optional(testing::Field(&Robot::id, Eq(2))))); } TEST(RobotAssignmentTest, TwoRobotsTwoGoals) @@ -99,7 +98,7 @@ TEST(RobotAssignmentTest, TwoRobotsTwoGoals) const auto assignments = assignRobots(robots, goals); EXPECT_THAT( assignments, - ElementsAre(Optional(Field(&Robot::id, Eq(1))), Optional(Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), Optional(testing::Field(&Robot::id, Eq(2))))); } TEST(RobotAssignmentTest, TwoRobotsSameDistance) @@ -119,7 +118,7 @@ TEST(RobotAssignmentTest, TwoRobotsSameDistance) const auto assignments = assignRobots(robots, goals); EXPECT_THAT( assignments, - ElementsAre(Optional(Field(&Robot::id, Eq(1))), Optional(Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), Optional(testing::Field(&Robot::id, Eq(2))))); } TEST(RobotAssignmentTest, DisallowAssigningDisallowedRobots) @@ -165,13 +164,13 @@ TEST(GroupAssignmentTest, ThreeGroups) { const auto assignments = play_helpers::assignGroups(robots, groups); - EXPECT_THAT(assignments.GetPositionAssignment("kicker"), Optional(Field(&Robot::id, Eq(0)))); + EXPECT_THAT(assignments.GetPositionAssignment("kicker"), Optional(testing::Field(&Robot::id, Eq(0)))); EXPECT_THAT( assignments.GetGroupAssignments("support"), - ElementsAre(Optional(Field(&Robot::id, Eq(1))), Optional(Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), Optional(testing::Field(&Robot::id, Eq(2))))); EXPECT_THAT( assignments.GetGroupAssignments("defense"), - ElementsAre(Optional(Field(&Robot::id, Eq(3))), Optional(Field(&Robot::id, Eq(4))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(3))), Optional(testing::Field(&Robot::id, Eq(4))))); } TEST(GroupAssignmentTest, DisallowedIds) { @@ -193,8 +192,8 @@ TEST(GroupAssignmentTest, DisallowedIds) { const auto assignments = play_helpers::assignGroups(robots, groups); - EXPECT_THAT(assignments.GetPositionAssignment("kicker"), Optional(Field(&Robot::id, Eq(1)))); + EXPECT_THAT(assignments.GetPositionAssignment("kicker"), Optional(testing::Field(&Robot::id, Eq(1)))); EXPECT_THAT( assignments.GetGroupAssignments("support"), - ElementsAre(Optional(Field(&Robot::id, Eq(0))), Optional(Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(0))), Optional(testing::Field(&Robot::id, Eq(2))))); } From 78d82b08b171b447b751b742d6257f5228d04110 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 23:25:18 -0400 Subject: [PATCH 15/19] Adds game_state_tracker to autonomy launch file. --- ateam_bringup/launch/autonomy.launch.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ateam_bringup/launch/autonomy.launch.xml b/ateam_bringup/launch/autonomy.launch.xml index e9cb31816..6f0a5f00f 100644 --- a/ateam_bringup/launch/autonomy.launch.xml +++ b/ateam_bringup/launch/autonomy.launch.xml @@ -15,11 +15,13 @@ + + + - From 01bfe13fd347734177742becd2a690edc7b95956 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 1 Oct 2025 02:04:11 -0400 Subject: [PATCH 16/19] Fixes several issues from kenobi split. --- ateam_kenobi/src/kenobi_node.cpp | 1 + ateam_ui/src/src/rosManager.ts | 26 +++++++++---- .../include/ateam_game_state/field.hpp | 2 + .../game_state_tracker/game_state_tracker.cpp | 5 ++- .../ateam_game_state/src/type_adapters.cpp | 38 ++++++++++--------- 5 files changed, 47 insertions(+), 25 deletions(-) diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index e6ea8ea6b..e11b51be1 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include "core/types.hpp" diff --git a/ateam_ui/src/src/rosManager.ts b/ateam_ui/src/src/rosManager.ts index ad958ed24..d021c732a 100644 --- a/ateam_ui/src/src/rosManager.ts +++ b/ateam_ui/src/src/rosManager.ts @@ -108,6 +108,14 @@ export class RosManager { joystickStatusTopic.subscribe(this.getJoystickStatusCallback(appState)); this.subscriptions.set("joystickStatus", joystickStatusTopic); + let kenobiStatusTopic = new ROSLIB.Topic({ + ros: this.ros, + name: '/kenobi_node/status', + messageType: 'ateam_msgs/msg/KenobiStatus' + }); + kenobiStatusTopic.subscribe(this.getKenobiStatusCallback(appState)); + this.subscriptions.set("kenobiStatus", kenobiStatusTopic); + let goalieService = new ROSLIB.Service({ ros: this.ros, name: '/team_client_node/set_desired_keeper', @@ -182,8 +190,8 @@ export class RosManager { if (!kenobiTopic) { kenobiTopic = new ROSLIB.Topic({ ros: this.ros, - name: '/kenobi_node/world', - messageType: 'ateam_msgs/msg/World' + name: '/world', + messageType: 'ateam_msgs/msg/GameStateWorld' }); this.subscriptions.set("kenobi", kenobiTopic); } @@ -259,13 +267,10 @@ export class RosManager { return function(msg: any): void { // Convert timestamp to millis appState.realtimeWorld.timestamp = (msg.current_time.sec * 1e3) + (msg.current_time.nanosec / 1e6); - appState.realtimeWorld.fps = msg.fps; appState.lastTimeReceivedKenobi = Date.now(); - if (msg.balls.length > 0) { - appState.realtimeWorld.ball.pose = msg.balls[0].pose; - appState.realtimeWorld.ball.visible = msg.balls[0].visible; - } + appState.realtimeWorld.ball.pose = msg.ball.pose; + appState.realtimeWorld.ball.visible = msg.ball.visible; for (const team of [TeamColor.Blue, TeamColor.Yellow]) { const msgRobotArray = (team === appState.realtimeWorld.team) ? msg.our_robots : msg.their_robots; @@ -476,4 +481,11 @@ export class RosManager { } } } + + getKenobiStatusCallback(appState: AppState): (msg: any) => void { + return function(msg: any): void { + appState.realtimeWorld.fps = msg.fps; + + } + } } diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp index e4e446caf..06754e050 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp @@ -40,6 +40,8 @@ struct Field float boundary_width = 0.f; float defense_area_width = 0.f; float defense_area_depth = 0.f; + ateam_geometry::Point center_circle_center; + float center_circle_radius; std::array field_corners; int ignore_side = 0; FieldSidedInfo ours; diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index b5fcc7cee..6732abf61 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -50,6 +50,8 @@ class GameStateTracker : public rclcpp::Node { : rclcpp::Node("game_state_tracker", options), gc_listener_(*this) { + InitializeRobotIds(); + world_pub_ = create_publisher(std::string(Topics::kWorld), rclcpp::QoS(1).best_effort().durability_volatile()); @@ -100,7 +102,7 @@ class GameStateTracker : public rclcpp::Node { this); timer_ = create_wall_timer(std::chrono::duration(1.0 / - declare_parameter("update_frequency")), + declare_parameter("update_frequency", 100.0)), std::bind(&GameStateTracker::TimerCallback, this)); } @@ -242,6 +244,7 @@ class GameStateTracker : public rclcpp::Node { void TimerCallback() { + world_.current_time = std::chrono::steady_clock::now(); UpdateRefInfo(); double_touch_evaluator_.Update(world_); in_play_evaluator_.Update(world_); diff --git a/state_tracking/ateam_game_state/src/type_adapters.cpp b/state_tracking/ateam_game_state/src/type_adapters.cpp index 26486147e..1649134eb 100644 --- a/state_tracking/ateam_game_state/src/type_adapters.cpp +++ b/state_tracking/ateam_game_state/src/type_adapters.cpp @@ -194,28 +194,27 @@ void rclcpp::TypeAdaptergeometry_msgs::msg::Point32 { return geometry_msgs::build().x(val.x()).y(val.y()).z(0); }); }; - convert_point_array(field.field_corners, std::back_inserter(ros_msg.field_corners.points)); - convert_point_array( - field.ours.defense_area_corners, - std::back_inserter(ros_msg.ours.defense_area_corners.points)); - convert_point_array( - field.ours.goal_corners, - std::back_inserter(ros_msg.ours.goal_corners.points)); - convert_point_array( - field.theirs.defense_area_corners, - std::back_inserter(ros_msg.theirs.defense_area_corners.points)); - convert_point_array( - field.theirs.goal_corners, - std::back_inserter(ros_msg.theirs.goal_corners.points)); + convert_point_array(field.field_corners, ros_msg.field_corners.points); + convert_point_array(field.ours.defense_area_corners, ros_msg.ours.defense_area_corners.points); + convert_point_array(field.ours.goal_corners, ros_msg.ours.goal_corners.points); + convert_point_array(field.theirs.defense_area_corners, + ros_msg.theirs.defense_area_corners.points); + convert_point_array(field.theirs.goal_corners, ros_msg.theirs.goal_corners.points); } void rclcpp::TypeAdapter::convert_to_custom( @@ -227,8 +226,13 @@ void rclcpp::TypeAdapter::c field.goal_depth = ros_msg.goal_depth; field.boundary_width = ros_msg.boundary_width; field.ignore_side = ros_msg.ignore_side; + field.defense_area_width = ros_msg.defense_area_width; + field.defense_area_depth = ros_msg.defense_area_depth; + field.center_circle_center = ateam_geometry::Point(ros_msg.center_circle.x, + ros_msg.center_circle.y); + field.center_circle_radius = ros_msg.center_circle_radius; - auto convert_point_array = [&](auto & in_vector, auto out_array) { + auto convert_point_array = [&](auto & in_vector, auto & out_array) { const auto count = std::min(in_vector.size(), out_array.size()); std::transform( in_vector.begin(), in_vector.begin() + count, out_array.begin(), From 86235d58b4d3290642391077d83cd8e13f6e1417 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 1 Oct 2025 02:30:45 -0400 Subject: [PATCH 17/19] Fixes linting failures --- .../src/core/motion/motion_controller.hpp | 1 - .../core/play_helpers/available_robots.hpp | 2 -- .../src/core/play_helpers/easy_move_to.hpp | 1 - ateam_kenobi/src/tactics/defenders.hpp | 1 - ateam_kenobi/src/tactics/standard_defense.hpp | 1 - ateam_kenobi/test/path_planner_test.cpp | 1 - ateam_kenobi/test/robot_assignment_test.cpp | 21 ++++++++++++------- .../include/ateam_game_state/ball.hpp | 2 +- .../include/ateam_game_state/field.hpp | 2 +- .../include/ateam_game_state/referee_info.hpp | 2 +- .../include/ateam_game_state/robot.hpp | 2 +- .../ateam_game_state/type_adapters.hpp | 15 ++++++++----- .../include/ateam_game_state/world.hpp | 2 +- .../double_touch_evaluator.hpp | 6 +++--- .../game_state_tracker/game_state_tracker.cpp | 3 ++- .../game_state_tracker/in_play_evaluator.cpp | 2 +- .../game_state_tracker/in_play_evaluator.hpp | 6 +++--- 17 files changed, 38 insertions(+), 32 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_controller.hpp b/ateam_kenobi/src/core/motion/motion_controller.hpp index 5e53ddcef..8009572ca 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -28,7 +28,6 @@ #include #include #include "core/types.hpp" -#include "core/types.hpp" #include "pid.hpp" // cause the robot to: always face a point, face in the direction of travel, or stay facing the diff --git a/ateam_kenobi/src/core/play_helpers/available_robots.hpp b/ateam_kenobi/src/core/play_helpers/available_robots.hpp index e1209b620..7818742cd 100644 --- a/ateam_kenobi/src/core/play_helpers/available_robots.hpp +++ b/ateam_kenobi/src/core/play_helpers/available_robots.hpp @@ -25,8 +25,6 @@ #include #include #include "core/types.hpp" -#include "core/types.hpp" - namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp index 124f7a1b6..5e806dacf 100644 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp +++ b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp @@ -31,7 +31,6 @@ #include "core/motion/motion_controller.hpp" #include "core/stp/base.hpp" #include "core/types.hpp" -#include "core/types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/tactics/defenders.hpp b/ateam_kenobi/src/tactics/defenders.hpp index 2176d3e9b..b038e93f4 100644 --- a/ateam_kenobi/src/tactics/defenders.hpp +++ b/ateam_kenobi/src/tactics/defenders.hpp @@ -26,7 +26,6 @@ #include #include #include "core/types.hpp" -#include "core/types.hpp" #include "core/stp/tactic.hpp" #include "core/play_helpers/easy_move_to.hpp" diff --git a/ateam_kenobi/src/tactics/standard_defense.hpp b/ateam_kenobi/src/tactics/standard_defense.hpp index 55c6039e7..a6d7d5460 100644 --- a/ateam_kenobi/src/tactics/standard_defense.hpp +++ b/ateam_kenobi/src/tactics/standard_defense.hpp @@ -24,7 +24,6 @@ #include #include #include "core/types.hpp" -#include "core/types.hpp" #include "core/stp/tactic.hpp" #include "defenders.hpp" #include "skills/goalie.hpp" diff --git a/ateam_kenobi/test/path_planner_test.cpp b/ateam_kenobi/test/path_planner_test.cpp index 403514c2e..62e341be7 100644 --- a/ateam_kenobi/test/path_planner_test.cpp +++ b/ateam_kenobi/test/path_planner_test.cpp @@ -25,7 +25,6 @@ #include "core/path_planning/path_planner.hpp" #include "core/types.hpp" -#include "core/types.hpp" #include #include diff --git a/ateam_kenobi/test/robot_assignment_test.cpp b/ateam_kenobi/test/robot_assignment_test.cpp index e6927b805..6ca6f319a 100644 --- a/ateam_kenobi/test/robot_assignment_test.cpp +++ b/ateam_kenobi/test/robot_assignment_test.cpp @@ -98,7 +98,8 @@ TEST(RobotAssignmentTest, TwoRobotsTwoGoals) const auto assignments = assignRobots(robots, goals); EXPECT_THAT( assignments, - ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), Optional(testing::Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), + Optional(testing::Field(&Robot::id, Eq(2))))); } TEST(RobotAssignmentTest, TwoRobotsSameDistance) @@ -118,7 +119,8 @@ TEST(RobotAssignmentTest, TwoRobotsSameDistance) const auto assignments = assignRobots(robots, goals); EXPECT_THAT( assignments, - ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), Optional(testing::Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), + Optional(testing::Field(&Robot::id, Eq(2))))); } TEST(RobotAssignmentTest, DisallowAssigningDisallowedRobots) @@ -164,13 +166,16 @@ TEST(GroupAssignmentTest, ThreeGroups) { const auto assignments = play_helpers::assignGroups(robots, groups); - EXPECT_THAT(assignments.GetPositionAssignment("kicker"), Optional(testing::Field(&Robot::id, Eq(0)))); + EXPECT_THAT(assignments.GetPositionAssignment("kicker"), + Optional(testing::Field(&Robot::id, Eq(0)))); EXPECT_THAT( assignments.GetGroupAssignments("support"), - ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), Optional(testing::Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(1))), + Optional(testing::Field(&Robot::id, Eq(2))))); EXPECT_THAT( assignments.GetGroupAssignments("defense"), - ElementsAre(Optional(testing::Field(&Robot::id, Eq(3))), Optional(testing::Field(&Robot::id, Eq(4))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(3))), + Optional(testing::Field(&Robot::id, Eq(4))))); } TEST(GroupAssignmentTest, DisallowedIds) { @@ -192,8 +197,10 @@ TEST(GroupAssignmentTest, DisallowedIds) { const auto assignments = play_helpers::assignGroups(robots, groups); - EXPECT_THAT(assignments.GetPositionAssignment("kicker"), Optional(testing::Field(&Robot::id, Eq(1)))); + EXPECT_THAT(assignments.GetPositionAssignment("kicker"), + Optional(testing::Field(&Robot::id, Eq(1)))); EXPECT_THAT( assignments.GetGroupAssignments("support"), - ElementsAre(Optional(testing::Field(&Robot::id, Eq(0))), Optional(testing::Field(&Robot::id, Eq(2))))); + ElementsAre(Optional(testing::Field(&Robot::id, Eq(0))), + Optional(testing::Field(&Robot::id, Eq(2))))); } diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp index 9a1516df6..683860a1a 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp @@ -34,6 +34,6 @@ struct Ball bool visible = false; std::chrono::steady_clock::time_point last_visible_time; }; -} // namespace ateam_kenobi +} // namespace ateam_game_state #endif // ATEAM_GAME_STATE__BALL_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp index 06754e050..221a81702 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp @@ -47,6 +47,6 @@ struct Field FieldSidedInfo ours; FieldSidedInfo theirs; }; -} // namespace ateam_kenobi +} // namespace ateam_game_state #endif // ATEAM_GAME_STATE__FIELD_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp index d46750392..a419969a6 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp @@ -40,6 +40,6 @@ struct RefereeInfo std::optional next_command = std::nullopt; std::chrono::system_clock::time_point command_time; }; -} // namespace ateam_kenobi +} // namespace ateam_game_state #endif // ATEAM_GAME_STATE__REFEREE_INFO_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp index 308bad66d..2ca8858e0 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp @@ -51,6 +51,6 @@ struct Robot return visible && radio_connected; } }; -} // namespace ateam_kenobi +} // namespace ateam_game_state #endif // ATEAM_GAME_STATE__ROBOT_HPP_ diff --git a/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp index f5c7e84df..58e92ecd5 100644 --- a/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp @@ -95,10 +95,15 @@ struct rclcpp::TypeAdapter double_touch_forbidden_id_; }; -} // namespace ateam_kenobi +} // namespace ateam_game_state #endif // ATEAM_GAME_STATE__WORLD_HPP_ diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp b/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp index 18071b8aa..aaf59111a 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/double_touch_evaluator.hpp @@ -19,8 +19,8 @@ // THE SOFTWARE. -#ifndef GAME_STATE_TRACKER__DOUBLE_TOUCH_EVAL_HPP_ -#define GAME_STATE_TRACKER__DOUBLE_TOUCH_EVAL_HPP_ +#ifndef GAME_STATE_TRACKER__DOUBLE_TOUCH_EVALUATOR_HPP_ +#define GAME_STATE_TRACKER__DOUBLE_TOUCH_EVALUATOR_HPP_ #include #include @@ -51,4 +51,4 @@ class DoubleTouchEvaluator } // namespace ateam_game_state -#endif // GAME_STATE_TRACKER__DOUBLE_TOUCH_EVAL_HPP_ +#endif // GAME_STATE_TRACKER__DOUBLE_TOUCH_EVALUATOR_HPP_ diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index 6732abf61..be88fe4df 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -122,7 +122,8 @@ class GameStateTracker : public rclcpp::Node { 16> robot_connection_status_subs_; std::array::SharedPtr, 16> robot_command_subs_; - ateam_common::GameControllerListener gc_listener_; // TODO(barulicm): Move GCListener type to ateam_game_state package + // TODO(barulicm): Move GCListener type to ateam_game_state package + ateam_common::GameControllerListener gc_listener_; World world_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp index 4ff712db7..871f30366 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.cpp @@ -153,4 +153,4 @@ bool InPlayEvaluator::HasTimeoutExpired() return (std::chrono::steady_clock::now() - timeout_duration_.value()) > timeout_start_; } -} // namespace ateam_kenobi +} // namespace ateam_game_state diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp index e286a29f1..d100a8253 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/in_play_evaluator.hpp @@ -19,8 +19,8 @@ // THE SOFTWARE. -#ifndef GAME_STATE_TRACKER__IN_PLAY_EVAL_HPP_ -#define GAME_STATE_TRACKER__IN_PLAY_EVAL_HPP_ +#ifndef GAME_STATE_TRACKER__IN_PLAY_EVALUATOR_HPP_ +#define GAME_STATE_TRACKER__IN_PLAY_EVALUATOR_HPP_ #include #include @@ -61,4 +61,4 @@ class InPlayEvaluator } // namespace ateam_game_state -#endif // GAME_STATE_TRACKER__IN_PLAY_EVAL_HPP_ +#endif // GAME_STATE_TRACKER__IN_PLAY_EVALUATOR_HPP_ From f80708edd3a8acddd6d00da3f4f5bbc996fdaba8 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 12 Oct 2025 21:56:38 -0400 Subject: [PATCH 18/19] Moves types.hpp -> state_types.hpp --- ateam_kenobi/src/core/ballsense_emulator.hpp | 2 +- ateam_kenobi/src/core/ballsense_filter.hpp | 2 +- ateam_kenobi/src/core/defense_area_enforcement.hpp | 2 +- ateam_kenobi/src/core/double_touch_eval.hpp | 2 +- ateam_kenobi/src/core/fps_tracker.hpp | 2 +- ateam_kenobi/src/core/in_play_eval.hpp | 2 +- ateam_kenobi/src/core/motion/frame_conversions.hpp | 2 +- ateam_kenobi/src/core/motion/motion_controller.hpp | 2 +- ateam_kenobi/src/core/motion/motion_intent.hpp | 4 ++-- ateam_kenobi/src/core/path_planning/escape_velocity.hpp | 2 +- ateam_kenobi/src/core/path_planning/obstacles.hpp | 2 +- ateam_kenobi/src/core/path_planning/path_planner.hpp | 2 +- ateam_kenobi/src/core/play_helpers/available_robots.hpp | 2 +- ateam_kenobi/src/core/play_helpers/lanes.hpp | 2 +- ateam_kenobi/src/core/play_helpers/possession.cpp | 2 +- ateam_kenobi/src/core/play_helpers/possession.hpp | 2 +- ateam_kenobi/src/core/play_helpers/robot_assignment.hpp | 2 +- ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp | 2 +- ateam_kenobi/src/core/play_helpers/window_evaluation.hpp | 2 +- ateam_kenobi/src/core/play_selector.hpp | 2 +- ateam_kenobi/src/core/stp/play.hpp | 2 +- ateam_kenobi/src/core/{types.hpp => types/state_types.hpp} | 6 +++--- ateam_kenobi/src/kenobi_node.cpp | 2 +- .../src/plays/kickoff_plays/offense/kickoff_pass_play.cpp | 2 +- .../plays/passing_plays/segment_passing_target_funcs.hpp | 2 +- ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp | 2 +- ateam_kenobi/src/plays/test_plays/controls_test_play.cpp | 2 +- ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp | 2 +- ateam_kenobi/src/plays/test_plays/test_pass_play.cpp | 2 +- ateam_kenobi/src/plays/test_plays/test_play.cpp | 2 +- ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp | 2 +- ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp | 2 +- ateam_kenobi/src/plays/wall_play.cpp | 2 +- ateam_kenobi/src/plays/wall_play.hpp | 2 +- ateam_kenobi/src/skills/capture.hpp | 2 +- ateam_kenobi/src/skills/dribble.hpp | 2 +- ateam_kenobi/src/skills/goalie.hpp | 2 +- ateam_kenobi/src/skills/intercept.hpp | 2 +- ateam_kenobi/src/skills/line_kick.hpp | 2 +- ateam_kenobi/src/skills/pass_receiver.hpp | 2 +- ateam_kenobi/src/skills/pivot_kick.hpp | 2 +- ateam_kenobi/src/tactics/blockers.hpp | 2 +- ateam_kenobi/src/tactics/defenders.hpp | 2 +- ateam_kenobi/src/tactics/pass.hpp | 2 +- ateam_kenobi/src/tactics/standard_defense.hpp | 2 +- ateam_kenobi/test/path_planner_test.cpp | 2 +- 46 files changed, 49 insertions(+), 49 deletions(-) rename ateam_kenobi/src/core/{types.hpp => types/state_types.hpp} (94%) diff --git a/ateam_kenobi/src/core/ballsense_emulator.hpp b/ateam_kenobi/src/core/ballsense_emulator.hpp index c73710fd3..794fedd9d 100644 --- a/ateam_kenobi/src/core/ballsense_emulator.hpp +++ b/ateam_kenobi/src/core/ballsense_emulator.hpp @@ -27,7 +27,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/ballsense_filter.hpp b/ateam_kenobi/src/core/ballsense_filter.hpp index 70d83e218..b67b5ca1a 100644 --- a/ateam_kenobi/src/core/ballsense_filter.hpp +++ b/ateam_kenobi/src/core/ballsense_filter.hpp @@ -24,7 +24,7 @@ #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/defense_area_enforcement.hpp b/ateam_kenobi/src/core/defense_area_enforcement.hpp index 4dd68f543..b7a112dcb 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.hpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.hpp @@ -22,7 +22,7 @@ #define CORE__DEFENSE_AREA_ENFORCEMENT_HPP_ #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::defense_area_enforcement { diff --git a/ateam_kenobi/src/core/double_touch_eval.hpp b/ateam_kenobi/src/core/double_touch_eval.hpp index cfacd863e..ff41e19ee 100644 --- a/ateam_kenobi/src/core/double_touch_eval.hpp +++ b/ateam_kenobi/src/core/double_touch_eval.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/visualization/overlays.hpp" namespace ateam_kenobi diff --git a/ateam_kenobi/src/core/fps_tracker.hpp b/ateam_kenobi/src/core/fps_tracker.hpp index 18ab9a472..82295547c 100644 --- a/ateam_kenobi/src/core/fps_tracker.hpp +++ b/ateam_kenobi/src/core/fps_tracker.hpp @@ -23,7 +23,7 @@ #define CORE__FPS_TRACKER_HPP_ #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/in_play_eval.hpp b/ateam_kenobi/src/core/in_play_eval.hpp index a2d0aa2ff..96131b05f 100644 --- a/ateam_kenobi/src/core/in_play_eval.hpp +++ b/ateam_kenobi/src/core/in_play_eval.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/motion/frame_conversions.hpp b/ateam_kenobi/src/core/motion/frame_conversions.hpp index 1c1f93f27..6456ee1ff 100644 --- a/ateam_kenobi/src/core/motion/frame_conversions.hpp +++ b/ateam_kenobi/src/core/motion/frame_conversions.hpp @@ -23,7 +23,7 @@ #define CORE__MOTION__FRAME_CONVERSIONS_HPP_ #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/motion/motion_intent.hpp" namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/motion/motion_controller.hpp b/ateam_kenobi/src/core/motion/motion_controller.hpp index 533d8904c..6b60c6140 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -27,7 +27,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "pid.hpp" #include "motion_options.hpp" #include "motion_intent.hpp" diff --git a/ateam_kenobi/src/core/motion/motion_intent.hpp b/ateam_kenobi/src/core/motion/motion_intent.hpp index 2f9c312e8..01a3c597c 100644 --- a/ateam_kenobi/src/core/motion/motion_intent.hpp +++ b/ateam_kenobi/src/core/motion/motion_intent.hpp @@ -26,8 +26,8 @@ #include #include #include -#include "core/types.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" +#include "core/types/state_types.hpp" #include "core/path_planning/path.hpp" #include "core/path_planning/planner_options.hpp" #include "motion_options.hpp" diff --git a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp index 11accd751..a200a803a 100644 --- a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp +++ b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp @@ -24,7 +24,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::path_planning { diff --git a/ateam_kenobi/src/core/path_planning/obstacles.hpp b/ateam_kenobi/src/core/path_planning/obstacles.hpp index dcad2670d..bd36ef002 100644 --- a/ateam_kenobi/src/core/path_planning/obstacles.hpp +++ b/ateam_kenobi/src/core/path_planning/obstacles.hpp @@ -24,7 +24,7 @@ #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::path_planning { diff --git a/ateam_kenobi/src/core/path_planning/path_planner.hpp b/ateam_kenobi/src/core/path_planning/path_planner.hpp index 697e3f84d..34e66dab9 100644 --- a/ateam_kenobi/src/core/path_planning/path_planner.hpp +++ b/ateam_kenobi/src/core/path_planning/path_planner.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/stp/base.hpp" #include "path.hpp" #include "planner_options.hpp" diff --git a/ateam_kenobi/src/core/play_helpers/available_robots.hpp b/ateam_kenobi/src/core/play_helpers/available_robots.hpp index 7818742cd..bfb5c5107 100644 --- a/ateam_kenobi/src/core/play_helpers/available_robots.hpp +++ b/ateam_kenobi/src/core/play_helpers/available_robots.hpp @@ -24,7 +24,7 @@ #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/lanes.hpp b/ateam_kenobi/src/core/play_helpers/lanes.hpp index 4c42a86c5..b37555271 100644 --- a/ateam_kenobi/src/core/play_helpers/lanes.hpp +++ b/ateam_kenobi/src/core/play_helpers/lanes.hpp @@ -22,7 +22,7 @@ #define CORE__PLAY_HELPERS__LANES_HPP_ #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::play_helpers::lanes { diff --git a/ateam_kenobi/src/core/play_helpers/possession.cpp b/ateam_kenobi/src/core/play_helpers/possession.cpp index 21a2518ae..7aa689160 100644 --- a/ateam_kenobi/src/core/play_helpers/possession.cpp +++ b/ateam_kenobi/src/core/play_helpers/possession.cpp @@ -23,7 +23,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/possession.hpp b/ateam_kenobi/src/core/play_helpers/possession.hpp index 242570254..42c9a15bd 100644 --- a/ateam_kenobi/src/core/play_helpers/possession.hpp +++ b/ateam_kenobi/src/core/play_helpers/possession.hpp @@ -21,7 +21,7 @@ #ifndef CORE__PLAY_HELPERS__POSSESSION_HPP_ #define CORE__PLAY_HELPERS__POSSESSION_HPP_ -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp b/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp index 093fe69fc..9f7d4d0d3 100644 --- a/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp +++ b/ateam_kenobi/src/core/play_helpers/robot_assignment.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp b/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp index 25f391424..dce6b9122 100644 --- a/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp +++ b/ateam_kenobi/src/core/play_helpers/shot_evaluation.hpp @@ -23,7 +23,7 @@ #define CORE__PLAY_HELPERS__SHOT_EVALUATION_HPP_ #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::play_helpers { diff --git a/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp b/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp index 7ad60021e..ee83c1fba 100644 --- a/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp +++ b/ateam_kenobi/src/core/play_helpers/window_evaluation.hpp @@ -26,7 +26,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/visualization/overlays.hpp" namespace ateam_kenobi::play_helpers::window_evaluation diff --git a/ateam_kenobi/src/core/play_selector.hpp b/ateam_kenobi/src/core/play_selector.hpp index 3e6166274..726597442 100644 --- a/ateam_kenobi/src/core/play_selector.hpp +++ b/ateam_kenobi/src/core/play_selector.hpp @@ -29,7 +29,7 @@ #include #include #include "core/stp/play.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi { diff --git a/ateam_kenobi/src/core/stp/play.hpp b/ateam_kenobi/src/core/stp/play.hpp index e026a9461..fc54244f6 100644 --- a/ateam_kenobi/src/core/stp/play.hpp +++ b/ateam_kenobi/src/core/stp/play.hpp @@ -27,7 +27,7 @@ #include #include "base.hpp" #include "play_score.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" namespace ateam_kenobi::stp diff --git a/ateam_kenobi/src/core/types.hpp b/ateam_kenobi/src/core/types/state_types.hpp similarity index 94% rename from ateam_kenobi/src/core/types.hpp rename to ateam_kenobi/src/core/types/state_types.hpp index d0fd27eb9..725711893 100644 --- a/ateam_kenobi/src/core/types.hpp +++ b/ateam_kenobi/src/core/types/state_types.hpp @@ -19,8 +19,8 @@ // THE SOFTWARE. -#ifndef CORE__TYPES_HPP_ -#define CORE__TYPES_HPP_ +#ifndef CORE__STATE_TYPES_HPP_ +#define CORE__STATE_TYPES_HPP_ #include #include @@ -37,4 +37,4 @@ using Robot = ateam_game_state::Robot; using World = ateam_game_state::World; } // namespace ateam_kenobi -#endif // CORE__TYPES_HPP_ +#endif // CORE__STATE_TYPES_HPP_ diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 728f0f418..665e6ee98 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -40,7 +40,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/play_selector.hpp" #include "core/in_play_eval.hpp" #include "core/double_touch_eval.hpp" diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp index 5eb767456..94e3c9e54 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp @@ -23,7 +23,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "skills/goalie.hpp" #include "core/play_helpers/robot_assignment.hpp" #include "core/play_helpers/available_robots.hpp" diff --git a/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp b/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp index c722c022b..268a0e62d 100644 --- a/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp +++ b/ateam_kenobi/src/plays/passing_plays/segment_passing_target_funcs.hpp @@ -23,7 +23,7 @@ #define PLAYS__PASSING_PLAYS__SEGMENT_PASSING_TARGET_FUNCS_HPP_ #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/play_helpers/lanes.hpp" #include "core/stp/base.hpp" diff --git a/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp b/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp index fa062a110..112bab7a1 100644 --- a/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp +++ b/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp @@ -28,7 +28,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" #include "core/visualization/overlays.hpp" diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp index 674d332d2..a8cc381cf 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -22,7 +22,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/play_helpers/available_robots.hpp" namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp b/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp index 18c8e0256..824efb5a7 100644 --- a/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp @@ -19,7 +19,7 @@ // THE SOFTWARE. #include "test_intercept_play.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/play_helpers/available_robots.hpp" #include "core/play_helpers/robot_assignment.hpp" diff --git a/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp b/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp index 3d8ac0570..ee2d9db38 100644 --- a/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp @@ -19,7 +19,7 @@ // THE SOFTWARE. #include "test_pass_play.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/play_helpers/available_robots.hpp" #include "core/play_helpers/robot_assignment.hpp" diff --git a/ateam_kenobi/src/plays/test_plays/test_play.cpp b/ateam_kenobi/src/plays/test_plays/test_play.cpp index 18efec0c4..3e883187f 100644 --- a/ateam_kenobi/src/plays/test_plays/test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_play.cpp @@ -20,7 +20,7 @@ #include "test_play.hpp" #include "ateam_geometry/types.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "skills/goalie.hpp" #include "core/play_helpers/available_robots.hpp" diff --git a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp index f5e9b1247..2379ac999 100644 --- a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp +++ b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp @@ -21,7 +21,7 @@ #include "corner_lineup_play.hpp" #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/play_helpers/available_robots.hpp" namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp index de5238070..ce1b1a6e1 100644 --- a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp +++ b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp @@ -26,7 +26,7 @@ #include #include "ateam_geometry/types.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/stp/play.hpp" #include "tactics/multi_move_to.hpp" diff --git a/ateam_kenobi/src/plays/wall_play.cpp b/ateam_kenobi/src/plays/wall_play.cpp index 94a5b4db9..8fc6570eb 100644 --- a/ateam_kenobi/src/plays/wall_play.cpp +++ b/ateam_kenobi/src/plays/wall_play.cpp @@ -24,7 +24,7 @@ #include #include #include "core/play_helpers/robot_assignment.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "skills/goalie.hpp" #include "core/play_helpers/available_robots.hpp" namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/wall_play.hpp b/ateam_kenobi/src/plays/wall_play.hpp index 479565888..625979f1c 100644 --- a/ateam_kenobi/src/plays/wall_play.hpp +++ b/ateam_kenobi/src/plays/wall_play.hpp @@ -26,7 +26,7 @@ #include #include "ateam_geometry/types.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/stp/play.hpp" #include "skills/goalie.hpp" #include "tactics/multi_move_to.hpp" diff --git a/ateam_kenobi/src/skills/capture.hpp b/ateam_kenobi/src/skills/capture.hpp index 5f318bd86..eb7060ac1 100644 --- a/ateam_kenobi/src/skills/capture.hpp +++ b/ateam_kenobi/src/skills/capture.hpp @@ -25,7 +25,7 @@ #include #include #include "core/stp/skill.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" diff --git a/ateam_kenobi/src/skills/dribble.hpp b/ateam_kenobi/src/skills/dribble.hpp index 6de4331c0..f7ffe1e44 100644 --- a/ateam_kenobi/src/skills/dribble.hpp +++ b/ateam_kenobi/src/skills/dribble.hpp @@ -24,7 +24,7 @@ #include #include "core/stp/skill.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" diff --git a/ateam_kenobi/src/skills/goalie.hpp b/ateam_kenobi/src/skills/goalie.hpp index 8803ec2d9..49c7126b5 100644 --- a/ateam_kenobi/src/skills/goalie.hpp +++ b/ateam_kenobi/src/skills/goalie.hpp @@ -23,7 +23,7 @@ #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" #include "core/stp/skill.hpp" #include "pivot_kick.hpp" diff --git a/ateam_kenobi/src/skills/intercept.hpp b/ateam_kenobi/src/skills/intercept.hpp index c10a18717..5b26d3a5e 100644 --- a/ateam_kenobi/src/skills/intercept.hpp +++ b/ateam_kenobi/src/skills/intercept.hpp @@ -26,7 +26,7 @@ #include "core/play_helpers/easy_move_to.hpp" #include #include "core/stp/skill.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/line_kick.hpp b/ateam_kenobi/src/skills/line_kick.hpp index 03e7c397f..b5ac9a7f6 100644 --- a/ateam_kenobi/src/skills/line_kick.hpp +++ b/ateam_kenobi/src/skills/line_kick.hpp @@ -24,7 +24,7 @@ #include #include "kick_skill.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/pass_receiver.hpp b/ateam_kenobi/src/skills/pass_receiver.hpp index b58a7b9f5..13654ba7f 100644 --- a/ateam_kenobi/src/skills/pass_receiver.hpp +++ b/ateam_kenobi/src/skills/pass_receiver.hpp @@ -22,7 +22,7 @@ #define SKILLS__PASS_RECEIVER_HPP_ #include "core/stp/skill.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/pivot_kick.hpp b/ateam_kenobi/src/skills/pivot_kick.hpp index 85eba3b61..eb608e175 100644 --- a/ateam_kenobi/src/skills/pivot_kick.hpp +++ b/ateam_kenobi/src/skills/pivot_kick.hpp @@ -24,7 +24,7 @@ #include #include "kick_skill.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" #include "skills/capture.hpp" diff --git a/ateam_kenobi/src/tactics/blockers.hpp b/ateam_kenobi/src/tactics/blockers.hpp index 7221a0f9d..9cee0e5eb 100644 --- a/ateam_kenobi/src/tactics/blockers.hpp +++ b/ateam_kenobi/src/tactics/blockers.hpp @@ -25,7 +25,7 @@ #include #include #include "core/stp/tactic.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" namespace ateam_kenobi::tactics diff --git a/ateam_kenobi/src/tactics/defenders.hpp b/ateam_kenobi/src/tactics/defenders.hpp index f27a10e40..36356da0e 100644 --- a/ateam_kenobi/src/tactics/defenders.hpp +++ b/ateam_kenobi/src/tactics/defenders.hpp @@ -24,7 +24,7 @@ #include #include #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" #include "core/stp/tactic.hpp" diff --git a/ateam_kenobi/src/tactics/pass.hpp b/ateam_kenobi/src/tactics/pass.hpp index 8fe144672..faa80dff9 100644 --- a/ateam_kenobi/src/tactics/pass.hpp +++ b/ateam_kenobi/src/tactics/pass.hpp @@ -24,7 +24,7 @@ #include #include "core/stp/tactic.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" #include "skills/universal_kick.hpp" #include "skills/pass_receiver.hpp" diff --git a/ateam_kenobi/src/tactics/standard_defense.hpp b/ateam_kenobi/src/tactics/standard_defense.hpp index e491e9449..03c2fda0e 100644 --- a/ateam_kenobi/src/tactics/standard_defense.hpp +++ b/ateam_kenobi/src/tactics/standard_defense.hpp @@ -22,7 +22,7 @@ #define TACTICS__STANDARD_DEFENSE_HPP_ #include -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" #include "core/stp/tactic.hpp" #include "defenders.hpp" diff --git a/ateam_kenobi/test/path_planner_test.cpp b/ateam_kenobi/test/path_planner_test.cpp index 6f93603a5..62630e0a2 100644 --- a/ateam_kenobi/test/path_planner_test.cpp +++ b/ateam_kenobi/test/path_planner_test.cpp @@ -24,7 +24,7 @@ #include #include "core/path_planning/path_planner.hpp" -#include "core/types.hpp" +#include "core/types/state_types.hpp" #include #include From 3c92d75820e97cbe5d8f0b6d34a085293ff3d518 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 13 Oct 2025 01:58:53 -0400 Subject: [PATCH 19/19] Fixes linting failurs in kenobi --- ateam_kenobi/src/core/motion/motion_intent.hpp | 1 - ateam_kenobi/src/core/types/state_types.hpp | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_intent.hpp b/ateam_kenobi/src/core/motion/motion_intent.hpp index 01a3c597c..50595bdd4 100644 --- a/ateam_kenobi/src/core/motion/motion_intent.hpp +++ b/ateam_kenobi/src/core/motion/motion_intent.hpp @@ -27,7 +27,6 @@ #include #include #include "core/types/state_types.hpp" -#include "core/types/state_types.hpp" #include "core/path_planning/path.hpp" #include "core/path_planning/planner_options.hpp" #include "motion_options.hpp" diff --git a/ateam_kenobi/src/core/types/state_types.hpp b/ateam_kenobi/src/core/types/state_types.hpp index 725711893..ab0f7317b 100644 --- a/ateam_kenobi/src/core/types/state_types.hpp +++ b/ateam_kenobi/src/core/types/state_types.hpp @@ -19,8 +19,8 @@ // THE SOFTWARE. -#ifndef CORE__STATE_TYPES_HPP_ -#define CORE__STATE_TYPES_HPP_ +#ifndef CORE__TYPES__STATE_TYPES_HPP_ +#define CORE__TYPES__STATE_TYPES_HPP_ #include #include @@ -37,4 +37,4 @@ using Robot = ateam_game_state::Robot; using World = ateam_game_state::World; } // namespace ateam_kenobi -#endif // CORE__STATE_TYPES_HPP_ +#endif // CORE__TYPES__STATE_TYPES_HPP_