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 @@ + + + - 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/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index d87c9fe11..c911ee7a7 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/frame_conversions.cpp src/core/motion/motion_controller.cpp src/core/motion/motion_executor.cpp @@ -123,6 +123,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..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/world.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 acb7b4184..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/world.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 bc913eac7..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/world.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 f65cb4a3f..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/world.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 ecbafa25c..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/world.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 7cab8a3af..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/world.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 9dfcd5321..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/robot.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 98a5eac8a..6b60c6140 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -27,8 +27,7 @@ #include #include #include -#include "core/types/robot.hpp" -#include "core/types/world.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 9f94d8161..50595bdd4 100644 --- a/ateam_kenobi/src/core/motion/motion_intent.hpp +++ b/ateam_kenobi/src/core/motion/motion_intent.hpp @@ -26,8 +26,7 @@ #include #include #include -#include "core/types/world.hpp" -#include "core/types/robot.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 21d0fd448..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/robot.hpp" +#include "core/types/state_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..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/world.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 5fef6bd94..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/world.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 1c3d6673b..bfb5c5107 100644 --- a/ateam_kenobi/src/core/play_helpers/available_robots.hpp +++ b/ateam_kenobi/src/core/play_helpers/available_robots.hpp @@ -24,9 +24,7 @@ #include #include -#include "core/types/robot.hpp" -#include "core/types/world.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 f33440bda..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/world.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 34675cd77..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/robot.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 87dc09abe..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/world.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 efa161bb3..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/robot.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 8da1d7385..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/world.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 53c0388c8..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/robot.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 a72370a2e..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/world.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 f14ad1718..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/world.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/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/state_types.hpp similarity index 61% rename from ateam_kenobi/src/core/types/message_conversions.hpp rename to ateam_kenobi/src/core/types/state_types.hpp index 5413438de..ab0f7317b 100644 --- a/ateam_kenobi/src/core/types/message_conversions.hpp +++ b/ateam_kenobi/src/core/types/state_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,24 +19,22 @@ // THE SOFTWARE. -#ifndef CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ -#define CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ +#ifndef CORE__TYPES__STATE_TYPES_HPP_ +#define CORE__TYPES__STATE_TYPES_HPP_ -#include -#include -#include +#include +#include +#include +#include +#include -#include "core/types/ball.hpp" -#include "core/types/robot.hpp" -#include "core/types/world.hpp" - - -namespace ateam_kenobi::message_conversions +namespace ateam_kenobi { -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_ +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__STATE_TYPES_HPP_ diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index b1f7a3992..665e6ee98 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 @@ -42,10 +37,10 @@ #include #include #include +#include #include #include -#include "core/types/world.hpp" -#include "core/types/message_conversions.hpp" +#include "core/types/state_types.hpp" #include "core/play_selector.hpp" #include "core/in_play_eval.hpp" #include "core/double_touch_eval.hpp" @@ -73,11 +68,8 @@ class KenobiNode : public rclcpp::Node play_selector_(*this), joystick_enforcer_(*this), overlays_(""), - motion_executor_(get_logger().get_child("motion")), - game_controller_listener_(*this) + motion_executor_(get_logger().get_child("motion")) { - initialize_robot_ids(); - declare_parameter("use_world_velocities", false); declare_parameter("use_emulated_ballsense", false); @@ -89,52 +81,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, @@ -152,7 +102,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"; @@ -176,7 +128,6 @@ class KenobiNode : public rclcpp::Node } private: - World world_; PlaySelector play_selector_; InPlayEval in_play_eval_; DoubleTouchEval double_touch_eval_; @@ -189,159 +140,13 @@ class KenobiNode : public rclcpp::Node motion::MotionExecutor motion_executor_; 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, @@ -380,70 +185,15 @@ 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); @@ -525,15 +275,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 f812f13e3..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/world.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 a7e6187db..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/world.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 dcf1efa09..112bab7a1 100644 --- a/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp +++ b/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp @@ -28,8 +28,8 @@ #include #include #include +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" -#include "core/types/world.hpp" #include "core/visualization/overlays.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 aea0c6c33..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/world.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 d4363b1fe..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/world.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 805fc33a6..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/world.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 a04e7c64d..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/world.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 4fcd7e88e..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/robot.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 52afcc1de..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/robot.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 40933c239..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/robot.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 84746b1e7..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/robot.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 d6447191d..eb7060ac1 100644 --- a/ateam_kenobi/src/skills/capture.hpp +++ b/ateam_kenobi/src/skills/capture.hpp @@ -25,8 +25,8 @@ #include #include #include "core/stp/skill.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" -#include "core/types/world.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/dribble.hpp b/ateam_kenobi/src/skills/dribble.hpp index b9d58cd2f..f7ffe1e44 100644 --- a/ateam_kenobi/src/skills/dribble.hpp +++ b/ateam_kenobi/src/skills/dribble.hpp @@ -24,8 +24,8 @@ #include #include "core/stp/skill.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" -#include "core/types/world.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/goalie.hpp b/ateam_kenobi/src/skills/goalie.hpp index f7afefd13..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/world.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 ef8e6c477..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/world.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 0b884fa12..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/world.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 d20782241..13654ba7f 100644 --- a/ateam_kenobi/src/skills/pass_receiver.hpp +++ b/ateam_kenobi/src/skills/pass_receiver.hpp @@ -22,8 +22,8 @@ #define SKILLS__PASS_RECEIVER_HPP_ #include "core/stp/skill.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" -#include "core/types/world.hpp" namespace ateam_kenobi::skills { diff --git a/ateam_kenobi/src/skills/pivot_kick.hpp b/ateam_kenobi/src/skills/pivot_kick.hpp index d8c5a4ea9..eb608e175 100644 --- a/ateam_kenobi/src/skills/pivot_kick.hpp +++ b/ateam_kenobi/src/skills/pivot_kick.hpp @@ -24,8 +24,8 @@ #include #include "kick_skill.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" -#include "core/types/world.hpp" #include "skills/capture.hpp" namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/universal_kick.hpp b/ateam_kenobi/src/skills/universal_kick.hpp index d4f53354e..f4063ed82 100644 --- a/ateam_kenobi/src/skills/universal_kick.hpp +++ b/ateam_kenobi/src/skills/universal_kick.hpp @@ -24,6 +24,7 @@ #include "kick_skill.hpp" #include "pivot_kick.hpp" #include "line_kick.hpp" +#include "core/types/robot_command.hpp" namespace ateam_kenobi::skills { diff --git a/ateam_kenobi/src/tactics/blockers.hpp b/ateam_kenobi/src/tactics/blockers.hpp index 1781ad5e7..9cee0e5eb 100644 --- a/ateam_kenobi/src/tactics/blockers.hpp +++ b/ateam_kenobi/src/tactics/blockers.hpp @@ -25,8 +25,8 @@ #include #include #include "core/stp/tactic.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" -#include "core/types/world.hpp" namespace ateam_kenobi::tactics { diff --git a/ateam_kenobi/src/tactics/defenders.hpp b/ateam_kenobi/src/tactics/defenders.hpp index 88d84167b..36356da0e 100644 --- a/ateam_kenobi/src/tactics/defenders.hpp +++ b/ateam_kenobi/src/tactics/defenders.hpp @@ -24,8 +24,7 @@ #include #include #include -#include "core/types/world.hpp" -#include "core/types/robot.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 57a99a070..faa80dff9 100644 --- a/ateam_kenobi/src/tactics/pass.hpp +++ b/ateam_kenobi/src/tactics/pass.hpp @@ -24,8 +24,8 @@ #include #include "core/stp/tactic.hpp" +#include "core/types/state_types.hpp" #include "core/types/robot_command.hpp" -#include "core/types/world.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 0e614387e..03c2fda0e 100644 --- a/ateam_kenobi/src/tactics/standard_defense.hpp +++ b/ateam_kenobi/src/tactics/standard_defense.hpp @@ -22,8 +22,7 @@ #define TACTICS__STANDARD_DEFENSE_HPP_ #include -#include "core/types/world.hpp" -#include "core/types/robot.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 98df6f9ed..62630e0a2 100644 --- a/ateam_kenobi/test/path_planner_test.cpp +++ b/ateam_kenobi/test/path_planner_test.cpp @@ -24,8 +24,7 @@ #include #include "core/path_planning/path_planner.hpp" -#include "core/types/world.hpp" -#include "core/types/robot.hpp" +#include "core/types/state_types.hpp" #include #include diff --git a/ateam_kenobi/test/robot_assignment_test.cpp b/ateam_kenobi/test/robot_assignment_test.cpp index ad3fd42f4..6ca6f319a 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,8 @@ 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 +119,8 @@ 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 +166,16 @@ 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 +197,10 @@ 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))))); } 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/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/CMakeLists.txt b/state_tracking/ateam_game_state/CMakeLists.txt new file mode 100644 index 000000000..d9c9d4254 --- /dev/null +++ b/state_tracking/ateam_game_state/CMakeLists.txt @@ -0,0 +1,83 @@ +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(ateam_radio_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 + src/game_state_tracker/double_touch_evaluator.cpp + src/game_state_tracker/in_play_evaluator.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 + ateam_radio_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/ateam_kenobi/src/core/types/ball.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp similarity index 86% rename from ateam_kenobi/src/core/types/ball.hpp rename to state_tracking/ateam_game_state/include/ateam_game_state/ball.hpp index ceb70e569..683860a1a 100644 --- a/ateam_kenobi/src/core/types/ball.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/ball.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,13 +19,13 @@ // THE SOFTWARE. -#ifndef CORE__TYPES__BALL_HPP_ -#define CORE__TYPES__BALL_HPP_ +#ifndef ATEAM_GAME_STATE__BALL_HPP_ +#define ATEAM_GAME_STATE__BALL_HPP_ #include #include -namespace ateam_kenobi +namespace ateam_game_state { struct Ball { @@ -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 // CORE__TYPES__BALL_HPP_ +#endif // ATEAM_GAME_STATE__BALL_HPP_ diff --git a/ateam_kenobi/src/core/types/field.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/field.hpp similarity index 84% rename from ateam_kenobi/src/core/types/field.hpp rename to state_tracking/ateam_game_state/include/ateam_game_state/field.hpp index 6ad998d4e..221a81702 100644 --- a/ateam_kenobi/src/core/types/field.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/field.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 @@ -18,19 +18,18 @@ // 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_ +#ifndef ATEAM_GAME_STATE__FIELD_HPP_ +#define ATEAM_GAME_STATE__FIELD_HPP_ #include #include -namespace ateam_kenobi +namespace ateam_game_state { struct FieldSidedInfo { std::array defense_area_corners; std::array goal_corners; - std::array goal_posts; }; struct Field { @@ -41,10 +40,13 @@ 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; FieldSidedInfo theirs; }; -} // namespace ateam_kenobi +} // namespace ateam_game_state -#endif // CORE__TYPES__FIELD_HPP_ +#endif // ATEAM_GAME_STATE__FIELD_HPP_ diff --git a/ateam_kenobi/src/core/types/referee_info.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp similarity index 84% rename from ateam_kenobi/src/core/types/referee_info.hpp rename to state_tracking/ateam_game_state/include/ateam_game_state/referee_info.hpp index cc39cd3d0..a419969a6 100644 --- a/ateam_kenobi/src/core/types/referee_info.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/referee_info.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,27 +19,27 @@ // THE SOFTWARE. -#ifndef CORE__TYPES__REFEREE_INFO_HPP_ -#define CORE__TYPES__REFEREE_INFO_HPP_ +#ifndef ATEAM_GAME_STATE__REFEREE_INFO_HPP_ +#define ATEAM_GAME_STATE__REFEREE_INFO_HPP_ #include #include #include #include -namespace ateam_kenobi +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::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; std::optional next_command = std::nullopt; std::chrono::system_clock::time_point command_time; }; -} // namespace ateam_kenobi +} // namespace ateam_game_state -#endif // CORE__TYPES__REFEREE_INFO_HPP_ +#endif // ATEAM_GAME_STATE__REFEREE_INFO_HPP_ diff --git a/ateam_kenobi/src/core/types/robot.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp similarity index 89% rename from ateam_kenobi/src/core/types/robot.hpp rename to state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp index 1b9690a1b..2ca8858e0 100644 --- a/ateam_kenobi/src/core/types/robot.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/robot.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,12 +19,12 @@ // THE SOFTWARE. -#ifndef CORE__TYPES__ROBOT_HPP_ -#define CORE__TYPES__ROBOT_HPP_ +#ifndef ATEAM_GAME_STATE__ROBOT_HPP_ +#define ATEAM_GAME_STATE__ROBOT_HPP_ #include -namespace ateam_kenobi +namespace ateam_game_state { struct Robot { @@ -51,6 +51,6 @@ struct Robot return visible && radio_connected; } }; -} // namespace ateam_kenobi +} // namespace ateam_game_state -#endif // CORE__TYPES__ROBOT_HPP_ +#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..58e92ecd5 --- /dev/null +++ b/state_tracking/ateam_game_state/include/ateam_game_state/type_adapters.hpp @@ -0,0 +1,109 @@ +// 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::GameStateWorld; + + 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::GameStateBall; + + 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::GameStateRobot; + + 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::GameStateWorld); +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(ateam_game_state::Ball, + ateam_msgs::msg::GameStateBall); +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(ateam_game_state::Robot, + ateam_msgs::msg::GameStateRobot); +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/ateam_kenobi/src/core/types/world.hpp b/state_tracking/ateam_game_state/include/ateam_game_state/world.hpp similarity index 78% rename from ateam_kenobi/src/core/types/world.hpp rename to state_tracking/ateam_game_state/include/ateam_game_state/world.hpp index aa5c1e8c4..f150473ae 100644 --- a/ateam_kenobi/src/core/types/world.hpp +++ b/state_tracking/ateam_game_state/include/ateam_game_state/world.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,19 +19,19 @@ // THE SOFTWARE. -#ifndef CORE__TYPES__WORLD_HPP_ -#define CORE__TYPES__WORLD_HPP_ +#ifndef ATEAM_GAME_STATE__WORLD_HPP_ +#define ATEAM_GAME_STATE__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" +#include "ball.hpp" +#include "field.hpp" +#include "referee_info.hpp" +#include "robot.hpp" -namespace ateam_kenobi +namespace ateam_game_state { struct World { @@ -45,16 +45,10 @@ struct World 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 +} // namespace ateam_game_state -#endif // CORE__TYPES__WORLD_HPP_ +#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..c5ee69d45 --- /dev/null +++ b/state_tracking/ateam_game_state/package.xml @@ -0,0 +1,28 @@ + + + + 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 + ateam_radio_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/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..aaf59111a --- /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_EVALUATOR_HPP_ +#define GAME_STATE_TRACKER__DOUBLE_TOUCH_EVALUATOR_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_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 new file mode 100644 index 000000000..be88fe4df --- /dev/null +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -0,0 +1,258 @@ +// 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 +#include +#include +#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; +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) + { + InitializeRobotIds(); + + 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)); + + create_indexed_subscribers( + blue_bot_subs_, + Topics::kBlueTeamRobotPrefix, + 1, + &GameStateTracker::BlueVisionBotCallback, + this); + + create_indexed_subscribers( + yellow_bot_subs_, + Topics::kYellowTeamRobotPrefix, + 1, + &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", 100.0)), + std::bind(&GameStateTracker::TimerCallback, this)); + } + +private: + DoubleTouchEvaluator double_touch_evaluator_; + InPlayEvaluator in_play_evaluator_; + 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_; + std::array::SharedPtr, + 16> robot_feedback_subs_; + std::array::SharedPtr, + 16> robot_connection_status_subs_; + std::array::SharedPtr, + 16> robot_command_subs_; + // TODO(barulicm): Move GCListener type to ateam_game_state package + ateam_common::GameControllerListener gc_listener_; + 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 & msg) + { + 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; + 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) + { + 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::VisionStateRobot::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::VisionStateRobot::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 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); + world_.referee_info.their_goalie_id = gc_listener_.GetTheirGoalieID().value_or(-1); + world_.referee_info.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() + { + world_.current_time = std::chrono::steady_clock::now(); + UpdateRefInfo(); + double_touch_evaluator_.Update(world_); + in_play_evaluator_.Update(world_); + 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/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..871f30366 --- /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_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 new file mode 100644 index 000000000..d100a8253 --- /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_EVALUATOR_HPP_ +#define GAME_STATE_TRACKER__IN_PLAY_EVALUATOR_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_EVALUATOR_HPP_ 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..1649134eb --- /dev/null +++ b/state_tracking/ateam_game_state/src/type_adapters.cpp @@ -0,0 +1,301 @@ +// 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.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::GameStateRobot()); + } + } + + 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::GameStateRobot()); + } + } + + 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.ball, 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; + ros_msg.last_visible_time = + rclcpp::Time( + std::chrono::duration_cast( + ball.last_visible_time.time_since_epoch()).count()); +} + +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; + 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.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( + 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.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::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; + ros_msg.defense_area_width = field.defense_area_width; + ros_msg.defense_area_depth = field.defense_area_depth; + ros_msg.center_circle.x = field.center_circle_center.x(); + ros_msg.center_circle.y = field.center_circle_center.y(); + ros_msg.center_circle_radius = field.center_circle_radius; + + auto convert_point_array = [&](auto & in_array, auto & out_vector) { + out_vector.reserve(in_array.size()); + std::transform( + in_array.begin(), in_array.end(), std::back_inserter(out_vector), + [&](auto & val)->geometry_msgs::msg::Point32 { + return geometry_msgs::build().x(val.x()).y(val.y()).z(0); + }); + }; + + 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( + 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; + 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) { + 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.game_stage); + ros_msg.running_command = static_cast(ref_info.running_command); + ros_msg.prev_command = static_cast(ref_info.prev_command); + + 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, + 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.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); + + 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)); +}