From 8b1aa38a60480bbfc826b02deb88465cb45d6ebc Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 20 Apr 2025 20:11:14 -0400 Subject: [PATCH 01/44] first steps --- launch/soccer.launch.py | 7 ++ rj_gameplay/rj_gameplay.egg-info/PKG-INFO | 12 +++ rj_gameplay/rj_gameplay.egg-info/SOURCES.txt | 90 +++++++++++++++++++ .../rj_gameplay.egg-info/dependency_links.txt | 1 + .../rj_gameplay.egg-info/entry_points.txt | 3 + rj_gameplay/rj_gameplay.egg-info/requires.txt | 1 + .../rj_gameplay.egg-info/top_level.txt | 2 + rj_gameplay/rj_gameplay.egg-info/zip-safe | 1 + 8 files changed, 117 insertions(+) create mode 100644 rj_gameplay/rj_gameplay.egg-info/PKG-INFO create mode 100644 rj_gameplay/rj_gameplay.egg-info/SOURCES.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/dependency_links.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/entry_points.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/requires.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/top_level.txt create mode 100644 rj_gameplay/rj_gameplay.egg-info/zip-safe diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index f8f992d0f84..461b4ecacb5 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -237,6 +237,13 @@ def generate_launch_description(): output="screen", parameters=[param_config_filepath], on_exit=Shutdown(), + ), + Node( + package="rj_robocup", + executable="marking_node" + output="screen", + parameters=[param_config_filepath], + on_exit=Shutdown(), ) ] ) diff --git a/rj_gameplay/rj_gameplay.egg-info/PKG-INFO b/rj_gameplay/rj_gameplay.egg-info/PKG-INFO new file mode 100644 index 00000000000..85e86975ea7 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/PKG-INFO @@ -0,0 +1,12 @@ +Metadata-Version: 2.1 +Name: rj-gameplay +Version: 0.0.0 +Summary: Rewrite of the gameplay library. +Home-page: UNKNOWN +Maintainer: oswinso +Maintainer-email: oswinso@gmail.com +License: UNKNOWN +Platform: UNKNOWN + +UNKNOWN + diff --git a/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt b/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt new file mode 100644 index 00000000000..83aa7c4406b --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt @@ -0,0 +1,90 @@ +README.md +setup.cfg +setup.py +rj_gameplay/__init__.py +rj_gameplay/gameplay_node.py +rj_gameplay/play_selector.py +rj_gameplay.egg-info/PKG-INFO +rj_gameplay.egg-info/SOURCES.txt +rj_gameplay.egg-info/dependency_links.txt +rj_gameplay.egg-info/entry_points.txt +rj_gameplay.egg-info/requires.txt +rj_gameplay.egg-info/top_level.txt +rj_gameplay.egg-info/zip-safe +rj_gameplay/eval/__init__.py +rj_gameplay/play/__init__.py +rj_gameplay/play/defend_restart.py +rj_gameplay/play/defense.py +rj_gameplay/play/defensive_clear.py +rj_gameplay/play/keepaway.py +rj_gameplay/play/kickoff_play.py +rj_gameplay/play/offense.py +rj_gameplay/play/penalty_defense.py +rj_gameplay/play/penalty_offense.py +rj_gameplay/play/prep_penalty_offense.py +rj_gameplay/play/restart.py +rj_gameplay/play/test_motion_planning.py +rj_gameplay/role/__init__.py +rj_gameplay/role/capture_role.py +rj_gameplay/role/dumb_move.py +rj_gameplay/role/goalie_role.py +rj_gameplay/role/marker.py +rj_gameplay/role/passer.py +rj_gameplay/role/receiver.py +rj_gameplay/role/seeker.py +rj_gameplay/role/striker.py +rj_gameplay/situation/__init__.py +rj_gameplay/situation/decision_tree/__init__.py +rj_gameplay/situation/decision_tree/plays.py +rj_gameplay/skill/__init__.py +rj_gameplay/skill/capture.py +rj_gameplay/skill/dribble.py +rj_gameplay/skill/intercept.py +rj_gameplay/skill/kick.py +rj_gameplay/skill/line_kick.py +rj_gameplay/skill/mark.py +rj_gameplay/skill/move.py +rj_gameplay/skill/pivot.py +rj_gameplay/skill/pivot_kick.py +rj_gameplay/skill/receive.py +rj_gameplay/skill/settle.py +rj_gameplay/tactic/__init__.py +rj_gameplay/tactic/clear_tactic.py +rj_gameplay/tactic/dumb_tactic.py +rj_gameplay/tactic/goalie_tactic.py +rj_gameplay/tactic/line_tactic.py +rj_gameplay/tactic/move_tactic.py +rj_gameplay/tactic/nmark_tactic.py +rj_gameplay/tactic/pass_tactic.py +rj_gameplay/tactic/prep_move.py +rj_gameplay/tactic/seek.py +rj_gameplay/tactic/striker_tactic.py +rj_gameplay/tactic/wall_tactic.py +stp/__init__.py +stp/global_parameters.py +stp/local_parameters.py +stp/pylint_stp.py +stp/rc.py +stp/testing.py +stp/action/__init__.py +stp/formations/__init__.py +stp/formations/diamond_formation.py +stp/formations/x_formation.py +stp/play/__init__.py +stp/play/pure_play.py +stp/role/__init__.py +stp/role/constraint.py +stp/role/cost.py +stp/role/assignment/__init__.py +stp/role/assignment/naive.py +stp/situation/__init__.py +stp/skill/__init__.py +stp/skill/action_behavior.py +stp/tactic/__init__.py +stp/utils/__init__.py +stp/utils/constants.py +stp/utils/enum.py +stp/utils/fsm.py +stp/utils/pass_seeker_optimizer.py +stp/utils/typed_key_dict.py +stp/utils/world_state_converter.py \ No newline at end of file diff --git a/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt b/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt new file mode 100644 index 00000000000..8b137891791 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/rj_gameplay/rj_gameplay.egg-info/entry_points.txt b/rj_gameplay/rj_gameplay.egg-info/entry_points.txt new file mode 100644 index 00000000000..a641d186309 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/entry_points.txt @@ -0,0 +1,3 @@ +[console_scripts] +gameplay_node = rj_gameplay.gameplay_node:main + diff --git a/rj_gameplay/rj_gameplay.egg-info/requires.txt b/rj_gameplay/rj_gameplay.egg-info/requires.txt new file mode 100644 index 00000000000..49fe098d9e6 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/requires.txt @@ -0,0 +1 @@ +setuptools diff --git a/rj_gameplay/rj_gameplay.egg-info/top_level.txt b/rj_gameplay/rj_gameplay.egg-info/top_level.txt new file mode 100644 index 00000000000..9efb5b023a4 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/top_level.txt @@ -0,0 +1,2 @@ +rj_gameplay +stp diff --git a/rj_gameplay/rj_gameplay.egg-info/zip-safe b/rj_gameplay/rj_gameplay.egg-info/zip-safe new file mode 100644 index 00000000000..8b137891791 --- /dev/null +++ b/rj_gameplay/rj_gameplay.egg-info/zip-safe @@ -0,0 +1 @@ + From ef5bc01537484cd9ad0c725bed552ed69dcca017 Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 20 Apr 2025 20:21:12 -0400 Subject: [PATCH 02/44] added required messages for marking --- src/rj_msgs/CMakeLists.txt | 2 ++ src/rj_msgs/msg/Marking.msg | 2 ++ src/rj_msgs/srv/Marking.srv | 4 ++++ 3 files changed, 8 insertions(+) create mode 100644 src/rj_msgs/msg/Marking.msg create mode 100644 src/rj_msgs/srv/Marking.srv diff --git a/src/rj_msgs/CMakeLists.txt b/src/rj_msgs/CMakeLists.txt index 8d7117d82f6..2abb75b3ae3 100644 --- a/src/rj_msgs/CMakeLists.txt +++ b/src/rj_msgs/CMakeLists.txt @@ -59,6 +59,7 @@ rosidl_generate_interfaces( # Coordinators msg/KickerPicker.msg + msg/Marking.msg # Agent Request Messages request/JoinWallRequest.msg @@ -84,6 +85,7 @@ rosidl_generate_interfaces( # Services srv/AgentCommunication.srv srv/KickerPicker.srv + srv/Marking.srv srv/ListJoysticks.srv srv/PlanHypotheticalPath.srv srv/QuickCommands.srv diff --git a/src/rj_msgs/msg/Marking.msg b/src/rj_msgs/msg/Marking.msg new file mode 100644 index 00000000000..0d37b1c577b --- /dev/null +++ b/src/rj_msgs/msg/Marking.msg @@ -0,0 +1,2 @@ +# This is a message to pick a robot to mark +uint8 mark_robot_id # ID of the robot to mark on the opposing team \ No newline at end of file diff --git a/src/rj_msgs/srv/Marking.srv b/src/rj_msgs/srv/Marking.srv new file mode 100644 index 00000000000..26d71d880dd --- /dev/null +++ b/src/rj_msgs/srv/Marking.srv @@ -0,0 +1,4 @@ +# Request a robot to be added to or removed from the marking group +uint8 robot_id # ID of the robot (0 to kNumShells-1) +--- +bool success # Always true \ No newline at end of file From 74e795698e1de52aedf3691626e86f2016a1e937 Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 20 Apr 2025 20:46:07 -0400 Subject: [PATCH 03/44] more setup --- .../soccer/strategy/coordinator/marking.cpp | 61 +++++++++++++++++++ .../soccer/strategy/coordinator/marking.hpp | 40 ++++++++++++ .../strategy/coordinator/marking_main.cpp | 11 ++++ 3 files changed, 112 insertions(+) create mode 100644 soccer/src/soccer/strategy/coordinator/marking.cpp create mode 100644 soccer/src/soccer/strategy/coordinator/marking.hpp create mode 100644 soccer/src/soccer/strategy/coordinator/marking_main.cpp diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp new file mode 100644 index 00000000000..c6f21bc1bb5 --- /dev/null +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -0,0 +1,61 @@ +#include "marking.hpp" + +#include // for std::any_of +#include + +#include +#include +#include + +namespace strategy { + +Marking::Marking() + : Coordinator("marking_srv", "marking_data", "marking_node") { + // Subscribe to world state + world_state_sub_ = this->create_subscription( + vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), + [this](rj_msgs::msg::WorldState::SharedPtr world_state) { // NOLINT + last_world_state_ = rj_convert::convert_from_ros(*world_state); + publish_marking_list(); + }); +} + +void KickerPicker::service_callback(RequestPtr request, ResponsePtr response) { + // complete logic + // bool membership_changed = wants_to_kick_by_id_[request->robot_id] != request->wants_to_kick; + + // wants_to_kick_by_id_[request->robot_id] = request->wants_to_kick; + + // if (membership_changed) { + // publish_selected_kicker(); + // } + + // response->success = true; +} + +void KickerPicker::publish_selected_kicker() { + // Find closest robot to ball among group members + double min_distance = std::numeric_limits::infinity(); + uint8_t selected_kicker = kInvalidRobotId; + + const auto& ball_pos = last_world_state_.ball.position; + + for (uint8_t i = 0; i < kNumShells; ++i) { + if (wants_to_kick_by_id_[i]) { + const auto& robot = last_world_state_.get_robot(true, i); + double distance = ball_pos.dist_to(robot.pose.position()); + if (distance < min_distance) { + min_distance = distance; + selected_kicker = i; + } + } + } + + // Only publish if the selected kicker has changed + if (selected_kicker != last_published_kicker_) { + publisher_->publish(rj_msgs::msg::KickerPicker().set__robot_id(selected_kicker)); + last_published_kicker_ = selected_kicker; + } +} + +} // namespace strategy diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp new file mode 100644 index 00000000000..ae6a3ba914e --- /dev/null +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include + +#include + +#include +#include +#include + +#include "coordinator.hpp" +#include "rj_constants/constants.hpp" +#include "world_state.hpp" + +namespace strategy { + +class Marking + : public Coordinator { +public: + static constexpr uint8_t kInvalidRobotId = kNumShells; + + Marking(); + ~Marking() override = default; + Marking(const Marking&) = delete; + Marking& operator=(const Marking&) = delete; + Marking(Marking&&) = delete; + Marking& operator=(Marking&&) = delete; + + void service_callback(RequestPtr request, ResponsePtr response); + +private: + void publish_marking_list(); + + std::array marking_list{}; // TODO: Initialize it to invalid robot id + std::array valid_targets{}; // 0 initialized, no one is a valid target initially + WorldState last_world_state_; + rclcpp::Subscription::SharedPtr world_state_sub_; +}; + +} // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/coordinator/marking_main.cpp b/soccer/src/soccer/strategy/coordinator/marking_main.cpp new file mode 100644 index 00000000000..497b6d649d9 --- /dev/null +++ b/soccer/src/soccer/strategy/coordinator/marking_main.cpp @@ -0,0 +1,11 @@ +#include + +#include "marking.hpp" + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From a5487ae2c95e3bdd81f3391ea65f8f86dd1b54ff Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 20 Apr 2025 20:48:48 -0400 Subject: [PATCH 04/44] fixed some stuff --- .../soccer/strategy/coordinator/marking.cpp | 53 ++++++++++--------- 1 file changed, 29 insertions(+), 24 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index c6f21bc1bb5..1f184726721 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -20,7 +20,7 @@ Marking::Marking() }); } -void KickerPicker::service_callback(RequestPtr request, ResponsePtr response) { +void Marking::service_callback(RequestPtr request, ResponsePtr response) { // complete logic // bool membership_changed = wants_to_kick_by_id_[request->robot_id] != request->wants_to_kick; @@ -33,29 +33,34 @@ void KickerPicker::service_callback(RequestPtr request, ResponsePtr response) { // response->success = true; } -void KickerPicker::publish_selected_kicker() { - // Find closest robot to ball among group members - double min_distance = std::numeric_limits::infinity(); - uint8_t selected_kicker = kInvalidRobotId; - - const auto& ball_pos = last_world_state_.ball.position; - - for (uint8_t i = 0; i < kNumShells; ++i) { - if (wants_to_kick_by_id_[i]) { - const auto& robot = last_world_state_.get_robot(true, i); - double distance = ball_pos.dist_to(robot.pose.position()); - if (distance < min_distance) { - min_distance = distance; - selected_kicker = i; - } - } - } - - // Only publish if the selected kicker has changed - if (selected_kicker != last_published_kicker_) { - publisher_->publish(rj_msgs::msg::KickerPicker().set__robot_id(selected_kicker)); - last_published_kicker_ = selected_kicker; - } +void Marking::publish_marking_list() { + // Find all eligible targets and update the list + // Formula, closer to 0 is more priority: + // Danger value = 0.2 * distance to goal + 0.4 * distance to ball + 0.7 * distance to closest RJ robot + // Danger value <= 5 gets you onto the eligible target list (subject to change) + + + // double min_distance = std::numeric_limits::infinity(); + // uint8_t selected_kicker = kInvalidRobotId; + + // const auto& ball_pos = last_world_state_.ball.position; + + // for (uint8_t i = 0; i < kNumShells; ++i) { + // if (wants_to_kick_by_id_[i]) { + // const auto& robot = last_world_state_.get_robot(true, i); + // double distance = ball_pos.dist_to(robot.pose.position()); + // if (distance < min_distance) { + // min_distance = distance; + // selected_kicker = i; + // } + // } + // } + + // // Only publish if the selected kicker has changed + // if (selected_kicker != last_published_kicker_) { + // publisher_->publish(rj_msgs::msg::KickerPicker().set__robot_id(selected_kicker)); + // last_published_kicker_ = selected_kicker; + // } } } // namespace strategy From dfe621e3fac3a5572eae9984159795db4a9a4474 Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 20 Apr 2025 21:09:58 -0400 Subject: [PATCH 05/44] added a dangerous score array to keep track of valid targets --- soccer/src/soccer/strategy/coordinator/marking.cpp | 2 ++ soccer/src/soccer/strategy/coordinator/marking.hpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index 1f184726721..45d4a539be3 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -12,6 +12,8 @@ namespace strategy { Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state + marking_list.fill(kInvalidRobotId); // initializes to no valid markers + dangeruss_score.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score world_state_sub_ = this->create_subscription( vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), [this](rj_msgs::msg::WorldState::SharedPtr world_state) { // NOLINT diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp index ae6a3ba914e..cac49698149 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -31,8 +31,8 @@ class Marking private: void publish_marking_list(); - std::array marking_list{}; // TODO: Initialize it to invalid robot id - std::array valid_targets{}; // 0 initialized, no one is a valid target initially + std::array marking_list{}; // Initialize it to invalid robot id in constructor + std::array dangeruss_score{}; // infinity initialized in constructor, no one is a valid target initially WorldState last_world_state_; rclcpp::Subscription::SharedPtr world_state_sub_; }; From 01e44365d7aa6fa7c00553cd6e8ba676909b3689 Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 22 Apr 2025 19:37:02 -0400 Subject: [PATCH 06/44] made some progress --- soccer/src/soccer/strategy/coordinator/marking.cpp | 10 ++++++++++ soccer/src/soccer/strategy/coordinator/marking.hpp | 3 +++ src/rj_constants/include/rj_constants/constants.hpp | 1 + 3 files changed, 14 insertions(+) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index 45d4a539be3..295713e5b98 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -65,4 +65,14 @@ void Marking::publish_marking_list() { // } } +void Marking::update_danger_scores() { + rj_geometry::Point ball_pos = last_world_state_.ball.position; + + + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(true, i); + } +} + + } // namespace strategy diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp index cac49698149..383994d3c63 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include "coordinator.hpp" @@ -30,10 +31,12 @@ class Marking private: void publish_marking_list(); + void update_danger_scores(); std::array marking_list{}; // Initialize it to invalid robot id in constructor std::array dangeruss_score{}; // infinity initialized in constructor, no one is a valid target initially WorldState last_world_state_; + FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; rclcpp::Subscription::SharedPtr world_state_sub_; }; diff --git a/src/rj_constants/include/rj_constants/constants.hpp b/src/rj_constants/include/rj_constants/constants.hpp index 8a11ff79f79..ef110e0b37b 100644 --- a/src/rj_constants/include/rj_constants/constants.hpp +++ b/src/rj_constants/include/rj_constants/constants.hpp @@ -7,6 +7,7 @@ /// all weights in kilograms // Number of identifiable robots on one team +// IF U CHANGE THIS NUMBER CHECK Marking.msg constexpr size_t kNumShells = 16; // Number of playing robots on one team From 6301a5b233ae6f3b97f8a7016c57e2b903a69720 Mon Sep 17 00:00:00 2001 From: Aalind Date: Sat, 26 Apr 2025 11:47:39 -0400 Subject: [PATCH 07/44] danger score calcs --- soccer/src/soccer/strategy/coordinator/marking.cpp | 14 ++++++++------ soccer/src/soccer/strategy/coordinator/marking.hpp | 2 +- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index 295713e5b98..5aefe902d76 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -13,7 +13,7 @@ Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state marking_list.fill(kInvalidRobotId); // initializes to no valid markers - dangeruss_score.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score + danger_score.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score world_state_sub_ = this->create_subscription( vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), [this](rj_msgs::msg::WorldState::SharedPtr world_state) { // NOLINT @@ -66,12 +66,14 @@ void Marking::publish_marking_list() { } void Marking::update_danger_scores() { - rj_geometry::Point ball_pos = last_world_state_.ball.position; - + // const auto& ball_pos = last_world_state_.ball.position; - for (uint8_t i = 0; i < kNumShells; i++) { - const auto& robot = last_world_state_.get_robot(true, i); - } + // for (uint8_t i = 0; i < kNumShells; i++) { + // const auto& robot = last_world_state_.get_robot(false, i); + // double dist_to_ball_ = ball_pos.dist_to(robot.pose.position()); + // //double dist_to_goal_ = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); + + // } } diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp index 383994d3c63..8c8c4e3cd2c 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -34,7 +34,7 @@ class Marking void update_danger_scores(); std::array marking_list{}; // Initialize it to invalid robot id in constructor - std::array dangeruss_score{}; // infinity initialized in constructor, no one is a valid target initially + std::array danger_score{}; // infinity initialized in constructor, no one is a valid target initially WorldState last_world_state_; FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; rclcpp::Subscription::SharedPtr world_state_sub_; From c85f542e77ec40e22443339ea40dde49fd46e08a Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sat, 26 Apr 2025 11:46:13 -0400 Subject: [PATCH 08/44] more marking stuff --- .../soccer/strategy/coordinator/marking.cpp | 130 ++++++++++++------ .../soccer/strategy/coordinator/marking.hpp | 11 +- src/rj_msgs/msg/Marking.msg | 2 +- 3 files changed, 95 insertions(+), 48 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index 5aefe902d76..ebb56ebd10b 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -13,7 +13,9 @@ Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state marking_list.fill(kInvalidRobotId); // initializes to no valid markers - danger_score.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score + enemey_to_friends_.fill(kInvalidRobotId); + danger_score_.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score + num_markers_ = 0; world_state_sub_ = this->create_subscription( vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), [this](rj_msgs::msg::WorldState::SharedPtr world_state) { // NOLINT @@ -23,57 +25,97 @@ Marking::Marking() } void Marking::service_callback(RequestPtr request, ResponsePtr response) { - // complete logic - // bool membership_changed = wants_to_kick_by_id_[request->robot_id] != request->wants_to_kick; - - // wants_to_kick_by_id_[request->robot_id] = request->wants_to_kick; - - // if (membership_changed) { - // publish_selected_kicker(); - // } - - // response->success = true; + if (num_markers_ < kMaxMarkers) { + int most_dangerous = kInvalidRobotId; + double min = std::numeric_limits::infinity(); + for (size_t i = 0; i < danger_score_.size(); ++i) { + if (enemey_to_friends_[i] != kInvalidRobotId) { + continue; + } + if (danger_score[i] < min) { + most_dangerous = i; + min = danger_score[i]; + } + } + if (most_dangerous != kInvalidRobotId) { + enemey_to_friends_[most_dangerous] = request->robot_id; + marking_list_[request->robot_id] = most_dangerous; + num_markers++; + } + } else { + // should we kick someone out + double better_distance = 0; + int kick_out_this_robot_id = kInvalidRobotId; + const auto& robot_requesting = last_world_state_.get_robot(true, request->robot_id); + for (size_t i = 0; i < marking_list_.size(); ++i) { + if (marking_list_[i] != kInvalidRobotId) { + int enemey_id = marking_list_[i]; + const auto& i_robot = last_world_state_.get_robot(true, i); + const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); + double dist = (i_robot.pose.position().dist_to(enemey_robot.pose.position())) - (robot_requesting.pose.position().dist_to(enemey_robot.pose.position()); + if (dist > better_distance) { + better_distance = dist; + kick_out_this_robot_id = i; + } + } + } + if (kick_out_this_robot_id != kInvalidRobotId) { + int enemey_id = marking_list_[kick_out_this_robot_id]; + marking_list_[kick_out_this_robot_id] = kInvalidRobotId; + enemey_to_friends_[enemey_id] = request->robot_id; + marking_list_[request->robot_id] = enemey_id; + } + } + + response->success = true; } void Marking::publish_marking_list() { - // Find all eligible targets and update the list - // Formula, closer to 0 is more priority: - // Danger value = 0.2 * distance to goal + 0.4 * distance to ball + 0.7 * distance to closest RJ robot - // Danger value <= 5 gets you onto the eligible target list (subject to change) - - - // double min_distance = std::numeric_limits::infinity(); - // uint8_t selected_kicker = kInvalidRobotId; - - // const auto& ball_pos = last_world_state_.ball.position; - - // for (uint8_t i = 0; i < kNumShells; ++i) { - // if (wants_to_kick_by_id_[i]) { - // const auto& robot = last_world_state_.get_robot(true, i); - // double distance = ball_pos.dist_to(robot.pose.position()); - // if (distance < min_distance) { - // min_distance = distance; - // selected_kicker = i; - // } - // } - // } - - // // Only publish if the selected kicker has changed - // if (selected_kicker != last_published_kicker_) { - // publisher_->publish(rj_msgs::msg::KickerPicker().set__robot_id(selected_kicker)); - // last_published_kicker_ = selected_kicker; - // } + // make this run on a timer + update_danger_scores(); + + // reshuffle, only change one because on timer so will get others later + int most_dangerous = kInvalidRobotId; + double min = std::numeric_limits::infinity(); + for (size_t i = 0; i < danger_score_.size(); ++i) { + if (enemey_to_friends_[i] != kInvalidRobotId) { + continue; + } + if (danger_score[i] < min) { + most_dangerous = i; + min = danger_score[i]; + } + } + if (most_dangerous != kInvalidRobotId) { + int not_dangerous_robot_id = kInvalidRobotId; + double max_danger_sub = 0.0; + for (size_t i = 0; i < marking_list_.size(); ++i) { + if (marking_list_[i] != kInvalidRobotId) { + int enemey_id = marking_list_[i]; + double danger_sub = danger_score[enemey_id] - danger_score[most_dangerous]; + if (danger_sub > max_danger_sub) { + max_danger_sub = danger_sub; + not_dangerous_robot_id = enemey_id; + } + } + } + if (not_dangerous_robot_id != kInvalidRobotId) { + int friend_id = enemey_to_friends_[not_dangerous_robot_id]; + enemey_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; + marking_list_[friend_id] = most_dangerous; + enemey_to_friends_[most_dangerous] = friend_id; + } + } + + publisher_->publish(rj_msgs::msg::Marking().set__mark_robot_ids(marking_list_)); } void Marking::update_danger_scores() { // const auto& ball_pos = last_world_state_.ball.position; - // for (uint8_t i = 0; i < kNumShells; i++) { - // const auto& robot = last_world_state_.get_robot(false, i); - // double dist_to_ball_ = ball_pos.dist_to(robot.pose.position()); - // //double dist_to_goal_ = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); - - // } + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(true, i); + } } diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp index 8c8c4e3cd2c..35f76295410 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -33,11 +33,16 @@ class Marking void publish_marking_list(); void update_danger_scores(); - std::array marking_list{}; // Initialize it to invalid robot id in constructor - std::array danger_score{}; // infinity initialized in constructor, no one is a valid target initially + static constexpr int kMaxMarkers = 2; + static constexpr double kSuperDangerSub = 3.1415926535; + int num_markers_; + + std::array marking_list_{}; // Initialize it to invalid robot id in constructor + std::array danger_score_{}; // infinity initialized in constructor, no one is a valid target initially + std::array enemey_to_friends_{}; WorldState last_world_state_; FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; rclcpp::Subscription::SharedPtr world_state_sub_; }; -} // namespace strategy \ No newline at end of file +} // namespace strategy diff --git a/src/rj_msgs/msg/Marking.msg b/src/rj_msgs/msg/Marking.msg index 0d37b1c577b..01098ec5180 100644 --- a/src/rj_msgs/msg/Marking.msg +++ b/src/rj_msgs/msg/Marking.msg @@ -1,2 +1,2 @@ # This is a message to pick a robot to mark -uint8 mark_robot_id # ID of the robot to mark on the opposing team \ No newline at end of file +uint8[16] mark_robot_ids # ID of the robot to mark on the opposing team From e83d40244a91da4831f73c2c1c0bb811d9018e03 Mon Sep 17 00:00:00 2001 From: Aalind Date: Sat, 26 Apr 2025 12:12:35 -0400 Subject: [PATCH 09/44] update naming changes --- .../src/soccer/strategy/coordinator/marking.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index ebb56ebd10b..cc69dff5c85 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -12,7 +12,7 @@ namespace strategy { Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state - marking_list.fill(kInvalidRobotId); // initializes to no valid markers + marking_list_.fill(kInvalidRobotId); // initializes to no valid markers enemey_to_friends_.fill(kInvalidRobotId); danger_score_.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score num_markers_ = 0; @@ -32,15 +32,15 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { if (enemey_to_friends_[i] != kInvalidRobotId) { continue; } - if (danger_score[i] < min) { + if (danger_score_[i] < min) { most_dangerous = i; - min = danger_score[i]; + min = danger_score_[i]; } } if (most_dangerous != kInvalidRobotId) { enemey_to_friends_[most_dangerous] = request->robot_id; marking_list_[request->robot_id] = most_dangerous; - num_markers++; + num_markers_++; } } else { // should we kick someone out @@ -52,7 +52,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { int enemey_id = marking_list_[i]; const auto& i_robot = last_world_state_.get_robot(true, i); const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); - double dist = (i_robot.pose.position().dist_to(enemey_robot.pose.position())) - (robot_requesting.pose.position().dist_to(enemey_robot.pose.position()); + double dist = (i_robot.pose.position().dist_to(enemey_robot.pose.position())) - (robot_requesting.pose.position().dist_to(enemey_robot.pose.position())); if (dist > better_distance) { better_distance = dist; kick_out_this_robot_id = i; @@ -81,9 +81,9 @@ void Marking::publish_marking_list() { if (enemey_to_friends_[i] != kInvalidRobotId) { continue; } - if (danger_score[i] < min) { + if (danger_score_[i] < min) { most_dangerous = i; - min = danger_score[i]; + min = danger_score_[i]; } } if (most_dangerous != kInvalidRobotId) { @@ -92,7 +92,7 @@ void Marking::publish_marking_list() { for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] != kInvalidRobotId) { int enemey_id = marking_list_[i]; - double danger_sub = danger_score[enemey_id] - danger_score[most_dangerous]; + double danger_sub = danger_score_[enemey_id] - danger_score_[most_dangerous]; if (danger_sub > max_danger_sub) { max_danger_sub = danger_sub; not_dangerous_robot_id = enemey_id; From b501fe6b0f586f2d5582b292ff1ad2f283162b56 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sat, 26 Apr 2025 12:13:27 -0400 Subject: [PATCH 10/44] can leave marking --- .../soccer/strategy/coordinator/marking.cpp | 46 +++++++++++++++++-- .../soccer/strategy/coordinator/marking.hpp | 1 + src/rj_msgs/srv/Marking.srv | 3 +- 3 files changed, 45 insertions(+), 5 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index cc69dff5c85..3c3016ab5f3 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -25,6 +25,37 @@ Marking::Marking() } void Marking::service_callback(RequestPtr request, ResponsePtr response) { + if (request->join == false) { + bool marking = (marking_list_[request->robot_id] != kInvalidRobotId); + if (marking) { + int enemey_id = marking_list_[request->robot_id]; + marking_list_[request->robot_id] = kInvalidRobotId; + enemey_to_friends_[enemey_id] = kInvalidRobotId; + num_markers--; + // replace if possible + const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); + double min = std::numeric_limits::infinity(); + int waiting_robot_id = kInvalidRobotId; + for (size_t i = 0; i < queue_.size(); ++i) { + const auto& i_robot = last_world_state_.get_robot(true, queue_[i]); + double distance = i_robot.pose.position().dist_to(enemey_robot.pose.position()); + if (distance < min) { + min = distance; + waiting_robot_id = queue_[i]; + } + } + if (kInvalidRobotId != waiting_robot_id) { + queue_.erase(std::remove(queue_.begin(), queue_.end(), waiting_robot_id), queue_.end()); + marking_list_[waiting_robot_id] = enemey_id; + enemey_to_friends_[enemey_id] = waiting_robot_id; + num_markers++; + } + } else { + queue_.erase(std::remove(queue_.begin(), queue_.end(), request->robot_id), queue_.end()); + } + response->success = true; + return; + } if (num_markers_ < kMaxMarkers) { int most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); @@ -40,7 +71,9 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { if (most_dangerous != kInvalidRobotId) { enemey_to_friends_[most_dangerous] = request->robot_id; marking_list_[request->robot_id] = most_dangerous; - num_markers_++; + num_markers++; + } else { + queue_.push_back(request->robot_id); } } else { // should we kick someone out @@ -64,6 +97,8 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { marking_list_[kick_out_this_robot_id] = kInvalidRobotId; enemey_to_friends_[enemey_id] = request->robot_id; marking_list_[request->robot_id] = enemey_id; + } else { + queue_.push_back(request->robot_id); } } @@ -113,9 +148,12 @@ void Marking::publish_marking_list() { void Marking::update_danger_scores() { // const auto& ball_pos = last_world_state_.ball.position; - for (uint8_t i = 0; i < kNumShells; i++) { - const auto& robot = last_world_state_.get_robot(true, i); - } + // for (uint8_t i = 0; i < kNumShells; i++) { + // const auto& robot = last_world_state_.get_robot(false, i); + // double dist_to_ball_ = ball_pos.dist_to(robot.pose.position()); + // //double dist_to_goal_ = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); + + // } } diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp index 35f76295410..508166a10ff 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -40,6 +40,7 @@ class Marking std::array marking_list_{}; // Initialize it to invalid robot id in constructor std::array danger_score_{}; // infinity initialized in constructor, no one is a valid target initially std::array enemey_to_friends_{}; + std::vector queue_; WorldState last_world_state_; FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; rclcpp::Subscription::SharedPtr world_state_sub_; diff --git a/src/rj_msgs/srv/Marking.srv b/src/rj_msgs/srv/Marking.srv index 26d71d880dd..93f10863daa 100644 --- a/src/rj_msgs/srv/Marking.srv +++ b/src/rj_msgs/srv/Marking.srv @@ -1,4 +1,5 @@ # Request a robot to be added to or removed from the marking group uint8 robot_id # ID of the robot (0 to kNumShells-1) +bool join --- -bool success # Always true \ No newline at end of file +bool success # Always true From ded69acf3df4e0d3736e6066690ef71a2af24c53 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sat, 26 Apr 2025 12:43:43 -0400 Subject: [PATCH 11/44] fixed linker error --- .../soccer/strategy/coordinator/marking.cpp | 26 +++++++++---------- .../soccer/strategy/coordinator/marking.hpp | 8 +++--- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index 3c3016ab5f3..88ea9ba443b 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -28,14 +28,14 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { if (request->join == false) { bool marking = (marking_list_[request->robot_id] != kInvalidRobotId); if (marking) { - int enemey_id = marking_list_[request->robot_id]; + uint8_t enemey_id = marking_list_[request->robot_id]; marking_list_[request->robot_id] = kInvalidRobotId; enemey_to_friends_[enemey_id] = kInvalidRobotId; - num_markers--; + num_markers_--; // replace if possible const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); double min = std::numeric_limits::infinity(); - int waiting_robot_id = kInvalidRobotId; + uint8_t waiting_robot_id = kInvalidRobotId; for (size_t i = 0; i < queue_.size(); ++i) { const auto& i_robot = last_world_state_.get_robot(true, queue_[i]); double distance = i_robot.pose.position().dist_to(enemey_robot.pose.position()); @@ -48,7 +48,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { queue_.erase(std::remove(queue_.begin(), queue_.end(), waiting_robot_id), queue_.end()); marking_list_[waiting_robot_id] = enemey_id; enemey_to_friends_[enemey_id] = waiting_robot_id; - num_markers++; + num_markers_++; } } else { queue_.erase(std::remove(queue_.begin(), queue_.end(), request->robot_id), queue_.end()); @@ -57,7 +57,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { return; } if (num_markers_ < kMaxMarkers) { - int most_dangerous = kInvalidRobotId; + uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); for (size_t i = 0; i < danger_score_.size(); ++i) { if (enemey_to_friends_[i] != kInvalidRobotId) { @@ -71,18 +71,18 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { if (most_dangerous != kInvalidRobotId) { enemey_to_friends_[most_dangerous] = request->robot_id; marking_list_[request->robot_id] = most_dangerous; - num_markers++; + num_markers_++; } else { queue_.push_back(request->robot_id); } } else { // should we kick someone out double better_distance = 0; - int kick_out_this_robot_id = kInvalidRobotId; + uint8_t kick_out_this_robot_id = kInvalidRobotId; const auto& robot_requesting = last_world_state_.get_robot(true, request->robot_id); for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] != kInvalidRobotId) { - int enemey_id = marking_list_[i]; + uint8_t enemey_id = marking_list_[i]; const auto& i_robot = last_world_state_.get_robot(true, i); const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); double dist = (i_robot.pose.position().dist_to(enemey_robot.pose.position())) - (robot_requesting.pose.position().dist_to(enemey_robot.pose.position())); @@ -93,7 +93,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { } } if (kick_out_this_robot_id != kInvalidRobotId) { - int enemey_id = marking_list_[kick_out_this_robot_id]; + uint8_t enemey_id = marking_list_[kick_out_this_robot_id]; marking_list_[kick_out_this_robot_id] = kInvalidRobotId; enemey_to_friends_[enemey_id] = request->robot_id; marking_list_[request->robot_id] = enemey_id; @@ -110,7 +110,7 @@ void Marking::publish_marking_list() { update_danger_scores(); // reshuffle, only change one because on timer so will get others later - int most_dangerous = kInvalidRobotId; + uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); for (size_t i = 0; i < danger_score_.size(); ++i) { if (enemey_to_friends_[i] != kInvalidRobotId) { @@ -122,11 +122,11 @@ void Marking::publish_marking_list() { } } if (most_dangerous != kInvalidRobotId) { - int not_dangerous_robot_id = kInvalidRobotId; + uint8_t not_dangerous_robot_id = kInvalidRobotId; double max_danger_sub = 0.0; for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] != kInvalidRobotId) { - int enemey_id = marking_list_[i]; + uint8_t enemey_id = marking_list_[i]; double danger_sub = danger_score_[enemey_id] - danger_score_[most_dangerous]; if (danger_sub > max_danger_sub) { max_danger_sub = danger_sub; @@ -135,7 +135,7 @@ void Marking::publish_marking_list() { } } if (not_dangerous_robot_id != kInvalidRobotId) { - int friend_id = enemey_to_friends_[not_dangerous_robot_id]; + uint8_t friend_id = enemey_to_friends_[not_dangerous_robot_id]; enemey_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; marking_list_[friend_id] = most_dangerous; enemey_to_friends_[most_dangerous] = friend_id; diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp index 508166a10ff..5fe39fd7489 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -37,10 +37,10 @@ class Marking static constexpr double kSuperDangerSub = 3.1415926535; int num_markers_; - std::array marking_list_{}; // Initialize it to invalid robot id in constructor - std::array danger_score_{}; // infinity initialized in constructor, no one is a valid target initially - std::array enemey_to_friends_{}; - std::vector queue_; + std::array marking_list_{}; // Initialize it to invalid robot id in constructor + std::array danger_score_{}; // infinity initialized in constructor, no one is a valid target initially + std::array enemey_to_friends_{}; + std::vector queue_; WorldState last_world_state_; FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; rclcpp::Subscription::SharedPtr world_state_sub_; From 20b41d0dce1a91844851532dad3b5a98f9f12b18 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sat, 26 Apr 2025 12:47:17 -0400 Subject: [PATCH 12/44] started client --- .../strategy/coordinator/marking_client.cpp | 117 ++++++++++++++++++ .../strategy/coordinator/marking_client.hpp | 72 +++++++++++ 2 files changed, 189 insertions(+) create mode 100644 soccer/src/soccer/strategy/coordinator/marking_client.cpp create mode 100644 soccer/src/soccer/strategy/coordinator/marking_client.hpp diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.cpp b/soccer/src/soccer/strategy/coordinator/marking_client.cpp new file mode 100644 index 00000000000..35b6e9e78d3 --- /dev/null +++ b/soccer/src/soccer/strategy/coordinator/marking_client.cpp @@ -0,0 +1,117 @@ +#pragma once + +#include "kicker_picker_client.hpp" + +#include + +#include +#include + +#include +#include + +#include "kicker_picker.hpp" + +namespace strategy { + +/** + * @brief Client for interacting with the KickerPicker coordinator. + * + * Manages membership in the kicker group and tracks the currently selected kicker. + */ + +KickerPickerClient::KickerPickerClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) + : node_{std::move(node)}, robot_id_{robot_id}, selected_kicker_{KickerPicker::kInvalidRobotId} { + client_ = node_->create_client("kicker_picker_srv"); +} + +void KickerPickerClient::join_group(StatusCallback callback) { + if (am_i_member_) { + return; + } + + if (!client_->wait_for_service(std::chrono::seconds(1))) { + SPDLOG_ERROR("KickerPicker service not available."); + if (callback) { + callback(Result{false}); + } + return; + } + + auto request = std::make_shared(); + request->robot_id = robot_id_; + request->wants_to_kick = true; + + client_->async_send_request( + request, [this, callback = std::move(callback)]( + rclcpp::Client::SharedFuture + future) { // 6 NOLINT(performance-unnecessary-value-param) -- + // ROS2 async callbacks require value capture. + if (!future.valid() || !future.get()->success) { + if (callback) { + callback(Result{false}); + } + return; + } + + am_i_member_ = true; + + // Create subscription to track selected kicker. + subscription_ = node_->create_subscription( + "kicker_picker_data", rclcpp::QoS(1).transient_local(), + [this, + callback = std::move(callback)](const rj_msgs::msg::KickerPicker::SharedPtr msg) { + selected_kicker_ = msg->robot_id; + if (callback) { + callback(Result{true, selected_kicker_}); + } + }); + }); +} + +void KickerPickerClient::leave_group(StatusCallback callback) { + if (!am_i_member_) { + if (callback) { + callback(Result{false}); + } + return; + } + + auto request = std::make_shared(); + request->robot_id = robot_id_; + request->wants_to_kick = false; + + client_->async_send_request( + request, [this, callback = std::move(callback)]( + rclcpp::Client::SharedFuture + future) { // 6 NOLINT(performance-unnecessary-value-param) -- + // ROS2 async callbacks require value capture. + if (!future.valid() || !future.get()->success) { + if (callback) { + callback(Result{false}); + } + return; + } + + am_i_member_ = false; + + // Resetting the shared ptr releases our pointer to the + // subscription. ROS only keeps a weak_ptr, so this will + // deallocate the subscription. The callback will no longer be + // called. + subscription_.reset(); + selected_kicker_ = KickerPicker::kInvalidRobotId; + + if (callback) { + callback(Result{false}); + } + }); +} + +bool KickerPickerClient::am_i_member() const { return am_i_member_; } + +uint8_t KickerPickerClient::selected_kicker() const { return selected_kicker_; } + +bool KickerPickerClient::is_selected() const { return selected_kicker_ == robot_id_; } + +} // namespace strategy diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.hpp b/soccer/src/soccer/strategy/coordinator/marking_client.hpp new file mode 100644 index 00000000000..25e34a4a7fd --- /dev/null +++ b/soccer/src/soccer/strategy/coordinator/marking_client.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include + +#include + +#include +#include + +namespace strategy { + +/** + * @brief Client for interacting with the KickerPicker coordinator. + * + * Manages membership in the kicker group and tracks the currently selected kicker. + */ +class MarkingClient { +public: + struct Result { + bool am_i_member{false}; // Whether this robot is currently a member of the kicker group. + std::optional kicker_id{0}; // ID of Kicker id + }; + + using StatusCallback = std::function; + + explicit MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id); + ~MarkingClient() = default; + MarkingClient(const MarkingClient&) = delete; + MarkingClient& operator=(const MarkingClient&) = delete; + MarkingClient(MarkingClient&&) = delete; + MarkingClient& operator=(MarkingClient&&) = delete; + + /** + * @brief Join the kicker group. + * @param callback Called with current membership status after attempt to join. + */ + void join_group(StatusCallback callback = nullptr); + + /** + * @brief Leave the kicker group. + * @param callback Called with current membership status after attempt to leave. + */ + void leave_group(StatusCallback callback = nullptr); + + /** + * @brief Check if this robot is a member of the kicker group. + */ + [[nodiscard]] bool am_i_member() const; + + /** + * @brief Get the currently selected kicker. + * @return robot ID of selected kicker, or kInvalidRobotId if none selected. + */ + [[nodiscard]] uint8_t selected_kicker() const; + + /** + * @brief Check if this robot is currently selected as the kicker. + */ + [[nodiscard]] bool is_selected() const; + +private: + rclcpp::Node::SharedPtr node_; + const uint8_t robot_id_; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) -- class + // isn't move/copy-able anyway + rclcpp::Client::SharedPtr client_; + rclcpp::Subscription::SharedPtr subscription_; + + bool am_i_member_{false}; + uint8_t selected_kicker_; +}; + +} // namespace strategy From 787f99b250cb3d50ffb5437dd9c76c3638d88fc8 Mon Sep 17 00:00:00 2001 From: Squid6578 Date: Sat, 26 Apr 2025 14:49:26 -0400 Subject: [PATCH 13/44] marking client should be done --- .../strategy/coordinator/marking_client.cpp | 54 +++++++++---------- .../strategy/coordinator/marking_client.hpp | 25 +++++---- 2 files changed, 41 insertions(+), 38 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.cpp b/soccer/src/soccer/strategy/coordinator/marking_client.cpp index 35b6e9e78d3..0225c45f04f 100644 --- a/soccer/src/soccer/strategy/coordinator/marking_client.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking_client.cpp @@ -1,16 +1,14 @@ -#pragma once - -#include "kicker_picker_client.hpp" +#include "marking_client.hpp" #include #include #include -#include -#include +#include +#include -#include "kicker_picker.hpp" +#include "marking.hpp" namespace strategy { @@ -20,31 +18,31 @@ namespace strategy { * Manages membership in the kicker group and tracks the currently selected kicker. */ -KickerPickerClient::KickerPickerClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) - : node_{std::move(node)}, robot_id_{robot_id}, selected_kicker_{KickerPicker::kInvalidRobotId} { - client_ = node_->create_client("kicker_picker_srv"); +MarkingClient::MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) + : node_{std::move(node)}, robot_id_{robot_id}, selected_robot_marking_id_{MarkingClient::kInvalidRobotId} { + client_ = node_->create_client("marking_srv"); } -void KickerPickerClient::join_group(StatusCallback callback) { +void MarkingClient::join_group(StatusCallback callback) { if (am_i_member_) { return; } if (!client_->wait_for_service(std::chrono::seconds(1))) { - SPDLOG_ERROR("KickerPicker service not available."); + SPDLOG_ERROR("Marking service not available."); if (callback) { callback(Result{false}); } return; } - auto request = std::make_shared(); + auto request = std::make_shared(); request->robot_id = robot_id_; - request->wants_to_kick = true; + request->join = true; client_->async_send_request( request, [this, callback = std::move(callback)]( - rclcpp::Client::SharedFuture + rclcpp::Client::SharedFuture future) { // 6 NOLINT(performance-unnecessary-value-param) -- // ROS2 async callbacks require value capture. if (!future.valid() || !future.get()->success) { @@ -57,19 +55,20 @@ void KickerPickerClient::join_group(StatusCallback callback) { am_i_member_ = true; // Create subscription to track selected kicker. - subscription_ = node_->create_subscription( - "kicker_picker_data", rclcpp::QoS(1).transient_local(), + subscription_ = node_->create_subscription( + "marking_data", rclcpp::QoS(1).transient_local(), [this, - callback = std::move(callback)](const rj_msgs::msg::KickerPicker::SharedPtr msg) { - selected_kicker_ = msg->robot_id; + callback = std::move(callback)](const rj_msgs::msg::Marking::SharedPtr msg) { + selected_robot_marking_id_ = msg->mark_robot_ids[robot_id_]; + am_i_marking_ = (selected_robot_marking_id_ != kInvalidRobotId); if (callback) { - callback(Result{true, selected_kicker_}); + callback(Result{true, am_i_marking_, selected_robot_marking_id_}); } }); }); } -void KickerPickerClient::leave_group(StatusCallback callback) { +void MarkingClient::leave_group(StatusCallback callback) { if (!am_i_member_) { if (callback) { callback(Result{false}); @@ -77,13 +76,13 @@ void KickerPickerClient::leave_group(StatusCallback callback) { return; } - auto request = std::make_shared(); + auto request = std::make_shared(); request->robot_id = robot_id_; - request->wants_to_kick = false; + request->join = false; client_->async_send_request( request, [this, callback = std::move(callback)]( - rclcpp::Client::SharedFuture + rclcpp::Client::SharedFuture future) { // 6 NOLINT(performance-unnecessary-value-param) -- // ROS2 async callbacks require value capture. if (!future.valid() || !future.get()->success) { @@ -100,7 +99,8 @@ void KickerPickerClient::leave_group(StatusCallback callback) { // deallocate the subscription. The callback will no longer be // called. subscription_.reset(); - selected_kicker_ = KickerPicker::kInvalidRobotId; + am_i_marking_ = kInvalidRobotId; + selected_robot_marking_id_ = kInvalidRobotId; if (callback) { callback(Result{false}); @@ -108,10 +108,10 @@ void KickerPickerClient::leave_group(StatusCallback callback) { }); } -bool KickerPickerClient::am_i_member() const { return am_i_member_; } +bool MarkingClient::am_i_member() const { return am_i_member_; } -uint8_t KickerPickerClient::selected_kicker() const { return selected_kicker_; } +uint8_t MarkingClient::who_am_i_marking() const { return selected_robot_marking_id_; } -bool KickerPickerClient::is_selected() const { return selected_kicker_ == robot_id_; } +bool MarkingClient::am_i_marking() const { return am_i_marking_; } } // namespace strategy diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.hpp b/soccer/src/soccer/strategy/coordinator/marking_client.hpp index 25e34a4a7fd..c87d6affc9a 100644 --- a/soccer/src/soccer/strategy/coordinator/marking_client.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking_client.hpp @@ -4,8 +4,9 @@ #include -#include -#include +#include +#include +#include "rj_constants/constants.hpp" namespace strategy { @@ -16,9 +17,11 @@ namespace strategy { */ class MarkingClient { public: + static constexpr uint8_t kInvalidRobotId = kNumShells; struct Result { bool am_i_member{false}; // Whether this robot is currently a member of the kicker group. - std::optional kicker_id{0}; // ID of Kicker id + std::optional am_i_marking{false}; + std::optional who_i_am_marking{kInvalidRobotId}; // ID of Kicker id }; using StatusCallback = std::function; @@ -43,20 +46,20 @@ class MarkingClient { void leave_group(StatusCallback callback = nullptr); /** - * @brief Check if this robot is a member of the kicker group. + * @brief Check if this robot is a member of the marker group. */ [[nodiscard]] bool am_i_member() const; /** - * @brief Get the currently selected kicker. - * @return robot ID of selected kicker, or kInvalidRobotId if none selected. + * @brief Get the currently selected enemey robot id marking. + * @return robot ID of selected robot, or kInvalidRobotId if none selected. */ - [[nodiscard]] uint8_t selected_kicker() const; + [[nodiscard]] uint8_t who_am_i_marking() const; /** - * @brief Check if this robot is currently selected as the kicker. + * @brief Check if this robot is currently marking. */ - [[nodiscard]] bool is_selected() const; + [[nodiscard]] bool am_i_marking() const; private: rclcpp::Node::SharedPtr node_; @@ -64,9 +67,9 @@ class MarkingClient { // isn't move/copy-able anyway rclcpp::Client::SharedPtr client_; rclcpp::Subscription::SharedPtr subscription_; - bool am_i_member_{false}; - uint8_t selected_kicker_; + bool am_i_marking_{false}; + uint8_t selected_robot_marking_id_{kInvalidRobotId}; }; } // namespace strategy From 32f886084e40d498f982aca7f68494a654f31c67 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 27 Apr 2025 19:53:30 -0400 Subject: [PATCH 14/44] started on danger score calculation --- .../soccer/strategy/coordinator/marking.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index 88ea9ba443b..57391c751a9 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -110,6 +110,8 @@ void Marking::publish_marking_list() { update_danger_scores(); // reshuffle, only change one because on timer so will get others later + + // finding most dangerous of non-marked robots uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); for (size_t i = 0; i < danger_score_.size(); ++i) { @@ -134,7 +136,8 @@ void Marking::publish_marking_list() { } } } - if (not_dangerous_robot_id != kInvalidRobotId) { + // seeing if most dangerous of non-marked robots is significantly more dangerous than any marked robot + if (max_danger_sub > kSuperDangerSub && not_dangerous_robot_id != kInvalidRobotId) { uint8_t friend_id = enemey_to_friends_[not_dangerous_robot_id]; enemey_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; marking_list_[friend_id] = most_dangerous; @@ -146,14 +149,23 @@ void Marking::publish_marking_list() { } void Marking::update_danger_scores() { - // const auto& ball_pos = last_world_state_.ball.position; - // for (uint8_t i = 0; i < kNumShells; i++) { - // const auto& robot = last_world_state_.get_robot(false, i); - // double dist_to_ball_ = ball_pos.dist_to(robot.pose.position()); - // //double dist_to_goal_ = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); + // danger score calculation is distance_to_ball * constant + distance_to_goal * constant - distance_from_our_closest_robot * constant - danger_angle * constant + // lower danger score is more dangerous + + const auto& ball_pos = last_world_state_.ball.position; + + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(false, i); + if (!robot.visible) { + continue; + } + double dist_to_ball_ = ball_pos.dist_to(robot.pose.position()); + double dist_to_goal_ = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); + - // } + + } } From 64ef7d45997278c51c6f33abfe5f313417d91d09 Mon Sep 17 00:00:00 2001 From: petergarud Date: Mon, 28 Apr 2025 02:58:45 -0400 Subject: [PATCH 15/44] marking coordaintor should be done' --- launch/soccer.launch.py | 2 +- .../soccer/strategy/coordinator/marking.cpp | 28 ++++++++++++++++--- .../soccer/strategy/coordinator/marking.hpp | 4 +++ .../strategy/coordinator/marking_client.hpp | 4 +-- 4 files changed, 31 insertions(+), 7 deletions(-) diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index 461b4ecacb5..1558a2f4143 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -240,7 +240,7 @@ def generate_launch_description(): ), Node( package="rj_robocup", - executable="marking_node" + executable="marking_node", output="screen", parameters=[param_config_filepath], on_exit=Shutdown(), diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/soccer/src/soccer/strategy/coordinator/marking.cpp index 57391c751a9..d3f94dc56dc 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking.cpp @@ -160,11 +160,31 @@ void Marking::update_danger_scores() { if (!robot.visible) { continue; } - double dist_to_ball_ = ball_pos.dist_to(robot.pose.position()); - double dist_to_goal_ = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); - - + double dist_to_ball = ball_pos.dist_to(robot.pose.position()); + double dist_to_goal = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); + double min = std::numeric_limits::infinity(); + for (uint8_t j = 0; j < kNumShells; j++) { + const auto& i_friend = last_world_state_.get_robot(true, j); + if (!robot.visible) { + continue; + } + double dist = robot.pose.position().dist_to(i_friend.pose.position()); + if (dist < min) { + min = dist; + } + } + const auto& goal_to_ball = ball_pos - field_dimensions_.our_goal_loc(); + const auto& goal_to_robot = robot.pose.position() - field_dimensions_.our_goal_loc(); + double cosTheta = goal_to_ball.dot(goal_to_robot) / (goal_to_ball.mag() * goal_to_robot.mag()); + // Clamp value to [-1, 1] to avoid domain errors due to floating point precision + if (cosTheta > 1.0) cosTheta = 1.0; + if (cosTheta < -1.0) cosTheta = -1.0; + double angle_between = std::acos(cosTheta); // returns radians + angle_between = std::abs(angle_between); + + double danger_score = dist_to_ball * kDangerDistToBall + dist_to_goal * kDangerDistToGoal - min * kDangerDistToOurRobots - angle_between * kDangerAngle; + danger_score_[i] = danger_score; } } diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/soccer/src/soccer/strategy/coordinator/marking.hpp index 5fe39fd7489..f0a543759ed 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking.hpp @@ -35,6 +35,10 @@ class Marking static constexpr int kMaxMarkers = 2; static constexpr double kSuperDangerSub = 3.1415926535; + static constexpr double kDangerDistToBall = 1.0; + static constexpr double kDangerDistToGoal = 2.0; + static constexpr double kDangerDistToOurRobots = 3.0; + static constexpr double kDangerAngle = 2.0; int num_markers_; std::array marking_list_{}; // Initialize it to invalid robot id in constructor diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.hpp b/soccer/src/soccer/strategy/coordinator/marking_client.hpp index c87d6affc9a..fc48f1eb1b5 100644 --- a/soccer/src/soccer/strategy/coordinator/marking_client.hpp +++ b/soccer/src/soccer/strategy/coordinator/marking_client.hpp @@ -34,13 +34,13 @@ class MarkingClient { MarkingClient& operator=(MarkingClient&&) = delete; /** - * @brief Join the kicker group. + * @brief Join the marking group. * @param callback Called with current membership status after attempt to join. */ void join_group(StatusCallback callback = nullptr); /** - * @brief Leave the kicker group. + * @brief Leave the marking group. * @param callback Called with current membership status after attempt to leave. */ void leave_group(StatusCallback callback = nullptr); From 15f1b61d1cba4f7f9d9da6131888b6e03ef86683 Mon Sep 17 00:00:00 2001 From: petergarud Date: Tue, 29 Apr 2025 21:31:50 -0400 Subject: [PATCH 16/44] Struct of coordinators: compiles but fails to run at all --- .../include/rj_strategy/agent/position.hpp | 9 +++++++ .../rj_strategy/agent/position/defense.hpp | 4 ++++ .../agent/position/free_kicker.hpp | 4 ++++ .../agent/position/goal_kicker.hpp | 5 +++- .../rj_strategy/agent/position/goalie.hpp | 4 ++++ .../rj_strategy/agent/position/idle.hpp | 4 ++++ .../rj_strategy/agent/position/offense.hpp | 4 ++++ .../agent/position/penalty_non_kicker.hpp | 4 ++++ .../agent/position/penalty_player.hpp | 4 ++++ .../rj_strategy/agent/position/pivot_test.hpp | 2 ++ .../agent/position/robot_factory_position.hpp | 5 ++-- .../rj_strategy/agent/position/seeker.hpp | 4 ++++ .../agent/position/smartidling.hpp | 4 ++++ .../agent/position/solo_offense.hpp | 4 ++++ .../rj_strategy/agent/position/zoner.hpp | 4 ++++ .../src/agent/position/defense.cpp | 7 ++++++ .../src/agent/position/free_kicker.cpp | 5 ++++ .../src/agent/position/goal_kicker.cpp | 3 ++- src/rj_strategy/src/agent/position/goalie.cpp | 4 ++++ src/rj_strategy/src/agent/position/idle.cpp | 4 ++++ .../src/agent/position/offense.cpp | 6 +++++ .../src/agent/position/penalty_non_kicker.cpp | 4 ++++ .../src/agent/position/penalty_player.cpp | 4 ++++ .../src/agent/position/pivot_test.cpp | 2 ++ .../agent/position/robot_factory_position.cpp | 24 ++++++++++--------- src/rj_strategy/src/agent/position/seeker.cpp | 2 ++ .../src/agent/position/smartidling.cpp | 4 ++++ .../src/agent/position/solo_offense.cpp | 6 +++++ src/rj_strategy/src/agent/position/zoner.cpp | 5 ++++ 29 files changed, 130 insertions(+), 15 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position.hpp b/src/rj_strategy/include/rj_strategy/agent/position.hpp index a40d234763e..928bf459223 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position.hpp @@ -40,11 +40,20 @@ #include #include +// Coordinators +#include "strategy/coordinator/kicker_picker_client.hpp" +#include "strategy/coordinator/marking_client.hpp" + // tell compiler this class exists, but no need to import the whole header class AgentActionClient; namespace strategy { +struct ClientHandles { + std::unique_ptr kickerPickerClient; + std::unique_ptr markingClient; +}; + /* * Position is an abstract superclass. Its subclasses handle strategy logic. * The goal is to isolate the strategy logic from the ROS interfacing. This diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 19d0d224200..31da1d8e605 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -28,6 +28,8 @@ namespace strategy { class Defense : public Position { public: Defense(int r_id); + Defense(int r_id, std::shared_ptr clientHandles); + Defense(const Position& other, std::shared_ptr clientHandles); ~Defense() override = default; Defense(const Position& other); @@ -122,6 +124,8 @@ class Defense : public Position { int get_marker_target_id(); Marker marker_; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp index 0671ef1eb8b..803438a6185 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp @@ -18,6 +18,8 @@ namespace strategy { class FreeKicker : public Position { public: FreeKicker(int r_id); + FreeKicker(int r_id, std::shared_ptr clientHandles); + FreeKicker(const Position& other, std::shared_ptr clientHandles); FreeKicker(const Position&); ~FreeKicker() = default; @@ -38,6 +40,8 @@ class FreeKicker : public Position { private: std::optional derived_get_task(RobotIntent intent) override; + std::shared_ptr clientHandles_; + }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp index 278630d6da4..3dd06109628 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp @@ -21,6 +21,7 @@ class GoalKicker : public Position { public: GoalKicker(int r_id); ~GoalKicker() = default; + GoalKicker(int r_id, std::shared_ptr clientHandles); /** * @brief Does nothing; this position is a special case @@ -39,6 +40,8 @@ class GoalKicker : public Position { private: std::optional derived_get_task(RobotIntent intent) override; + + std::shared_ptr clientHandles_; }; -} // namespace strategy \ No newline at end of file +} // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp b/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp index ee86a7a1d35..bede3596c84 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp @@ -23,6 +23,8 @@ namespace strategy { class Goalie : public Position { public: Goalie(int r_id); + Goalie(int r_id, std::shared_ptr clientHandles); + Goalie(const Position& other, std::shared_ptr clientHandles); ~Goalie() override = default; Goalie(const Position& other); @@ -87,6 +89,8 @@ class Goalie : public Position { State latest_state_ = IDLING; rj_geometry::Point penalty_location(); + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp b/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp index fce38230b85..f192ac0768f 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp @@ -10,6 +10,8 @@ class Idle : public Position { Idle(int r_id); ~Idle() = default; Idle(const Position& other); + Idle(int r_id, std::shared_ptr clientHandles); + Idle(const Position& other, std::shared_ptr clientHandles); /** * @brief Does nothing; this position is a special case @@ -28,5 +30,7 @@ class Idle : public Position { private: std::optional derived_get_task(RobotIntent intent) override; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp index a8d287bcea1..2d23fa5feb7 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp @@ -30,6 +30,8 @@ class Offense : public Position { Offense(int r_id); ~Offense() override = default; Offense(const Position& other); + Offense(int r_id, std::shared_ptr clientHandles); + Offense(const Position& other, std::shared_ptr clientHandles); communication::PosAgentResponseWrapper receive_communication_request( communication::AgentPosRequestWrapper request) override; @@ -241,6 +243,8 @@ class Offense : public Position { void broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding); std::unordered_map seeker_points_; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp index 7f4dd39326f..8244e3132aa 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp @@ -20,6 +20,8 @@ class PenaltyNonKicker : public Position { PenaltyNonKicker(int r_id); ~PenaltyNonKicker() = default; PenaltyNonKicker(const Position& other); + PenaltyNonKicker(int r_id, std::shared_ptr clientHandles); + PenaltyNonKicker(const Position& other, std::shared_ptr clientHandles); /** * @brief Does nothing; this position is a special case @@ -51,5 +53,7 @@ class PenaltyNonKicker : public Position { std::optional state_to_task(RobotIntent intent); // State latest_state_ = GET_AWAY; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp b/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp index b124db42378..023bb862d1f 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp @@ -23,6 +23,8 @@ class PenaltyPlayer : public Position { PenaltyPlayer(int r_id); ~PenaltyPlayer() = default; PenaltyPlayer(const Position& other); + PenaltyPlayer(int r_id, std::shared_ptr clientHandles); + PenaltyPlayer(const Position& other, std::shared_ptr clientHandles); /** * @brief Does nothing; this position is a special case @@ -88,6 +90,8 @@ class PenaltyPlayer : public Position { // current state of Goalie (state machine) State latest_state_ = START; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp b/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp index 45532449788..a2564f4e224 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp @@ -26,6 +26,8 @@ class Pivot : public Position { ~Pivot() override = default; Pivot(const Pivot& other) = default; Pivot(Pivot&& other) = default; + Pivot(int r_id, std::shared_ptr clientHandles); + Pivot(const Position& other, std::shared_ptr clientHandles); Pivot& operator=(const Pivot& other) = delete; Pivot& operator=(Pivot&& other) = delete; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp b/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp index 6d445880d5f..112c4c1a846 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp @@ -116,7 +116,6 @@ class RobotFactoryPosition : public Position { private: std::unique_ptr current_position_; - KickerPickerClient kicker_picker_; OverridingPositions override_play_position_{OverridingPositions::AUTO}; std::optional derived_get_task(RobotIntent intent) override; @@ -148,12 +147,14 @@ class RobotFactoryPosition : public Position { // This line requires Pos to implement the constructor Pos(const // Position&) current_position_->die(); - current_position_ = std::make_unique(*current_position_); + current_position_ = std::make_unique(*current_position_, clientHandles_); SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name()); } } bool set_position_override_if_requested(); + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp index 67df9c227fd..7fc88b69cf7 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp @@ -32,6 +32,8 @@ class Seeker : public RoleInterface { Seeker(Seeker&& other) = default; Seeker& operator=(const Seeker& other) = default; Seeker& operator=(Seeker&& other) = default; + Seeker(int r_id, std::shared_ptr clientHandles); + Seeker(const Position& other, std::shared_ptr clientHandles); /** * @brief Returns a seeker behavior which aims to get open @@ -113,6 +115,8 @@ class Seeker : public RoleInterface { const FieldDimensions& field_dimensions) const; std::unordered_map seeker_points_{}; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp b/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp index 70ec3dff211..c25b988a974 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp @@ -10,6 +10,8 @@ class SmartIdle : public Position { SmartIdle(int r_id); ~SmartIdle() = default; SmartIdle(const Position& other); + SmartIdle(int r_id, std::shared_ptr clientHandles); + SmartIdle(const Position& other, std::shared_ptr clientHandles); /** * @brief Does nothing; this position is a special case @@ -28,5 +30,7 @@ class SmartIdle : public Position { private: std::optional derived_get_task(RobotIntent intent) override; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp index a158820edc3..aab7ced26b2 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp @@ -27,6 +27,8 @@ class SoloOffense : public Position { ~SoloOffense() override = default; SoloOffense(const SoloOffense& other) = default; SoloOffense(SoloOffense&& other) = default; + SoloOffense(int r_id, std::shared_ptr clientHandles); + SoloOffense(const Position& other, std::shared_ptr clientHandles); std::string get_current_state() override; @@ -59,6 +61,8 @@ class SoloOffense : public Position { rj_geometry::Point calculate_best_shot() const; double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp b/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp index 3ec810650bb..6614b42559c 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp @@ -32,6 +32,8 @@ class Zoner : public Position { Zoner(Zoner&& other) = default; Zoner& operator=(const Zoner& other) = default; Zoner& operator=(Zoner&& other) = default; + Zoner(int r_id, std::shared_ptr clientHandles); + Zoner(const Position& other, std::shared_ptr clientHandles); private: std::optional derived_get_task(RobotIntent intent) override; @@ -56,6 +58,8 @@ class Zoner : public Position { std::string get_current_state() override; static constexpr double kZonerRadius = 1.5; + + std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 81d1f0730eb..acc1005180b 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -9,6 +9,13 @@ Defense::Defense(const Position& other) : Position{other}, marker_{field_dimensi walling_robots_ = {}; } +Defense::Defense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Defense"), marker_{field_dimensions_}, clientHandles_{clientHandles} {} + +Defense::Defense(const Position& other, std::shared_ptr clientHandles) : Position{other}, marker_{field_dimensions_}, clientHandles_{clientHandles} { + position_name_ = "Defense"; + walling_robots_ = {}; +} + std::optional Defense::derived_get_task(RobotIntent intent) { // SPDLOG_INFO("waller length (sus) {}, {}", walling_robots_.size(), robot_id_); current_state_ = update_state(); diff --git a/src/rj_strategy/src/agent/position/free_kicker.cpp b/src/rj_strategy/src/agent/position/free_kicker.cpp index 29611d40d0e..7b64e1d882a 100644 --- a/src/rj_strategy/src/agent/position/free_kicker.cpp +++ b/src/rj_strategy/src/agent/position/free_kicker.cpp @@ -5,6 +5,11 @@ namespace strategy { FreeKicker::FreeKicker(int r_id) : Position(r_id, "FreeKicker") {} FreeKicker::FreeKicker(const Position& other) : Position{other} { position_name_ = "FreeKicker"; } +FreeKicker::FreeKicker(int r_id, std::shared_ptr clientHandles) : Position(r_id, "FreeKicker"), clientHandles_{clientHandles} {} + +FreeKicker::FreeKicker(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { + position_name_ = "FreeKicker"; +} std::optional FreeKicker::derived_get_task(RobotIntent intent) { // Penalty Kicker kicks the ball into the goal diff --git a/src/rj_strategy/src/agent/position/goal_kicker.cpp b/src/rj_strategy/src/agent/position/goal_kicker.cpp index c98f3cb1089..b0992be16cd 100644 --- a/src/rj_strategy/src/agent/position/goal_kicker.cpp +++ b/src/rj_strategy/src/agent/position/goal_kicker.cpp @@ -3,6 +3,7 @@ namespace strategy { GoalKicker::GoalKicker(int r_id) : Position(r_id, "GoalKicker") {} +GoalKicker::GoalKicker(int r_id, std::shared_ptr clientHandles) : Position(r_id, "GoalKicker"), clientHandles_{clientHandles} {} std::optional GoalKicker::derived_get_task(RobotIntent intent) { // Penalty Kicker kicks the ball into the goal @@ -36,4 +37,4 @@ void GoalKicker::derived_pass_ball() {} void GoalKicker::derived_acknowledge_ball_in_transit() {} -} // namespace strategy \ No newline at end of file +} // namespace strategy diff --git a/src/rj_strategy/src/agent/position/goalie.cpp b/src/rj_strategy/src/agent/position/goalie.cpp index 0b46f420e5c..67e20f1119f 100644 --- a/src/rj_strategy/src/agent/position/goalie.cpp +++ b/src/rj_strategy/src/agent/position/goalie.cpp @@ -6,6 +6,10 @@ Goalie::Goalie(int r_id) : Position(r_id, "Goalie") {} Goalie::Goalie(const Position& other) : Position{other} {} +Goalie::Goalie(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Goalie"), clientHandles_{clientHandles} {} + +Goalie::Goalie(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { } + std::optional Goalie::derived_get_task(RobotIntent intent) { latest_state_ = update_state(); return state_to_task(intent); diff --git a/src/rj_strategy/src/agent/position/idle.cpp b/src/rj_strategy/src/agent/position/idle.cpp index 391dfea1d99..2daf7ca3831 100644 --- a/src/rj_strategy/src/agent/position/idle.cpp +++ b/src/rj_strategy/src/agent/position/idle.cpp @@ -6,6 +6,10 @@ Idle::Idle(int r_id) : Position{r_id, "Idle"} {} Idle::Idle(const Position& other) : Position{other} {} +Idle::Idle(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Idle"), clientHandles_{clientHandles} {} + +Idle::Idle(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} {} + std::string Idle::get_current_state() { return "Idle"; } /** diff --git a/src/rj_strategy/src/agent/position/offense.cpp b/src/rj_strategy/src/agent/position/offense.cpp index b7d74ed646f..68ff1078f9b 100644 --- a/src/rj_strategy/src/agent/position/offense.cpp +++ b/src/rj_strategy/src/agent/position/offense.cpp @@ -8,6 +8,12 @@ Offense::Offense(const Position& other) : Position{other}, seeker_{robot_id_} { position_name_ = "Offense"; } +Offense::Offense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Offense"), seeker_{r_id}, clientHandles_{clientHandles} {} + +Offense::Offense(const Position& other, std::shared_ptr clientHandles) : Position{other}, seeker_{robot_id_}, clientHandles_{clientHandles} { + position_name_ = "Offense"; +} + std::optional Offense::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock State new_state = next_state(); diff --git a/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp b/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp index 12ba56c2fd6..4ca06e66347 100644 --- a/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp +++ b/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp @@ -6,6 +6,10 @@ PenaltyNonKicker::PenaltyNonKicker(int r_id) : Position{r_id, "PenaltyNonKicker" PenaltyNonKicker::PenaltyNonKicker(const Position& other) : Position{other} {} +PenaltyNonKicker::PenaltyNonKicker(int r_id, std::shared_ptr clientHandles) : Position(r_id, "PenaltyNonKicker"), clientHandles_{clientHandles} {} + +PenaltyNonKicker::PenaltyNonKicker(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} {} + std::string PenaltyNonKicker::get_current_state() { return "PenaltyNonKicker"; } /** diff --git a/src/rj_strategy/src/agent/position/penalty_player.cpp b/src/rj_strategy/src/agent/position/penalty_player.cpp index 454b7f68543..493236b853b 100644 --- a/src/rj_strategy/src/agent/position/penalty_player.cpp +++ b/src/rj_strategy/src/agent/position/penalty_player.cpp @@ -7,6 +7,10 @@ PenaltyPlayer::PenaltyPlayer(int r_id) : Position(r_id, "PenaltyPlayer") {} PenaltyPlayer::PenaltyPlayer(const Position& other) : Position{other} {} +PenaltyPlayer::PenaltyPlayer(int r_id, std::shared_ptr clientHandles) : Position(r_id, "PenaltyPlayer"), clientHandles_{clientHandles} {} + +PenaltyPlayer::PenaltyPlayer(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} {} + std::optional PenaltyPlayer::derived_get_task(RobotIntent intent) { latest_state_ = update_state(); return state_to_task(intent); diff --git a/src/rj_strategy/src/agent/position/pivot_test.cpp b/src/rj_strategy/src/agent/position/pivot_test.cpp index 76ea01f1d2b..02adbcef096 100644 --- a/src/rj_strategy/src/agent/position/pivot_test.cpp +++ b/src/rj_strategy/src/agent/position/pivot_test.cpp @@ -3,6 +3,8 @@ namespace strategy { Pivot::Pivot(int r_id) : Position{r_id, "Pivot"} {} +Pivot::Pivot(int r_id, std::shared_ptr clientHandles) : Position{r_id, "Pivot"} {} +Pivot::Pivot(const Position& other, std::shared_ptr clientHandles) : Position{other} {} std::optional Pivot::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock diff --git a/src/rj_strategy/src/agent/position/robot_factory_position.cpp b/src/rj_strategy/src/agent/position/robot_factory_position.cpp index 051b3a5f540..e9f2074fd5a 100644 --- a/src/rj_strategy/src/agent/position/robot_factory_position.cpp +++ b/src/rj_strategy/src/agent/position/robot_factory_position.cpp @@ -3,13 +3,15 @@ namespace strategy { RobotFactoryPosition::RobotFactoryPosition(int r_id, rclcpp::Node::SharedPtr node) - : Position(r_id, "RobotFactoryPosition"), kicker_picker_(std::move(node), r_id) { + : Position(r_id, "RobotFactoryPosition"), clientHandles_(std::shared_ptr()) { + clientHandles_->kickerPickerClient = std::make_unique(node, r_id); + clientHandles_->markingClient = std::make_unique(node, r_id); if (robot_id_ == 0) { - current_position_ = std::make_unique(robot_id_); + current_position_ = std::make_unique(robot_id_, clientHandles_); } else if (robot_id_ == 1 || robot_id_ == 2) { - current_position_ = std::make_unique(robot_id_); + current_position_ = std::make_unique(robot_id_, clientHandles_); } else { - current_position_ = std::make_unique(robot_id_); + current_position_ = std::make_unique(robot_id_, clientHandles_); } } @@ -39,7 +41,7 @@ void RobotFactoryPosition::process_play_state() { case PlayState::State::Playing: { // We just became regular playing. // set_default_position(); - kicker_picker_.leave_group(); + clientHandles_->kickerPickerClient->leave_group(); break; } @@ -69,7 +71,7 @@ void RobotFactoryPosition::process_play_state() { case PlayState::State::Halt: { // The game has been stopped or halted. In this case, we typically want to keep // our current position. The rules for movement should be handled at a lower level. - kicker_picker_.leave_group(); + clientHandles_->kickerPickerClient->leave_group(); handle_stop(); break; } @@ -81,7 +83,7 @@ void RobotFactoryPosition::process_play_state() { void RobotFactoryPosition::handle_stop() { set_default_position(); } void RobotFactoryPosition::handle_penalty_playing() { - if (!(kicker_picker_.am_i_member() && kicker_picker_.is_selected())) { + if (!(clientHandles_->kickerPickerClient->am_i_member() && clientHandles_->kickerPickerClient->is_selected())) { set_current_position(); } } @@ -92,8 +94,8 @@ void RobotFactoryPosition::handle_setup() { // Set up our restart if ((current_play_state_.is_kickoff() || current_play_state_.is_penalty()) && - !kicker_picker_.am_i_member()) { - kicker_picker_.join_group([this](KickerPickerClient::Result result) { + !clientHandles_->kickerPickerClient->am_i_member()) { + clientHandles_->kickerPickerClient->join_group([this](KickerPickerClient::Result result) { if (result.am_i_member && result.kicker_id == robot_id_ && current_play_state_.is_kickoff()) { set_current_position(); @@ -117,9 +119,9 @@ void RobotFactoryPosition::handle_ready() { // Time to kick if (current_play_state_.is_our_restart() && current_play_state_.is_free_kick() && - !kicker_picker_.am_i_member()) { + !clientHandles_->kickerPickerClient->am_i_member()) { // There is no "Setup" stage for free kicks, so this is when we choose kicker - kicker_picker_.join_group([this](KickerPickerClient::Result result) { + clientHandles_->kickerPickerClient->join_group([this](KickerPickerClient::Result result) { if (result.am_i_member && result.kicker_id == robot_id_) { set_current_position(); } else { diff --git a/src/rj_strategy/src/agent/position/seeker.cpp b/src/rj_strategy/src/agent/position/seeker.cpp index 64d748b02e1..f952173f565 100644 --- a/src/rj_strategy/src/agent/position/seeker.cpp +++ b/src/rj_strategy/src/agent/position/seeker.cpp @@ -3,6 +3,8 @@ namespace strategy { Seeker::Seeker(int robot_id) { robot_id_ = robot_id; } +Seeker::Seeker(int r_id, std::shared_ptr clientHandles) : clientHandles_{clientHandles} { robot_id_ = r_id; } +Seeker::Seeker(const Position& other, std::shared_ptr clientHandles) {} std::optional Seeker::get_task(RobotIntent intent, const WorldState* last_world_state, FieldDimensions field_dimensions) { diff --git a/src/rj_strategy/src/agent/position/smartidling.cpp b/src/rj_strategy/src/agent/position/smartidling.cpp index 83a882e1306..1dad60a9c2a 100644 --- a/src/rj_strategy/src/agent/position/smartidling.cpp +++ b/src/rj_strategy/src/agent/position/smartidling.cpp @@ -6,6 +6,10 @@ SmartIdle::SmartIdle(int r_id) : Position{r_id, "SmartIdle"} {} SmartIdle::SmartIdle(const Position& other) : Position{other} {} +SmartIdle::SmartIdle(int r_id, std::shared_ptr clientHandles) : Position(r_id, "SmartIdle"), clientHandles_{clientHandles} {} + +SmartIdle::SmartIdle(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { } + std::string SmartIdle::get_current_state() { return "SmartIdle"; } /** diff --git a/src/rj_strategy/src/agent/position/solo_offense.cpp b/src/rj_strategy/src/agent/position/solo_offense.cpp index 4c87cd82797..6a8f4e28d5e 100644 --- a/src/rj_strategy/src/agent/position/solo_offense.cpp +++ b/src/rj_strategy/src/agent/position/solo_offense.cpp @@ -8,6 +8,12 @@ SoloOffense::SoloOffense(const Position& other) : Position{other} { SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} +SoloOffense::SoloOffense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "SoloOffense"), clientHandles_{clientHandles} {} + +SoloOffense::SoloOffense(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { + position_name_ = "SoloOffense"; +} + std::optional SoloOffense::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock State new_state = next_state(); diff --git a/src/rj_strategy/src/agent/position/zoner.cpp b/src/rj_strategy/src/agent/position/zoner.cpp index 46d5743e6b7..e500e785d5a 100644 --- a/src/rj_strategy/src/agent/position/zoner.cpp +++ b/src/rj_strategy/src/agent/position/zoner.cpp @@ -6,6 +6,11 @@ Zoner::Zoner(int r_id) : Position(r_id, "Zoner") {} Zoner::Zoner(const Position& other) : Position{other} { position_name_ = "Zoner"; } +Zoner::Zoner(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Zoner"), clientHandles_{clientHandles} {} + +Zoner::Zoner(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { + position_name_ = "Zoner"; } + std::optional Zoner::derived_get_task(RobotIntent intent) { current_state_ = next_state(); // waller_id_ = get_waller_id(); From 267f9bd4b9dbe0039b68661061cb59e4fe413c09 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 31 Aug 2025 20:45:38 -0400 Subject: [PATCH 17/44] Fix issue with not running --- src/rj_strategy/src/agent/position/robot_factory_position.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rj_strategy/src/agent/position/robot_factory_position.cpp b/src/rj_strategy/src/agent/position/robot_factory_position.cpp index e9f2074fd5a..3451780d702 100644 --- a/src/rj_strategy/src/agent/position/robot_factory_position.cpp +++ b/src/rj_strategy/src/agent/position/robot_factory_position.cpp @@ -3,7 +3,7 @@ namespace strategy { RobotFactoryPosition::RobotFactoryPosition(int r_id, rclcpp::Node::SharedPtr node) - : Position(r_id, "RobotFactoryPosition"), clientHandles_(std::shared_ptr()) { + : Position(r_id, "RobotFactoryPosition"), clientHandles_(std::make_shared()) { clientHandles_->kickerPickerClient = std::make_unique(node, r_id); clientHandles_->markingClient = std::make_unique(node, r_id); if (robot_id_ == 0) { From 30e54f6e22bc361eaf71dd35400b58e5e5a5a750 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 31 Aug 2025 21:53:06 -0400 Subject: [PATCH 18/44] starting implementing coordaintor with defense --- .../rj_strategy/agent/position/defense.hpp | 4 ++- .../src/agent/position/defense.cpp | 32 +++++++++++++++---- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 31da1d8e605..1626fe2d78c 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -122,7 +122,9 @@ class Defense : public Position { int get_waller_id(); State current_state_ = JOINING_WALL; - int get_marker_target_id(); + bool sent_join_marking_group_request_ = false; + RJ::Time time_of_join_marking_group_request_; + double kMarkingGroupJoinTimeout = ; Marker marker_; std::shared_ptr clientHandles_; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index acc1005180b..270aef3eeaa 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -91,13 +91,33 @@ Defense::State Defense::update_state() { } break; case ENTERING_MARKING: - marker_.choose_target(world_state); - int target_id = marker_.get_target(); - if (target_id == -1) { - next_state = ENTERING_MARKING; - } else { - next_state = MARKING; + if (!sent_join_marking_group_request_) { + sent_join_marking_group_request_ = true; + + clientHandles->markingClient_->join_group([this](const MarkingClient::Result& result) { + // Defensive check: Only transition if we are still in the process of entering. + // We might have timed out and moved to another state in the meantime. + if (current_state_ != ENTERING_MARKING) { + return IDLING; + } + + if (result.am_i_member && result.am_i_marking) { + next_state = MARKING; + } else { + return IDLING; + } + }); + } + + auto elapsed = RJ::now() - state_entry_time_; + if (elapsed > kMarkingGroupJoinTimeout) { + // reset flag + sent_join_marking_group_request_ = false; + // ensure not in coordinator group + clientHandles->markingClient_->leave_group(); + SPDLOG_INFO("Took too long to join marking coordinator group for robot {}", robot_id_); } + break; } return next_state; From 86e538aa8dae4679153e381211e52f73d576219a Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 2 Sep 2025 21:14:35 -0400 Subject: [PATCH 19/44] added some more stuff to implement marking into the defense state machine. Still some issues with it actually going to mark, because it seems to idle --- .../strategy/coordinator/marking_client.cpp | 2 +- .../rj_strategy/agent/position/defense.hpp | 9 ++- .../rj_strategy/agent/position/marker.hpp | 1 + .../src/agent/position/defense.cpp | 57 ++++++++++++++----- src/rj_strategy/src/agent/position/marker.cpp | 2 + .../src/agent/position/offense.cpp | 2 +- 6 files changed, 53 insertions(+), 20 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.cpp b/soccer/src/soccer/strategy/coordinator/marking_client.cpp index 0225c45f04f..21189637725 100644 --- a/soccer/src/soccer/strategy/coordinator/marking_client.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking_client.cpp @@ -99,7 +99,7 @@ void MarkingClient::leave_group(StatusCallback callback) { // deallocate the subscription. The callback will no longer be // called. subscription_.reset(); - am_i_marking_ = kInvalidRobotId; + am_i_marking_ = false; selected_robot_marking_id_ = kInvalidRobotId; if (callback) { diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 1626fe2d78c..cf7cccdb7f3 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -47,8 +47,8 @@ class Defense : public Position { private: // static constexpr int kMaxWallers{6}; - static constexpr int kMaxWallers{ - static_cast(kNumShells)}; // This effectively turns off marking + static constexpr int kMaxWallers{1}; + // static_cast(kNumShells)}; // This effectively turns off marking /** * @brief The derived_get_task method returns the task for the defensive robot @@ -124,9 +124,12 @@ class Defense : public Position { bool sent_join_marking_group_request_ = false; RJ::Time time_of_join_marking_group_request_; - double kMarkingGroupJoinTimeout = ; + double kMarkingGroupJoinTimeout = 1000000; Marker marker_; + std::optional pending_state_; + std::optional pending_mark_target_; + std::shared_ptr clientHandles_; }; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp index 1c2eb7ebb3b..3215fc93a52 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp @@ -38,6 +38,7 @@ class Marker : public RoleInterface { FieldDimensions field_dimensions) override; void choose_target(const WorldState* ws); + void set_target(int target); int get_target(); bool target_out_of_bounds(const WorldState* ws); }; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 270aef3eeaa..1e297ffa500 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -36,6 +36,15 @@ Defense::State Defense::update_state() { rj_geometry::Point ball_position = world_state->ball.position; double distance_to_ball = robot_position.dist_to(ball_position); + if (pending_state_) { + next_state = *pending_state_; + if (next_state == MARKING && pending_mark_target_) { + marker_.set_target(*pending_mark_target_); + } + pending_state_.reset(); + pending_mark_target_.reset(); + } + if (current_state_ != WALLING && current_state_ != JOINING_WALL && waller_id_ != -1) { send_leave_wall_request(); walling_robots_ = {(u_int8_t)robot_id_}; @@ -86,37 +95,55 @@ Defense::State Defense::update_state() { next_state = IDLING; } case MARKING: - if (marker_.get_target() == -1 || marker_.target_out_of_bounds(world_state)) { + SPDLOG_INFO("Robot {}: marking robot {}", robot_id_, marker_.get_target()); + if (!clientHandles_->markingClient->am_i_member()) { + SPDLOG_INFO("Robot {}: no longer a member of marking group", robot_id_); + next_state = IDLING; + } else if (!clientHandles_->markingClient->am_i_marking()) { next_state = ENTERING_MARKING; } break; case ENTERING_MARKING: + // SPDLOG_INFO("Robot {}: entering marking", robot_id_); + if (!sent_join_marking_group_request_) { sent_join_marking_group_request_ = true; - clientHandles->markingClient_->join_group([this](const MarkingClient::Result& result) { + clientHandles_->markingClient->join_group([this](const MarkingClient::Result& result) { // Defensive check: Only transition if we are still in the process of entering. // We might have timed out and moved to another state in the meantime. if (current_state_ != ENTERING_MARKING) { - return IDLING; + pending_state_ = IDLING; + return; } - + + SPDLOG_INFO("Robot {}: checking if it is a member and if it is marking", robot_id_); if (result.am_i_member && result.am_i_marking) { - next_state = MARKING; + SPDLOG_INFO("{}: Yes", robot_id_); + pending_state_ = MARKING; + pending_mark_target_ = clientHandles_->markingClient->who_am_i_marking(); } else { - return IDLING; + SPDLOG_INFO("{}: No", robot_id_); + pending_state_ = IDLING; } }); } - - auto elapsed = RJ::now() - state_entry_time_; - if (elapsed > kMarkingGroupJoinTimeout) { - // reset flag - sent_join_marking_group_request_ = false; - // ensure not in coordinator group - clientHandles->markingClient_->leave_group(); - SPDLOG_INFO("Took too long to join marking coordinator group for robot {}", robot_id_); - } + // auto elapsed = RJ::now() - state_entry_time_; + // if (elapsed > kMarkingGroupJoinTimeout) { + // // reset flag + // sent_join_marking_group_request_ = false; + // // ensure not in coordinator group + // clientHandles_->markingClient->leave_group(); + // SPDLOG_INFO("Took too long to join marking coordinator group for robot {}", robot_id_); + // } + + // if (clientHandles_->markingClient->am_i_member() && + // clientHandles_->markingClient->am_i_marking()) { + // next_state = MARKING; + // SPDLOG_INFO("Next state is {}, ENTERING_MARKING is {}, MARKING is {}", static_cast(next_state), ENTERING_MARKING, MARKING); + // marker_.set_target(clientHandles_->markingClient->who_am_i_marking()); + // } + break; } diff --git a/src/rj_strategy/src/agent/position/marker.cpp b/src/rj_strategy/src/agent/position/marker.cpp index f66f1625f24..5b3cea727cf 100644 --- a/src/rj_strategy/src/agent/position/marker.cpp +++ b/src/rj_strategy/src/agent/position/marker.cpp @@ -36,6 +36,8 @@ void Marker::choose_target(const WorldState* ws) { target_ = -1; } +void Marker::set_target(int target) { target_ = target; } + bool Marker::target_out_of_bounds(const WorldState* ws) { if (target_ == -1) return true; if (ws->get_robot(false, target_).pose.position().y() > y_bound) { diff --git a/src/rj_strategy/src/agent/position/offense.cpp b/src/rj_strategy/src/agent/position/offense.cpp index 68ff1078f9b..491647c86e4 100644 --- a/src/rj_strategy/src/agent/position/offense.cpp +++ b/src/rj_strategy/src/agent/position/offense.cpp @@ -21,7 +21,7 @@ std::optional Offense::derived_get_task(RobotIntent intent) { if (current_state_ != new_state) { reset_timeout(); - SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_)); + // SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_)); if (current_state_ == SEEKING) { broadcast_seeker_request(rj_geometry::Point{}, false); } From c740b5e37c0e904b4f0254a5e9040ede775df752 Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 2 Sep 2025 21:46:12 -0400 Subject: [PATCH 20/44] some debugging done. marks now, but sim crashes --- src/rj_strategy/src/agent/position/defense.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 1e297ffa500..f626e61f24b 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -53,6 +53,7 @@ Defense::State Defense::update_state() { switch (current_state_) { case IDLING: + SPDLOG_INFO("Robot {}: idling", robot_id_); break; case JOINING_WALL: send_join_wall_request(); @@ -112,10 +113,10 @@ Defense::State Defense::update_state() { clientHandles_->markingClient->join_group([this](const MarkingClient::Result& result) { // Defensive check: Only transition if we are still in the process of entering. // We might have timed out and moved to another state in the meantime. - if (current_state_ != ENTERING_MARKING) { - pending_state_ = IDLING; - return; - } + // if (current_state_ != ENTERING_MARKING) { + // pending_state_ = IDLING; + // return; + // } SPDLOG_INFO("Robot {}: checking if it is a member and if it is marking", robot_id_); if (result.am_i_member && result.am_i_marking) { From 68ce9333ba287d5d1304fb2dc01d51a526dff6c6 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 7 Sep 2025 21:22:02 -0400 Subject: [PATCH 21/44] stuff --- .../rj_strategy/agent/position/defense.hpp | 19 ++-- .../rj_strategy/agent/position/marker.hpp | 45 --------- .../src/agent/position/defense.cpp | 92 ++++++++----------- src/rj_strategy/src/agent/position/marker.cpp | 51 ---------- 4 files changed, 51 insertions(+), 156 deletions(-) delete mode 100644 src/rj_strategy/include/rj_strategy/agent/position/marker.hpp delete mode 100644 src/rj_strategy/src/agent/position/marker.cpp diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index cf7cccdb7f3..b6843101efa 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -15,9 +15,14 @@ #include #include -#include "rj_strategy/agent/position.hpp" -#include "rj_strategy/agent/position/marker.hpp" -#include "rj_strategy/agent/position/waller.hpp" +#include "planning/instant.hpp" +#include "position.hpp" +#include "rj_common/field_dimensions.hpp" +#include "rj_common/time.hpp" +#include "rj_constants/constants.hpp" +#include "rj_geometry/geometry_conversions.hpp" +#include "rj_geometry/point.hpp" +#include "waller.hpp" namespace strategy { @@ -123,12 +128,10 @@ class Defense : public Position { State current_state_ = JOINING_WALL; bool sent_join_marking_group_request_ = false; - RJ::Time time_of_join_marking_group_request_; - double kMarkingGroupJoinTimeout = 1000000; - Marker marker_; + RJ::Time request_time_; + RJ::Seconds kMarkingGroupJoinTimeout{3.0}; // 3 seconds - std::optional pending_state_; - std::optional pending_mark_target_; + pending_state_; std::shared_ptr clientHandles_; }; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp deleted file mode 100644 index 3215fc93a52..00000000000 --- a/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include - -#include "rj_strategy/agent/position.hpp" -#include "rj_strategy/agent/position/role_interface.hpp" - -namespace strategy { - -/** - * Represents a Marker, a defensive role that targets an enemy robot - * and follows it around the field while it is on our side, blocking passes. - */ -class Marker : public RoleInterface { -private: - int target_{-1}; - - // Calculated from field dimensions - Prevent the marker from - // marking any enemies that are out of range; e. g. on the other - // side of the field or the sidelines. - float y_bound{FieldDimensions::kDefaultDimensions.length() / 2}; - float marker_follow_cutoff{FieldDimensions::kDefaultDimensions.width() / 2}; - -public: - Marker(FieldDimensions field_dimensions); - ~Marker() = default; - Marker(const Marker& other) = default; - Marker(Marker&& other) = default; - Marker& operator=(const Marker& other) = default; - Marker& operator=(Marker&& other) = default; - - std::optional get_task(RobotIntent intent, const WorldState* world_state, - FieldDimensions field_dimensions) override; - - void choose_target(const WorldState* ws); - void set_target(int target); - int get_target(); - bool target_out_of_bounds(const WorldState* ws); -}; -} // namespace strategy diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index f626e61f24b..46558e1120a 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -2,16 +2,16 @@ namespace strategy { -Defense::Defense(int r_id) : Position(r_id, "Defense"), marker_{field_dimensions_} {} +Defense::Defense(int r_id) : Position(r_id, "Defense") {} -Defense::Defense(const Position& other) : Position{other}, marker_{field_dimensions_} { +Defense::Defense(const Position& other) : Position{other} { position_name_ = "Defense"; walling_robots_ = {}; } -Defense::Defense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Defense"), marker_{field_dimensions_}, clientHandles_{clientHandles} {} +Defense::Defense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Defense"), clientHandles_{clientHandles} {} -Defense::Defense(const Position& other, std::shared_ptr clientHandles) : Position{other}, marker_{field_dimensions_}, clientHandles_{clientHandles} { +Defense::Defense(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { position_name_ = "Defense"; walling_robots_ = {}; } @@ -28,23 +28,30 @@ std::string Defense::get_current_state() { } Defense::State Defense::update_state() { - State next_state = current_state_; - // handle transitions between states WorldState* world_state = last_world_state_; rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position(); rj_geometry::Point ball_position = world_state->ball.position; double distance_to_ball = robot_position.dist_to(ball_position); - if (pending_state_) { - next_state = *pending_state_; - if (next_state == MARKING && pending_mark_target_) { - marker_.set_target(*pending_mark_target_); + // Update state based on coordinator async calls resolving + if (pending_marking_state_) { + // Defensive check: Only transition if we are still in the process of entering. + // We might have timed out and moved to another state in the meantime. + if (current_state_ != ENTERING_MARKING) { + return IDLING; + } + + SPDLOG_INFO("Robot {}: checking if it is a member and if it is marking", robot_id_); + if (client_handles->marking_client->am_i_member && result.am_i_marking) { + return MARKING; + } else { + return IDLING; } - pending_state_.reset(); - pending_mark_target_.reset(); } + State next_state = current_state_; + if (current_state_ != WALLING && current_state_ != JOINING_WALL && waller_id_ != -1) { send_leave_wall_request(); walling_robots_ = {(u_int8_t)robot_id_}; @@ -96,12 +103,10 @@ Defense::State Defense::update_state() { next_state = IDLING; } case MARKING: - SPDLOG_INFO("Robot {}: marking robot {}", robot_id_, marker_.get_target()); - if (!clientHandles_->markingClient->am_i_member()) { + SPDLOG_INFO("Robot {}: marking robot {}", robot_id_); + if (!clientHandles_->markingClient->am_i_member() || !clientHandles_->markingClient->am_i_marking()) { SPDLOG_INFO("Robot {}: no longer a member of marking group", robot_id_); next_state = IDLING; - } else if (!clientHandles_->markingClient->am_i_marking()) { - next_state = ENTERING_MARKING; } break; case ENTERING_MARKING: @@ -109,42 +114,19 @@ Defense::State Defense::update_state() { if (!sent_join_marking_group_request_) { sent_join_marking_group_request_ = true; + request_time_ = RJ::now(); - clientHandles_->markingClient->join_group([this](const MarkingClient::Result& result) { - // Defensive check: Only transition if we are still in the process of entering. - // We might have timed out and moved to another state in the meantime. - // if (current_state_ != ENTERING_MARKING) { - // pending_state_ = IDLING; - // return; - // } - - SPDLOG_INFO("Robot {}: checking if it is a member and if it is marking", robot_id_); - if (result.am_i_member && result.am_i_marking) { - SPDLOG_INFO("{}: Yes", robot_id_); - pending_state_ = MARKING; - pending_mark_target_ = clientHandles_->markingClient->who_am_i_marking(); - } else { - SPDLOG_INFO("{}: No", robot_id_); - pending_state_ = IDLING; - } - }); + clientHandles_->markingClient->join_group() { + } + auto elapsed = RJ::now() - request_time_; + if (elapsed > kMarkingGroupJoinTimeout) { + // reset flag + sent_join_marking_group_request_ = false; + // ensure not in coordinator group + clientHandles_->markingClient->leave_group(); + SPDLOG_INFO("Took too long to join marking coordinator group for robot {}", robot_id_); } - // auto elapsed = RJ::now() - state_entry_time_; - // if (elapsed > kMarkingGroupJoinTimeout) { - // // reset flag - // sent_join_marking_group_request_ = false; - // // ensure not in coordinator group - // clientHandles_->markingClient->leave_group(); - // SPDLOG_INFO("Took too long to join marking coordinator group for robot {}", robot_id_); - // } - - // if (clientHandles_->markingClient->am_i_member() && - // clientHandles_->markingClient->am_i_marking()) { - // next_state = MARKING; - // SPDLOG_INFO("Next state is {}, ENTERING_MARKING is {}, MARKING is {}", static_cast(next_state), ENTERING_MARKING, MARKING); - // marker_.set_target(clientHandles_->markingClient->who_am_i_marking()); - // } - + break; } @@ -217,8 +199,14 @@ std::optional Defense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == MARKING) { - // Marker marker = Marker((u_int8_t) robot_id_); - return marker_.get_task(intent, last_world_state_, this->field_dimensions_); + rj_geometry::Point targetPoint = last_world_state_->get_robot(false, clientHandles_->markingClient->who_am_i_marking()).pose.position(); + + rj_geometry::Point ballPoint = last_world_state_->ball.position; + rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); + planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; + intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; + + return intent; } return std::nullopt; diff --git a/src/rj_strategy/src/agent/position/marker.cpp b/src/rj_strategy/src/agent/position/marker.cpp deleted file mode 100644 index 5b3cea727cf..00000000000 --- a/src/rj_strategy/src/agent/position/marker.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "rj_strategy/agent/position/marker.hpp" - -namespace strategy { -Marker::Marker(FieldDimensions field_dimensions) { - this->y_bound = field_dimensions.length() / 2; - this->marker_follow_cutoff = field_dimensions.width() / 2; -} - -std::optional Marker::get_task(RobotIntent intent, const WorldState* world_state, - [[maybe_unused]] FieldDimensions field_dimensions) { - this->y_bound = field_dimensions.length() / 2; - this->marker_follow_cutoff = field_dimensions.width() / 2; - - rj_geometry::Point targetPoint = world_state->get_robot(false, target_).pose.position(); - rj_geometry::Point ballPoint = world_state->ball.position; - rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); - planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = - planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; - return intent; -} - -void Marker::choose_target(const WorldState* ws) { - // TODO: (James Vogt, github: jvogt23) - // If we ever use multiple Markers, they should choose different - // robots to track from each other. Logic for this operation must be - // added because multiple markers currently mark the same robot. - for (int i = 0; i < kNumShells; i++) { - if (std::fabs(ws->get_robot(false, i).pose.position().x()) < marker_follow_cutoff && - ws->get_robot(false, i).pose.position().y() < y_bound && - (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) { - target_ = i; - return; - } - } - target_ = -1; -} - -void Marker::set_target(int target) { target_ = target; } - -bool Marker::target_out_of_bounds(const WorldState* ws) { - if (target_ == -1) return true; - if (ws->get_robot(false, target_).pose.position().y() > y_bound) { - target_ = -1; - return true; - } - return false; -} - -int Marker::get_target() { return target_; } -} // namespace strategy \ No newline at end of file From 245cfd4bbc6b2679dd08414d09bbc1c1a595a8fc Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 14 Sep 2025 19:28:48 -0400 Subject: [PATCH 22/44] fixed it so it compiles --- .../include/rj_strategy/agent/position/defense.hpp | 2 +- src/rj_strategy/src/agent/position/defense.cpp | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index b6843101efa..e4a607e99ae 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -131,7 +131,7 @@ class Defense : public Position { RJ::Time request_time_; RJ::Seconds kMarkingGroupJoinTimeout{3.0}; // 3 seconds - pending_state_; + bool pending_marking_state_ = false; std::shared_ptr clientHandles_; }; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 46558e1120a..6377d5c743c 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -38,12 +38,14 @@ Defense::State Defense::update_state() { if (pending_marking_state_) { // Defensive check: Only transition if we are still in the process of entering. // We might have timed out and moved to another state in the meantime. + pending_marking_state_ = false; if (current_state_ != ENTERING_MARKING) { return IDLING; } SPDLOG_INFO("Robot {}: checking if it is a member and if it is marking", robot_id_); - if (client_handles->marking_client->am_i_member && result.am_i_marking) { + if (clientHandles_->markingClient->am_i_member() && clientHandles_->markingClient->am_i_marking()) { + SPDLOG_INFO("Robot {}: successfully joined marking coordinator group and is marking robot {}", robot_id_, clientHandles_->markingClient->who_am_i_marking()); return MARKING; } else { return IDLING; @@ -116,7 +118,11 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = true; request_time_ = RJ::now(); - clientHandles_->markingClient->join_group() { + clientHandles_->markingClient->join_group([this](const MarkingClient::Result& res) { + if (res.am_i_member && res.am_i_marking) { + pending_marking_state_ = true; + } + }); } auto elapsed = RJ::now() - request_time_; if (elapsed > kMarkingGroupJoinTimeout) { @@ -130,6 +136,10 @@ Defense::State Defense::update_state() { break; } + if (robot_id_ == 4) { + SPDLOG_INFO("Robot {}: current state {}, next state {}, marking state {}, idling state {}", robot_id_, current_state_, next_state, MARKING, IDLING); + } + return next_state; } From 1d6c90dc2bf00ca0144a09f550cf948843a0dd7d Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 14 Sep 2025 21:43:34 -0400 Subject: [PATCH 23/44] Stuff --- .../strategy/coordinator/marking_client.cpp | 15 ++++++++++----- src/rj_planning/src/planner_for_robot.cpp | 4 ++-- .../rj_strategy/agent/position/defense.hpp | 4 ++-- src/rj_strategy/src/agent/position/defense.cpp | 4 ++++ .../src/coordinator/kicker_picker_client.cpp | 1 + 5 files changed, 19 insertions(+), 9 deletions(-) diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.cpp b/soccer/src/soccer/strategy/coordinator/marking_client.cpp index 21189637725..e0653156ce7 100644 --- a/soccer/src/soccer/strategy/coordinator/marking_client.cpp +++ b/soccer/src/soccer/strategy/coordinator/marking_client.cpp @@ -24,6 +24,8 @@ MarkingClient::MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) } void MarkingClient::join_group(StatusCallback callback) { + + SPDLOG_INFO("Hello, please print"); if (am_i_member_) { return; } @@ -40,12 +42,15 @@ void MarkingClient::join_group(StatusCallback callback) { request->robot_id = robot_id_; request->join = true; + SPDLOG_INFO("About to send async request"); client_->async_send_request( request, [this, callback = std::move(callback)]( rclcpp::Client::SharedFuture future) { // 6 NOLINT(performance-unnecessary-value-param) -- // ROS2 async callbacks require value capture. + SPDLOG_INFO("Hello, finished async"); if (!future.valid() || !future.get()->success) { + SPDLOG_INFO("Something bad happened"); if (callback) { callback(Result{false}); } @@ -57,14 +62,14 @@ void MarkingClient::join_group(StatusCallback callback) { // Create subscription to track selected kicker. subscription_ = node_->create_subscription( "marking_data", rclcpp::QoS(1).transient_local(), - [this, - callback = std::move(callback)](const rj_msgs::msg::Marking::SharedPtr msg) { + [this](const rj_msgs::msg::Marking::SharedPtr msg) { selected_robot_marking_id_ = msg->mark_robot_ids[robot_id_]; am_i_marking_ = (selected_robot_marking_id_ != kInvalidRobotId); - if (callback) { - callback(Result{true, am_i_marking_, selected_robot_marking_id_}); - } }); + + callback(Result{true}); + + SPDLOG_INFO("Should be done with group call"); }); } diff --git a/src/rj_planning/src/planner_for_robot.cpp b/src/rj_planning/src/planner_for_robot.cpp index 35df184aa98..15006ed7d30 100644 --- a/src/rj_planning/src/planner_for_robot.cpp +++ b/src/rj_planning/src/planner_for_robot.cpp @@ -283,8 +283,8 @@ Trajectory PlannerForRobot::safe_plan_for_robot(const planning::PlanRequest& req try { trajectory = unsafe_plan_for_robot(request); } catch (std::runtime_error exception) { - // SPDLOG_WARN("PlannerForRobot {} error caught: {}", robot_id_, exception.what()); - // SPDLOG_WARN("PlannerForRobot {}: Defaulting to EscapeObstaclesPathPlanner", robot_id_); + SPDLOG_WARN("PlannerForRobot {} error caught: {}", robot_id_, exception.what()); + SPDLOG_WARN("PlannerForRobot {}: Defaulting to EscapeObstaclesPathPlanner", robot_id_); current_path_planner_ = default_path_planner_.get(); trajectory = current_path_planner_->plan(request); diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index e4a607e99ae..3e21f756d63 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -52,7 +52,7 @@ class Defense : public Position { private: // static constexpr int kMaxWallers{6}; - static constexpr int kMaxWallers{1}; + static constexpr int kMaxWallers{0}; // static_cast(kNumShells)}; // This effectively turns off marking /** @@ -129,7 +129,7 @@ class Defense : public Position { bool sent_join_marking_group_request_ = false; RJ::Time request_time_; - RJ::Seconds kMarkingGroupJoinTimeout{3.0}; // 3 seconds + RJ::Seconds kMarkingGroupJoinTimeout{999999999999999999999999999.0}; // 3 seconds bool pending_marking_state_ = false; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 6377d5c743c..9241ee11a86 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -118,7 +118,10 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = true; request_time_ = RJ::now(); + SPDLOG_INFO("About to try to join marking"); + clientHandles_->markingClient->join_group([this](const MarkingClient::Result& res) { + SPDLOG_INFO("Completed join group call"); if (res.am_i_member && res.am_i_marking) { pending_marking_state_ = true; } @@ -214,6 +217,7 @@ std::optional Defense::state_to_task(RobotIntent intent) { rj_geometry::Point ballPoint = last_world_state_->ball.position; rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; + SPDLOG_INFO("Location to mark: {}, {}", (targetPoint + targetToBall).x(), (targetPoint + targetToBall).y()); intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; return intent; diff --git a/src/rj_strategy/src/coordinator/kicker_picker_client.cpp b/src/rj_strategy/src/coordinator/kicker_picker_client.cpp index 08820d0f904..772e08f1031 100644 --- a/src/rj_strategy/src/coordinator/kicker_picker_client.cpp +++ b/src/rj_strategy/src/coordinator/kicker_picker_client.cpp @@ -14,6 +14,7 @@ KickerPickerClient::KickerPickerClient(rclcpp::Node::SharedPtr node, uint8_t rob } void KickerPickerClient::join_group(StatusCallback callback) { + SPDLOG_INFO("Hello, I'm trying to join"); if (am_i_member_) { return; } From a6c207a44b72da12fbfaa7ffa4b68eac8af4da91 Mon Sep 17 00:00:00 2001 From: sdhanyamraju6 Date: Sun, 28 Sep 2025 21:15:14 -0400 Subject: [PATCH 24/44] rebase to ros2 to have colcon --- launch/soccer.launch.py | 2 +- .../strategy/coordinator/marking_main.cpp | 11 ---------- src/rj_strategy/CMakeLists.txt | 21 +++++++++++++++++-- .../include/rj_strategy/agent/position.hpp | 4 ++-- .../rj_strategy/agent/position/defense.hpp | 11 ++-------- .../rj_strategy}/coordinator/marking.hpp | 13 ++++++++---- .../coordinator/marking_client.hpp | 1 + .../rj_strategy/src}/coordinator/marking.cpp | 17 ++++++++------- .../src}/coordinator/marking_client.cpp | 12 +---------- 9 files changed, 44 insertions(+), 48 deletions(-) delete mode 100644 soccer/src/soccer/strategy/coordinator/marking_main.cpp rename {soccer/src/soccer/strategy => src/rj_strategy/include/rj_strategy}/coordinator/marking.hpp (86%) rename {soccer/src/soccer/strategy => src/rj_strategy/include/rj_strategy}/coordinator/marking_client.hpp (98%) rename {soccer/src/soccer/strategy => src/rj_strategy/src}/coordinator/marking.cpp (97%) rename {soccer/src/soccer/strategy => src/rj_strategy/src}/coordinator/marking_client.cpp (94%) diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index 1558a2f4143..670783a572a 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -239,7 +239,7 @@ def generate_launch_description(): on_exit=Shutdown(), ), Node( - package="rj_robocup", + package="rj_strategy", executable="marking_node", output="screen", parameters=[param_config_filepath], diff --git a/soccer/src/soccer/strategy/coordinator/marking_main.cpp b/soccer/src/soccer/strategy/coordinator/marking_main.cpp deleted file mode 100644 index 497b6d649d9..00000000000 --- a/soccer/src/soccer/strategy/coordinator/marking_main.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include - -#include "marking.hpp" - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/rj_strategy/CMakeLists.txt b/src/rj_strategy/CMakeLists.txt index b00d47d05a1..979ec935329 100644 --- a/src/rj_strategy/CMakeLists.txt +++ b/src/rj_strategy/CMakeLists.txt @@ -47,7 +47,6 @@ set(RJ_STRATEGY_SRC src/agent/position/goalie.cpp src/agent/position/idle.cpp src/agent/position/line.cpp - src/agent/position/marker.cpp src/agent/position/offense.cpp src/agent/position/penalty_non_kicker.cpp src/agent/position/penalty_player.cpp @@ -64,6 +63,7 @@ set(RJ_STRATEGY_SRC # Coordinators src/coordinator/kicker_picker_client.cpp + src/coordinator/marking_client.cpp ) # rj_strategy library @@ -115,6 +115,23 @@ ament_target_dependencies(kicker_picker_node ${RJ_STRATEGY_DEPS} ) +# Marking Node +add_executable(marking_node + ${RJ_STRATEGY_SRC} + src/coordinator/marking_client.cpp + src/coordinator/marking.cpp +) + +target_include_directories(marking_node + PUBLIC + $ + $ +) + +ament_target_dependencies(marking_node + ${RJ_STRATEGY_DEPS} +) + # Agent Action Clients add_executable(agent_action_clients ${RJ_STRATEGY_SRC} @@ -146,7 +163,7 @@ install( ) install( - TARGETS straight_line_test_node kicker_picker_node agent_action_clients + TARGETS straight_line_test_node kicker_picker_node marking_node agent_action_clients DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/rj_strategy/include/rj_strategy/agent/position.hpp b/src/rj_strategy/include/rj_strategy/agent/position.hpp index 928bf459223..3d88301f419 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position.hpp @@ -41,8 +41,8 @@ #include // Coordinators -#include "strategy/coordinator/kicker_picker_client.hpp" -#include "strategy/coordinator/marking_client.hpp" +#include "rj_strategy/coordinator/kicker_picker_client.hpp" +#include "rj_strategy/coordinator/marking_client.hpp" // tell compiler this class exists, but no need to import the whole header class AgentActionClient; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 3e21f756d63..5170068a36b 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -15,15 +15,8 @@ #include #include -#include "planning/instant.hpp" -#include "position.hpp" -#include "rj_common/field_dimensions.hpp" -#include "rj_common/time.hpp" -#include "rj_constants/constants.hpp" -#include "rj_geometry/geometry_conversions.hpp" -#include "rj_geometry/point.hpp" -#include "waller.hpp" - +#include "rj_strategy/agent/position.hpp" +#include "rj_strategy/agent/position/waller.hpp" namespace strategy { /* diff --git a/soccer/src/soccer/strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp similarity index 86% rename from soccer/src/soccer/strategy/coordinator/marking.hpp rename to src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index f0a543759ed..c5bc3a37d18 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -1,17 +1,22 @@ #pragma once #include +#include // for std::any_of +#include #include +#include +#include +#include +#include +#include #include #include -#include #include -#include "coordinator.hpp" -#include "rj_constants/constants.hpp" -#include "world_state.hpp" +#include "rj_strategy/coordinator.hpp" + namespace strategy { diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp similarity index 98% rename from soccer/src/soccer/strategy/coordinator/marking_client.hpp rename to src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp index fc48f1eb1b5..0a262cee5e0 100644 --- a/soccer/src/soccer/strategy/coordinator/marking_client.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp @@ -3,6 +3,7 @@ #include #include +#include #include #include diff --git a/soccer/src/soccer/strategy/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp similarity index 97% rename from soccer/src/soccer/strategy/coordinator/marking.cpp rename to src/rj_strategy/src/coordinator/marking.cpp index d3f94dc56dc..cafcde40040 100644 --- a/soccer/src/soccer/strategy/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -1,11 +1,4 @@ -#include "marking.hpp" - -#include // for std::any_of -#include - -#include -#include -#include +#include "rj_strategy/coordinator/marking.hpp" namespace strategy { @@ -190,3 +183,11 @@ void Marking::update_danger_scores() { } // namespace strategy + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/soccer/src/soccer/strategy/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp similarity index 94% rename from soccer/src/soccer/strategy/coordinator/marking_client.cpp rename to src/rj_strategy/src/coordinator/marking_client.cpp index e0653156ce7..6259ddad6fb 100644 --- a/soccer/src/soccer/strategy/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -1,14 +1,4 @@ -#include "marking_client.hpp" - -#include - -#include -#include - -#include -#include - -#include "marking.hpp" +#include "rj_strategy/coordinator/marking_client.hpp" namespace strategy { From b653fd7a5fcbc1a7aa53908f0e0745ddd3626866 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 12 Oct 2025 21:43:28 -0400 Subject: [PATCH 25/44] stuff --- install/setup.bash | 2 +- install/setup.zsh | 2 +- src/rj_planning/src/planner_for_robot.cpp | 4 ++-- .../rj_strategy/agent/position/defense.hpp | 2 +- .../rj_strategy/coordinator/marking.hpp | 3 ++- src/rj_strategy/src/agent/position/defense.cpp | 18 +++++------------- src/rj_strategy/src/coordinator/marking.cpp | 6 ++++++ .../src/coordinator/marking_client.cpp | 5 ----- 8 files changed, 18 insertions(+), 24 deletions(-) diff --git a/install/setup.bash b/install/setup.bash index da36e7c2000..10ea0f7c07f 100644 --- a/install/setup.bash +++ b/install/setup.bash @@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null _colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script \ No newline at end of file +unset _colcon_prefix_chain_bash_source_script diff --git a/install/setup.zsh b/install/setup.zsh index a1ba46044b5..54799fde6f8 100644 --- a/install/setup.zsh +++ b/install/setup.zsh @@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && p _colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script \ No newline at end of file +unset _colcon_prefix_chain_zsh_source_script diff --git a/src/rj_planning/src/planner_for_robot.cpp b/src/rj_planning/src/planner_for_robot.cpp index 15006ed7d30..35df184aa98 100644 --- a/src/rj_planning/src/planner_for_robot.cpp +++ b/src/rj_planning/src/planner_for_robot.cpp @@ -283,8 +283,8 @@ Trajectory PlannerForRobot::safe_plan_for_robot(const planning::PlanRequest& req try { trajectory = unsafe_plan_for_robot(request); } catch (std::runtime_error exception) { - SPDLOG_WARN("PlannerForRobot {} error caught: {}", robot_id_, exception.what()); - SPDLOG_WARN("PlannerForRobot {}: Defaulting to EscapeObstaclesPathPlanner", robot_id_); + // SPDLOG_WARN("PlannerForRobot {} error caught: {}", robot_id_, exception.what()); + // SPDLOG_WARN("PlannerForRobot {}: Defaulting to EscapeObstaclesPathPlanner", robot_id_); current_path_planner_ = default_path_planner_.get(); trajectory = current_path_planner_->plan(request); diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 5170068a36b..2f4ba0aa9d6 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -122,7 +122,7 @@ class Defense : public Position { bool sent_join_marking_group_request_ = false; RJ::Time request_time_; - RJ::Seconds kMarkingGroupJoinTimeout{999999999999999999999999999.0}; // 3 seconds + RJ::Seconds kMarkingGroupJoinTimeout{2.0}; // 2 seconds bool pending_marking_state_ = false; diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index c5bc3a37d18..65bd3245426 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -38,7 +38,8 @@ class Marking void publish_marking_list(); void update_danger_scores(); - static constexpr int kMaxMarkers = 2; + static constexpr int kMaxMarkers = 20; + // this is the threshold value for switching static constexpr double kSuperDangerSub = 3.1415926535; static constexpr double kDangerDistToBall = 1.0; static constexpr double kDangerDistToGoal = 2.0; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 9241ee11a86..ab11806bcb7 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -43,11 +43,10 @@ Defense::State Defense::update_state() { return IDLING; } - SPDLOG_INFO("Robot {}: checking if it is a member and if it is marking", robot_id_); if (clientHandles_->markingClient->am_i_member() && clientHandles_->markingClient->am_i_marking()) { - SPDLOG_INFO("Robot {}: successfully joined marking coordinator group and is marking robot {}", robot_id_, clientHandles_->markingClient->who_am_i_marking()); return MARKING; } else { + SPDLOG_INFO("Robot {}: After pending marking, not a member so idling", robot_id_); return IDLING; } } @@ -62,7 +61,7 @@ Defense::State Defense::update_state() { switch (current_state_) { case IDLING: - SPDLOG_INFO("Robot {}: idling", robot_id_); + // SPDLOG_INFO("Robot {}: idling", robot_id_); break; case JOINING_WALL: send_join_wall_request(); @@ -105,9 +104,7 @@ Defense::State Defense::update_state() { next_state = IDLING; } case MARKING: - SPDLOG_INFO("Robot {}: marking robot {}", robot_id_); if (!clientHandles_->markingClient->am_i_member() || !clientHandles_->markingClient->am_i_marking()) { - SPDLOG_INFO("Robot {}: no longer a member of marking group", robot_id_); next_state = IDLING; } break; @@ -118,10 +115,8 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = true; request_time_ = RJ::now(); - SPDLOG_INFO("About to try to join marking"); clientHandles_->markingClient->join_group([this](const MarkingClient::Result& res) { - SPDLOG_INFO("Completed join group call"); if (res.am_i_member && res.am_i_marking) { pending_marking_state_ = true; } @@ -133,16 +128,13 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = false; // ensure not in coordinator group clientHandles_->markingClient->leave_group(); - SPDLOG_INFO("Took too long to join marking coordinator group for robot {}", robot_id_); + SPDLOG_INFO("Robot {}: Timeout on join group, IDLING now", robot_id_); + next_state = IDLING; } break; } - if (robot_id_ == 4) { - SPDLOG_INFO("Robot {}: current state {}, next state {}, marking state {}, idling state {}", robot_id_, current_state_, next_state, MARKING, IDLING); - } - return next_state; } @@ -217,7 +209,7 @@ std::optional Defense::state_to_task(RobotIntent intent) { rj_geometry::Point ballPoint = last_world_state_->ball.position; rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; - SPDLOG_INFO("Location to mark: {}, {}", (targetPoint + targetToBall).x(), (targetPoint + targetToBall).y()); + // SPDLOG_INFO("Location to mark: {}, {}", (targetPoint + targetToBall).x(), (targetPoint + targetToBall).y()); intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; return intent; diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index cafcde40040..3f5726d2d86 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -177,8 +177,14 @@ void Marking::update_danger_scores() { angle_between = std::abs(angle_between); double danger_score = dist_to_ball * kDangerDistToBall + dist_to_goal * kDangerDistToGoal - min * kDangerDistToOurRobots - angle_between * kDangerAngle; + if (i < 6) { + SPDLOG_INFO("Robot {} has dist to goal {}", i, dist_to_goal); + } danger_score_[i] = danger_score; } + for (size_t i = 0; i < 6; ++i) { + SPDLOG_INFO("Robot {} has danger score {}", i, danger_score_[i]); + } } diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp index 6259ddad6fb..f777cd24a54 100644 --- a/src/rj_strategy/src/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -15,7 +15,6 @@ MarkingClient::MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) void MarkingClient::join_group(StatusCallback callback) { - SPDLOG_INFO("Hello, please print"); if (am_i_member_) { return; } @@ -32,15 +31,12 @@ void MarkingClient::join_group(StatusCallback callback) { request->robot_id = robot_id_; request->join = true; - SPDLOG_INFO("About to send async request"); client_->async_send_request( request, [this, callback = std::move(callback)]( rclcpp::Client::SharedFuture future) { // 6 NOLINT(performance-unnecessary-value-param) -- // ROS2 async callbacks require value capture. - SPDLOG_INFO("Hello, finished async"); if (!future.valid() || !future.get()->success) { - SPDLOG_INFO("Something bad happened"); if (callback) { callback(Result{false}); } @@ -59,7 +55,6 @@ void MarkingClient::join_group(StatusCallback callback) { callback(Result{true}); - SPDLOG_INFO("Should be done with group call"); }); } From 5477bf5b637c8867186f8abe99c40eb6a19003fd Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 26 Oct 2025 19:42:00 -0400 Subject: [PATCH 26/44] New danger score parameters --- src/rj_strategy/include/rj_strategy/coordinator/marking.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index 65bd3245426..74bcd9e0454 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -41,8 +41,8 @@ class Marking static constexpr int kMaxMarkers = 20; // this is the threshold value for switching static constexpr double kSuperDangerSub = 3.1415926535; - static constexpr double kDangerDistToBall = 1.0; - static constexpr double kDangerDistToGoal = 2.0; + static constexpr double kDangerDistToBall = 3.0; + static constexpr double kDangerDistToGoal = 5.0; static constexpr double kDangerDistToOurRobots = 3.0; static constexpr double kDangerAngle = 2.0; int num_markers_; From f070f8ae1c92f0a20ebc1ae15f85d6b51ad92ffb Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 26 Oct 2025 21:53:35 -0400 Subject: [PATCH 27/44] Danger angle change --- src/rj_strategy/src/coordinator/marking.cpp | 46 +++++++++++++++------ 1 file changed, 34 insertions(+), 12 deletions(-) diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 3f5726d2d86..a2000883ce9 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -147,6 +147,8 @@ void Marking::update_danger_scores() { // lower danger score is more dangerous const auto& ball_pos = last_world_state_.ball.position; + const auto& goal_loc = field_dimensions_.our_goal_loc(); + const auto& field_center = field_dimensions_.center_field_loc(); for (uint8_t i = 0; i < kNumShells; i++) { const auto& robot = last_world_state_.get_robot(false, i); @@ -154,7 +156,7 @@ void Marking::update_danger_scores() { continue; } double dist_to_ball = ball_pos.dist_to(robot.pose.position()); - double dist_to_goal = robot.pose.position().dist_to(field_dimensions_.our_goal_loc()); + double dist_to_goal = robot.pose.position().dist_to(goal_loc); double min = std::numeric_limits::infinity(); for (uint8_t j = 0; j < kNumShells; j++) { @@ -167,21 +169,41 @@ void Marking::update_danger_scores() { min = dist; } } - const auto& goal_to_ball = ball_pos - field_dimensions_.our_goal_loc(); - const auto& goal_to_robot = robot.pose.position() - field_dimensions_.our_goal_loc(); - double cosTheta = goal_to_ball.dot(goal_to_robot) / (goal_to_ball.mag() * goal_to_robot.mag()); - // Clamp value to [-1, 1] to avoid domain errors due to floating point precision - if (cosTheta > 1.0) cosTheta = 1.0; - if (cosTheta < -1.0) cosTheta = -1.0; - double angle_between = std::acos(cosTheta); // returns radians - angle_between = std::abs(angle_between); - double danger_score = dist_to_ball * kDangerDistToBall + dist_to_goal * kDangerDistToGoal - min * kDangerDistToOurRobots - angle_between * kDangerAngle; - if (i < 6) { - SPDLOG_INFO("Robot {} has dist to goal {}", i, dist_to_goal); + double angle_between = 0.0; // Default to 0 (not dangerous) + // Check if beyond midfield + + bool onOurSide = false; + if (goal_loc.y() < field_center.y()) { + onOurSide = robot.pose.position().y() < field_center.y(); + } else { + onOurSide = robot.pose.position().y() > field_center.y(); + } + + if (onOurSide) { + const auto& vec_goal_to_center = field_center - goal_loc; + const auto& vec_goal_to_robot = robot.pose.position() - goal_loc; + + double cosTheta = vec_goal_to_center.dot(vec_goal_to_robot) / + (vec_goal_to_center.mag() * vec_goal_to_robot.mag()); + + if (cosTheta > 1.0) cosTheta = 1.0; + if (cosTheta < -1.0) cosTheta = -1.0; + double central_angle = std::abs(std::acos(cosTheta)); // [0, PI/2] + // Normalize + double normalized_danger = (M_PI_2 - central_angle) / M_PI_2; + if (normalized_danger < 0.0) normalized_danger = 0.0; // Clamp + + // Scales angles so that more central angles close together and more sideline are futher apart + const double kDangerAngleExponent = 0.25; + angle_between = std::pow(normalized_danger, kDangerAngleExponent); } + + double danger_score = dist_to_ball * kDangerDistToBall + dist_to_goal * kDangerDistToGoal - min * kDangerDistToOurRobots - angle_between * kDangerAngle; + danger_score_[i] = danger_score; } + for (size_t i = 0; i < 6; ++i) { SPDLOG_INFO("Robot {} has danger score {}", i, danger_score_[i]); } From 2f1354a2d95f037cf69e02c79c216f904b99b97d Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 28 Oct 2025 21:35:56 -0400 Subject: [PATCH 28/44] doesn't mark the guy with the ball anymore --- .../src/agent/position/defense.cpp | 1 + src/rj_strategy/src/coordinator/marking.cpp | 50 +++++++++++++++++-- 2 files changed, 48 insertions(+), 3 deletions(-) diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index ab11806bcb7..64a22738da1 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -62,6 +62,7 @@ Defense::State Defense::update_state() { switch (current_state_) { case IDLING: // SPDLOG_INFO("Robot {}: idling", robot_id_); + next_state = JOINING_WALL; break; case JOINING_WALL: send_join_wall_request(); diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index a2000883ce9..649449f5d3e 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -50,10 +50,26 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { return; } if (num_markers_ < kMaxMarkers) { + uint8_t robotInPossession = kInvalidRobotId; + double min_dist_to_ball = std::numeric_limits::infinity(); + const auto& ball_pos = last_world_state_.ball.position; + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(false, i); + if (!robot.visible) { + continue; + } + double dist_to_ball = ball_pos.dist_to(robot.pose.position()); + if (dist_to_ball < min_dist_to_ball && dist_to_ball < 0.3) { + robotInPossession = i; + min_dist_to_ball = dist_to_ball; + } + } + + uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); for (size_t i = 0; i < danger_score_.size(); ++i) { - if (enemey_to_friends_[i] != kInvalidRobotId) { + if (enemey_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { continue; } if (danger_score_[i] < min) { @@ -102,13 +118,32 @@ void Marking::publish_marking_list() { // make this run on a timer update_danger_scores(); - // reshuffle, only change one because on timer so will get others later + // find the guy with possession of the ball if exists so we don't mark them + uint8_t robotInPossession = kInvalidRobotId; + double min_dist_to_ball = std::numeric_limits::infinity(); + const auto& ball_pos = last_world_state_.ball.position; + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(false, i); + if (!robot.visible) { + continue; + } + double dist_to_ball = ball_pos.dist_to(robot.pose.position()); + if (dist_to_ball < min_dist_to_ball && dist_to_ball < 0.3) { + robotInPossession = i; + min_dist_to_ball = dist_to_ball; + } + } + + if (robotInPossession != kInvalidRobotId) { + SPDLOG_INFO("Robot in possession is {}", robotInPossession); + } + // finding most dangerous of non-marked robots uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); for (size_t i = 0; i < danger_score_.size(); ++i) { - if (enemey_to_friends_[i] != kInvalidRobotId) { + if (enemey_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { continue; } if (danger_score_[i] < min) { @@ -116,6 +151,15 @@ void Marking::publish_marking_list() { min = danger_score_[i]; } } + + for (size_t i = 0; i < marking_list_.size(); ++i) { + if (marking_list_[i] == robotInPossession && robotInPossession != kInvalidRobotId) { + enemey_to_friends_[robotInPossession] = kInvalidRobotId; + marking_list_[i] = most_dangerous; + enemey_to_friends_[most_dangerous] = i; + } + } + if (most_dangerous != kInvalidRobotId) { uint8_t not_dangerous_robot_id = kInvalidRobotId; double max_danger_sub = 0.0; From ac18cc7dd0913c8dcc32e64af492dc09b9416030 Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 28 Oct 2025 21:48:19 -0400 Subject: [PATCH 29/44] completed marking coordinator --- .../include/rj_strategy/agent/position/defense.hpp | 2 +- .../include/rj_strategy/coordinator/marking.hpp | 2 +- src/rj_strategy/src/coordinator/marking.cpp | 11 +++-------- 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 2f4ba0aa9d6..50e1827bbcf 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -45,7 +45,7 @@ class Defense : public Position { private: // static constexpr int kMaxWallers{6}; - static constexpr int kMaxWallers{0}; + static constexpr int kMaxWallers{2}; // static_cast(kNumShells)}; // This effectively turns off marking /** diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index 74bcd9e0454..1892ac02bac 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -38,7 +38,7 @@ class Marking void publish_marking_list(); void update_danger_scores(); - static constexpr int kMaxMarkers = 20; + static constexpr int kMaxMarkers = 2; // this is the threshold value for switching static constexpr double kSuperDangerSub = 3.1415926535; static constexpr double kDangerDistToBall = 3.0; diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 649449f5d3e..aa8aa3f27f1 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -134,11 +134,6 @@ void Marking::publish_marking_list() { } } - if (robotInPossession != kInvalidRobotId) { - SPDLOG_INFO("Robot in possession is {}", robotInPossession); - } - - // finding most dangerous of non-marked robots uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); @@ -248,9 +243,9 @@ void Marking::update_danger_scores() { danger_score_[i] = danger_score; } - for (size_t i = 0; i < 6; ++i) { - SPDLOG_INFO("Robot {} has danger score {}", i, danger_score_[i]); - } + // for (size_t i = 0; i < 6; ++i) { + // SPDLOG_INFO("Robot {} has danger score {}", i, danger_score_[i]); + // } } From a53480b4d60d8a1ee983afb33b91ec2413953df5 Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 28 Oct 2025 21:56:33 -0400 Subject: [PATCH 30/44] cleaned up a little --- .../rj_strategy/coordinator/marking.hpp | 2 + src/rj_strategy/src/coordinator/marking.cpp | 60 +++++++++---------- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index 1892ac02bac..b4494c5c053 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -37,9 +37,11 @@ class Marking private: void publish_marking_list(); void update_danger_scores(); + double find_their_robot_in_possession(); static constexpr int kMaxMarkers = 2; // this is the threshold value for switching + // you can play with these constants if the current results aren't good enough static constexpr double kSuperDangerSub = 3.1415926535; static constexpr double kDangerDistToBall = 3.0; static constexpr double kDangerDistToGoal = 5.0; diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index aa8aa3f27f1..28197711436 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -6,7 +6,7 @@ Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state marking_list_.fill(kInvalidRobotId); // initializes to no valid markers - enemey_to_friends_.fill(kInvalidRobotId); + enemey_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them danger_score_.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score num_markers_ = 0; world_state_sub_ = this->create_subscription( @@ -50,25 +50,12 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { return; } if (num_markers_ < kMaxMarkers) { - uint8_t robotInPossession = kInvalidRobotId; - double min_dist_to_ball = std::numeric_limits::infinity(); - const auto& ball_pos = last_world_state_.ball.position; - for (uint8_t i = 0; i < kNumShells; i++) { - const auto& robot = last_world_state_.get_robot(false, i); - if (!robot.visible) { - continue; - } - double dist_to_ball = ball_pos.dist_to(robot.pose.position()); - if (dist_to_ball < min_dist_to_ball && dist_to_ball < 0.3) { - robotInPossession = i; - min_dist_to_ball = dist_to_ball; - } - } - + uint8_t robotInPossession = find_their_robot_in_possession(); uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); for (size_t i = 0; i < danger_score_.size(); ++i) { + // don't include marked robots or the guy with the ball in most dangerous calculation if (enemey_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { continue; } @@ -119,25 +106,13 @@ void Marking::publish_marking_list() { update_danger_scores(); // find the guy with possession of the ball if exists so we don't mark them - uint8_t robotInPossession = kInvalidRobotId; - double min_dist_to_ball = std::numeric_limits::infinity(); - const auto& ball_pos = last_world_state_.ball.position; - for (uint8_t i = 0; i < kNumShells; i++) { - const auto& robot = last_world_state_.get_robot(false, i); - if (!robot.visible) { - continue; - } - double dist_to_ball = ball_pos.dist_to(robot.pose.position()); - if (dist_to_ball < min_dist_to_ball && dist_to_ball < 0.3) { - robotInPossession = i; - min_dist_to_ball = dist_to_ball; - } - } + uint8_t robotInPossession = find_their_robot_in_possession(); // finding most dangerous of non-marked robots uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); for (size_t i = 0; i < danger_score_.size(); ++i) { + // don't include marked robots or the guy with the ball in most dangerous calculation if (enemey_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { continue; } @@ -147,15 +122,19 @@ void Marking::publish_marking_list() { } } + // checking if anyone we are marking has possession and if they are, then they should take the most dangerous guy + // then he would have been assigned to someone so no need to check the others + bool assigned = false; for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] == robotInPossession && robotInPossession != kInvalidRobotId) { enemey_to_friends_[robotInPossession] = kInvalidRobotId; marking_list_[i] = most_dangerous; enemey_to_friends_[most_dangerous] = i; + assigned = true; } } - if (most_dangerous != kInvalidRobotId) { + if (most_dangerous != kInvalidRobotId && !assigned) { uint8_t not_dangerous_robot_id = kInvalidRobotId; double max_danger_sub = 0.0; for (size_t i = 0; i < marking_list_.size(); ++i) { @@ -248,6 +227,25 @@ void Marking::update_danger_scores() { // } } +double Marking::find_their_robot_in_possession() { + uint8_t robotInPossession = kInvalidRobotId; + double min_dist_to_ball = std::numeric_limits::infinity(); + const auto& ball_pos = last_world_state_.ball.position; + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(false, i); + if (!robot.visible) { + continue; + } + double dist_to_ball = ball_pos.dist_to(robot.pose.position()); + if (dist_to_ball < min_dist_to_ball && dist_to_ball < 0.3) { + robotInPossession = i; + min_dist_to_ball = dist_to_ball; + } + } + + return robotInPossession; +} + } // namespace strategy From 3914d607cd9d1f946981bbbfffc5809ba181ed48 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 2 Nov 2025 19:52:56 -0500 Subject: [PATCH 31/44] Fix Code Style On marking-coordinator (#2447) automated style fixes Co-authored-by: petergarud --- .../rj_strategy/agent/position/defense.hpp | 4 +- .../agent/position/free_kicker.hpp | 1 - .../rj_strategy/coordinator/marking.hpp | 14 +++--- .../coordinator/marking_client.hpp | 1 + .../src/agent/position/defense.cpp | 26 ++++++---- .../src/agent/position/free_kicker.cpp | 6 ++- .../src/agent/position/goal_kicker.cpp | 3 +- src/rj_strategy/src/agent/position/goalie.cpp | 6 ++- src/rj_strategy/src/agent/position/idle.cpp | 6 ++- .../src/agent/position/offense.cpp | 6 ++- .../src/agent/position/penalty_non_kicker.cpp | 7 ++- .../src/agent/position/penalty_player.cpp | 6 ++- .../src/agent/position/pivot_test.cpp | 3 +- .../agent/position/robot_factory_position.cpp | 8 ++-- src/rj_strategy/src/agent/position/seeker.cpp | 5 +- .../src/agent/position/smartidling.cpp | 6 ++- .../src/agent/position/solo_offense.cpp | 6 ++- src/rj_strategy/src/agent/position/zoner.cpp | 9 ++-- src/rj_strategy/src/coordinator/marking.cpp | 48 +++++++++++-------- .../src/coordinator/marking_client.cpp | 5 +- 20 files changed, 109 insertions(+), 67 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 50e1827bbcf..73bb3b7bc3d 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -46,7 +46,7 @@ class Defense : public Position { private: // static constexpr int kMaxWallers{6}; static constexpr int kMaxWallers{2}; - // static_cast(kNumShells)}; // This effectively turns off marking + // static_cast(kNumShells)}; // This effectively turns off marking /** * @brief The derived_get_task method returns the task for the defensive robot @@ -122,7 +122,7 @@ class Defense : public Position { bool sent_join_marking_group_request_ = false; RJ::Time request_time_; - RJ::Seconds kMarkingGroupJoinTimeout{2.0}; // 2 seconds + RJ::Seconds kMarkingGroupJoinTimeout{2.0}; // 2 seconds bool pending_marking_state_ = false; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp index 803438a6185..1df127c5941 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp @@ -41,7 +41,6 @@ class FreeKicker : public Position { private: std::optional derived_get_task(RobotIntent intent) override; std::shared_ptr clientHandles_; - }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index b4494c5c053..29522beeea0 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -1,13 +1,13 @@ #pragma once -#include #include // for std::any_of +#include #include #include -#include #include +#include #include #include #include @@ -17,11 +17,9 @@ #include "rj_strategy/coordinator.hpp" - namespace strategy { -class Marking - : public Coordinator { +class Marking : public Coordinator { public: static constexpr uint8_t kInvalidRobotId = kNumShells; @@ -49,8 +47,10 @@ class Marking static constexpr double kDangerAngle = 2.0; int num_markers_; - std::array marking_list_{}; // Initialize it to invalid robot id in constructor - std::array danger_score_{}; // infinity initialized in constructor, no one is a valid target initially + std::array + marking_list_{}; // Initialize it to invalid robot id in constructor + std::array + danger_score_{}; // infinity initialized in constructor, no one is a valid target initially std::array enemey_to_friends_{}; std::vector queue_; WorldState last_world_state_; diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp index 0a262cee5e0..d9b745fe6e8 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp @@ -7,6 +7,7 @@ #include #include + #include "rj_constants/constants.hpp" namespace strategy { diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 64a22738da1..6d18575b3fe 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -9,9 +9,11 @@ Defense::Defense(const Position& other) : Position{other} { walling_robots_ = {}; } -Defense::Defense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Defense"), clientHandles_{clientHandles} {} +Defense::Defense(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "Defense"), clientHandles_{clientHandles} {} -Defense::Defense(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { +Defense::Defense(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} { position_name_ = "Defense"; walling_robots_ = {}; } @@ -43,7 +45,8 @@ Defense::State Defense::update_state() { return IDLING; } - if (clientHandles_->markingClient->am_i_member() && clientHandles_->markingClient->am_i_marking()) { + if (clientHandles_->markingClient->am_i_member() && + clientHandles_->markingClient->am_i_marking()) { return MARKING; } else { SPDLOG_INFO("Robot {}: After pending marking, not a member so idling", robot_id_); @@ -105,7 +108,8 @@ Defense::State Defense::update_state() { next_state = IDLING; } case MARKING: - if (!clientHandles_->markingClient->am_i_member() || !clientHandles_->markingClient->am_i_marking()) { + if (!clientHandles_->markingClient->am_i_member() || + !clientHandles_->markingClient->am_i_marking()) { next_state = IDLING; } break; @@ -116,7 +120,6 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = true; request_time_ = RJ::now(); - clientHandles_->markingClient->join_group([this](const MarkingClient::Result& res) { if (res.am_i_member && res.am_i_marking) { pending_marking_state_ = true; @@ -205,13 +208,18 @@ std::optional Defense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == MARKING) { - rj_geometry::Point targetPoint = last_world_state_->get_robot(false, clientHandles_->markingClient->who_am_i_marking()).pose.position(); + rj_geometry::Point targetPoint = + last_world_state_->get_robot(false, clientHandles_->markingClient->who_am_i_marking()) + .pose.position(); rj_geometry::Point ballPoint = last_world_state_->ball.position; rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); - planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; - // SPDLOG_INFO("Location to mark: {}, {}", (targetPoint + targetToBall).x(), (targetPoint + targetToBall).y()); - intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; + planning::LinearMotionInstant goal{targetPoint + targetToBall, + rj_geometry::Point{0.0, 0.0}}; + // SPDLOG_INFO("Location to mark: {}, {}", (targetPoint + targetToBall).x(), (targetPoint + + // targetToBall).y()); + intent.motion_command = + planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; return intent; } diff --git a/src/rj_strategy/src/agent/position/free_kicker.cpp b/src/rj_strategy/src/agent/position/free_kicker.cpp index 7b64e1d882a..5e864c3bf86 100644 --- a/src/rj_strategy/src/agent/position/free_kicker.cpp +++ b/src/rj_strategy/src/agent/position/free_kicker.cpp @@ -5,9 +5,11 @@ namespace strategy { FreeKicker::FreeKicker(int r_id) : Position(r_id, "FreeKicker") {} FreeKicker::FreeKicker(const Position& other) : Position{other} { position_name_ = "FreeKicker"; } -FreeKicker::FreeKicker(int r_id, std::shared_ptr clientHandles) : Position(r_id, "FreeKicker"), clientHandles_{clientHandles} {} +FreeKicker::FreeKicker(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "FreeKicker"), clientHandles_{clientHandles} {} -FreeKicker::FreeKicker(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { +FreeKicker::FreeKicker(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} { position_name_ = "FreeKicker"; } diff --git a/src/rj_strategy/src/agent/position/goal_kicker.cpp b/src/rj_strategy/src/agent/position/goal_kicker.cpp index b0992be16cd..fb621958db7 100644 --- a/src/rj_strategy/src/agent/position/goal_kicker.cpp +++ b/src/rj_strategy/src/agent/position/goal_kicker.cpp @@ -3,7 +3,8 @@ namespace strategy { GoalKicker::GoalKicker(int r_id) : Position(r_id, "GoalKicker") {} -GoalKicker::GoalKicker(int r_id, std::shared_ptr clientHandles) : Position(r_id, "GoalKicker"), clientHandles_{clientHandles} {} +GoalKicker::GoalKicker(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "GoalKicker"), clientHandles_{clientHandles} {} std::optional GoalKicker::derived_get_task(RobotIntent intent) { // Penalty Kicker kicks the ball into the goal diff --git a/src/rj_strategy/src/agent/position/goalie.cpp b/src/rj_strategy/src/agent/position/goalie.cpp index 67e20f1119f..119686e3b8b 100644 --- a/src/rj_strategy/src/agent/position/goalie.cpp +++ b/src/rj_strategy/src/agent/position/goalie.cpp @@ -6,9 +6,11 @@ Goalie::Goalie(int r_id) : Position(r_id, "Goalie") {} Goalie::Goalie(const Position& other) : Position{other} {} -Goalie::Goalie(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Goalie"), clientHandles_{clientHandles} {} +Goalie::Goalie(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "Goalie"), clientHandles_{clientHandles} {} -Goalie::Goalie(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { } +Goalie::Goalie(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} {} std::optional Goalie::derived_get_task(RobotIntent intent) { latest_state_ = update_state(); diff --git a/src/rj_strategy/src/agent/position/idle.cpp b/src/rj_strategy/src/agent/position/idle.cpp index 2daf7ca3831..4c4658f2abe 100644 --- a/src/rj_strategy/src/agent/position/idle.cpp +++ b/src/rj_strategy/src/agent/position/idle.cpp @@ -6,9 +6,11 @@ Idle::Idle(int r_id) : Position{r_id, "Idle"} {} Idle::Idle(const Position& other) : Position{other} {} -Idle::Idle(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Idle"), clientHandles_{clientHandles} {} +Idle::Idle(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "Idle"), clientHandles_{clientHandles} {} -Idle::Idle(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} {} +Idle::Idle(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} {} std::string Idle::get_current_state() { return "Idle"; } diff --git a/src/rj_strategy/src/agent/position/offense.cpp b/src/rj_strategy/src/agent/position/offense.cpp index 0c3d73e9e8c..1162bcfb5c1 100644 --- a/src/rj_strategy/src/agent/position/offense.cpp +++ b/src/rj_strategy/src/agent/position/offense.cpp @@ -8,9 +8,11 @@ Offense::Offense(const Position& other) : Position{other}, seeker_{robot_id_} { position_name_ = "Offense"; } -Offense::Offense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Offense"), seeker_{r_id}, clientHandles_{clientHandles} {} +Offense::Offense(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "Offense"), seeker_{r_id}, clientHandles_{clientHandles} {} -Offense::Offense(const Position& other, std::shared_ptr clientHandles) : Position{other}, seeker_{robot_id_}, clientHandles_{clientHandles} { +Offense::Offense(const Position& other, std::shared_ptr clientHandles) + : Position{other}, seeker_{robot_id_}, clientHandles_{clientHandles} { position_name_ = "Offense"; } diff --git a/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp b/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp index 4ca06e66347..e36b1a995db 100644 --- a/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp +++ b/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp @@ -6,9 +6,12 @@ PenaltyNonKicker::PenaltyNonKicker(int r_id) : Position{r_id, "PenaltyNonKicker" PenaltyNonKicker::PenaltyNonKicker(const Position& other) : Position{other} {} -PenaltyNonKicker::PenaltyNonKicker(int r_id, std::shared_ptr clientHandles) : Position(r_id, "PenaltyNonKicker"), clientHandles_{clientHandles} {} +PenaltyNonKicker::PenaltyNonKicker(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "PenaltyNonKicker"), clientHandles_{clientHandles} {} -PenaltyNonKicker::PenaltyNonKicker(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} {} +PenaltyNonKicker::PenaltyNonKicker(const Position& other, + std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} {} std::string PenaltyNonKicker::get_current_state() { return "PenaltyNonKicker"; } diff --git a/src/rj_strategy/src/agent/position/penalty_player.cpp b/src/rj_strategy/src/agent/position/penalty_player.cpp index 493236b853b..33b3e5f4f93 100644 --- a/src/rj_strategy/src/agent/position/penalty_player.cpp +++ b/src/rj_strategy/src/agent/position/penalty_player.cpp @@ -7,9 +7,11 @@ PenaltyPlayer::PenaltyPlayer(int r_id) : Position(r_id, "PenaltyPlayer") {} PenaltyPlayer::PenaltyPlayer(const Position& other) : Position{other} {} -PenaltyPlayer::PenaltyPlayer(int r_id, std::shared_ptr clientHandles) : Position(r_id, "PenaltyPlayer"), clientHandles_{clientHandles} {} +PenaltyPlayer::PenaltyPlayer(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "PenaltyPlayer"), clientHandles_{clientHandles} {} -PenaltyPlayer::PenaltyPlayer(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} {} +PenaltyPlayer::PenaltyPlayer(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} {} std::optional PenaltyPlayer::derived_get_task(RobotIntent intent) { latest_state_ = update_state(); diff --git a/src/rj_strategy/src/agent/position/pivot_test.cpp b/src/rj_strategy/src/agent/position/pivot_test.cpp index 02adbcef096..bfa46e60b3d 100644 --- a/src/rj_strategy/src/agent/position/pivot_test.cpp +++ b/src/rj_strategy/src/agent/position/pivot_test.cpp @@ -4,7 +4,8 @@ namespace strategy { Pivot::Pivot(int r_id) : Position{r_id, "Pivot"} {} Pivot::Pivot(int r_id, std::shared_ptr clientHandles) : Position{r_id, "Pivot"} {} -Pivot::Pivot(const Position& other, std::shared_ptr clientHandles) : Position{other} {} +Pivot::Pivot(const Position& other, std::shared_ptr clientHandles) + : Position{other} {} std::optional Pivot::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock diff --git a/src/rj_strategy/src/agent/position/robot_factory_position.cpp b/src/rj_strategy/src/agent/position/robot_factory_position.cpp index 3451780d702..bc6bda4c0a1 100644 --- a/src/rj_strategy/src/agent/position/robot_factory_position.cpp +++ b/src/rj_strategy/src/agent/position/robot_factory_position.cpp @@ -3,7 +3,7 @@ namespace strategy { RobotFactoryPosition::RobotFactoryPosition(int r_id, rclcpp::Node::SharedPtr node) - : Position(r_id, "RobotFactoryPosition"), clientHandles_(std::make_shared()) { + : Position(r_id, "RobotFactoryPosition"), clientHandles_(std::make_shared()) { clientHandles_->kickerPickerClient = std::make_unique(node, r_id); clientHandles_->markingClient = std::make_unique(node, r_id); if (robot_id_ == 0) { @@ -83,7 +83,8 @@ void RobotFactoryPosition::process_play_state() { void RobotFactoryPosition::handle_stop() { set_default_position(); } void RobotFactoryPosition::handle_penalty_playing() { - if (!(clientHandles_->kickerPickerClient->am_i_member() && clientHandles_->kickerPickerClient->is_selected())) { + if (!(clientHandles_->kickerPickerClient->am_i_member() && + clientHandles_->kickerPickerClient->is_selected())) { set_current_position(); } } @@ -95,7 +96,8 @@ void RobotFactoryPosition::handle_setup() { if ((current_play_state_.is_kickoff() || current_play_state_.is_penalty()) && !clientHandles_->kickerPickerClient->am_i_member()) { - clientHandles_->kickerPickerClient->join_group([this](KickerPickerClient::Result result) { + clientHandles_->kickerPickerClient->join_group([this]( + KickerPickerClient::Result result) { if (result.am_i_member && result.kicker_id == robot_id_ && current_play_state_.is_kickoff()) { set_current_position(); diff --git a/src/rj_strategy/src/agent/position/seeker.cpp b/src/rj_strategy/src/agent/position/seeker.cpp index f952173f565..51ccb464654 100644 --- a/src/rj_strategy/src/agent/position/seeker.cpp +++ b/src/rj_strategy/src/agent/position/seeker.cpp @@ -3,7 +3,10 @@ namespace strategy { Seeker::Seeker(int robot_id) { robot_id_ = robot_id; } -Seeker::Seeker(int r_id, std::shared_ptr clientHandles) : clientHandles_{clientHandles} { robot_id_ = r_id; } +Seeker::Seeker(int r_id, std::shared_ptr clientHandles) + : clientHandles_{clientHandles} { + robot_id_ = r_id; +} Seeker::Seeker(const Position& other, std::shared_ptr clientHandles) {} std::optional Seeker::get_task(RobotIntent intent, const WorldState* last_world_state, diff --git a/src/rj_strategy/src/agent/position/smartidling.cpp b/src/rj_strategy/src/agent/position/smartidling.cpp index 1dad60a9c2a..3c7b2a82ceb 100644 --- a/src/rj_strategy/src/agent/position/smartidling.cpp +++ b/src/rj_strategy/src/agent/position/smartidling.cpp @@ -6,9 +6,11 @@ SmartIdle::SmartIdle(int r_id) : Position{r_id, "SmartIdle"} {} SmartIdle::SmartIdle(const Position& other) : Position{other} {} -SmartIdle::SmartIdle(int r_id, std::shared_ptr clientHandles) : Position(r_id, "SmartIdle"), clientHandles_{clientHandles} {} +SmartIdle::SmartIdle(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "SmartIdle"), clientHandles_{clientHandles} {} -SmartIdle::SmartIdle(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { } +SmartIdle::SmartIdle(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} {} std::string SmartIdle::get_current_state() { return "SmartIdle"; } diff --git a/src/rj_strategy/src/agent/position/solo_offense.cpp b/src/rj_strategy/src/agent/position/solo_offense.cpp index 2cfedddf283..a2ed0e10fe6 100644 --- a/src/rj_strategy/src/agent/position/solo_offense.cpp +++ b/src/rj_strategy/src/agent/position/solo_offense.cpp @@ -8,9 +8,11 @@ SoloOffense::SoloOffense(const Position& other) : Position{other} { SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} -SoloOffense::SoloOffense(int r_id, std::shared_ptr clientHandles) : Position(r_id, "SoloOffense"), clientHandles_{clientHandles} {} +SoloOffense::SoloOffense(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "SoloOffense"), clientHandles_{clientHandles} {} -SoloOffense::SoloOffense(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { +SoloOffense::SoloOffense(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} { position_name_ = "SoloOffense"; } diff --git a/src/rj_strategy/src/agent/position/zoner.cpp b/src/rj_strategy/src/agent/position/zoner.cpp index e500e785d5a..e5d3ccd8443 100644 --- a/src/rj_strategy/src/agent/position/zoner.cpp +++ b/src/rj_strategy/src/agent/position/zoner.cpp @@ -6,10 +6,13 @@ Zoner::Zoner(int r_id) : Position(r_id, "Zoner") {} Zoner::Zoner(const Position& other) : Position{other} { position_name_ = "Zoner"; } -Zoner::Zoner(int r_id, std::shared_ptr clientHandles) : Position(r_id, "Zoner"), clientHandles_{clientHandles} {} +Zoner::Zoner(int r_id, std::shared_ptr clientHandles) + : Position(r_id, "Zoner"), clientHandles_{clientHandles} {} -Zoner::Zoner(const Position& other, std::shared_ptr clientHandles) : Position{other}, clientHandles_{clientHandles} { - position_name_ = "Zoner"; } +Zoner::Zoner(const Position& other, std::shared_ptr clientHandles) + : Position{other}, clientHandles_{clientHandles} { + position_name_ = "Zoner"; +} std::optional Zoner::derived_get_task(RobotIntent intent) { current_state_ = next_state(); diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 28197711436..034eb73f6c1 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -2,12 +2,12 @@ namespace strategy { -Marking::Marking() - : Coordinator("marking_srv", "marking_data", "marking_node") { +Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state - marking_list_.fill(kInvalidRobotId); // initializes to no valid markers - enemey_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them - danger_score_.fill(std::numeric_limits::infinity()); // everyone starts with an infinite danger score + marking_list_.fill(kInvalidRobotId); // initializes to no valid markers + enemey_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them + danger_score_.fill( + std::numeric_limits::infinity()); // everyone starts with an infinite danger score num_markers_ = 0; world_state_sub_ = this->create_subscription( vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), @@ -38,13 +38,15 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { } } if (kInvalidRobotId != waiting_robot_id) { - queue_.erase(std::remove(queue_.begin(), queue_.end(), waiting_robot_id), queue_.end()); + queue_.erase(std::remove(queue_.begin(), queue_.end(), waiting_robot_id), + queue_.end()); marking_list_[waiting_robot_id] = enemey_id; enemey_to_friends_[enemey_id] = waiting_robot_id; num_markers_++; } } else { - queue_.erase(std::remove(queue_.begin(), queue_.end(), request->robot_id), queue_.end()); + queue_.erase(std::remove(queue_.begin(), queue_.end(), request->robot_id), + queue_.end()); } response->success = true; return; @@ -81,7 +83,9 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { uint8_t enemey_id = marking_list_[i]; const auto& i_robot = last_world_state_.get_robot(true, i); const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); - double dist = (i_robot.pose.position().dist_to(enemey_robot.pose.position())) - (robot_requesting.pose.position().dist_to(enemey_robot.pose.position())); + double dist = + (i_robot.pose.position().dist_to(enemey_robot.pose.position())) - + (robot_requesting.pose.position().dist_to(enemey_robot.pose.position())); if (dist > better_distance) { better_distance = dist; kick_out_this_robot_id = i; @@ -122,8 +126,8 @@ void Marking::publish_marking_list() { } } - // checking if anyone we are marking has possession and if they are, then they should take the most dangerous guy - // then he would have been assigned to someone so no need to check the others + // checking if anyone we are marking has possession and if they are, then they should take the + // most dangerous guy then he would have been assigned to someone so no need to check the others bool assigned = false; for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] == robotInPossession && robotInPossession != kInvalidRobotId) { @@ -147,7 +151,8 @@ void Marking::publish_marking_list() { } } } - // seeing if most dangerous of non-marked robots is significantly more dangerous than any marked robot + // seeing if most dangerous of non-marked robots is significantly more dangerous than any + // marked robot if (max_danger_sub > kSuperDangerSub && not_dangerous_robot_id != kInvalidRobotId) { uint8_t friend_id = enemey_to_friends_[not_dangerous_robot_id]; enemey_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; @@ -160,9 +165,9 @@ void Marking::publish_marking_list() { } void Marking::update_danger_scores() { - - // danger score calculation is distance_to_ball * constant + distance_to_goal * constant - distance_from_our_closest_robot * constant - danger_angle * constant - // lower danger score is more dangerous + // danger score calculation is distance_to_ball * constant + distance_to_goal * constant - + // distance_from_our_closest_robot * constant - danger_angle * constant lower danger score is + // more dangerous const auto& ball_pos = last_world_state_.ball.position; const auto& goal_loc = field_dimensions_.our_goal_loc(); @@ -188,7 +193,7 @@ void Marking::update_danger_scores() { } } - double angle_between = 0.0; // Default to 0 (not dangerous) + double angle_between = 0.0; // Default to 0 (not dangerous) // Check if beyond midfield bool onOurSide = false; @@ -203,21 +208,23 @@ void Marking::update_danger_scores() { const auto& vec_goal_to_robot = robot.pose.position() - goal_loc; double cosTheta = vec_goal_to_center.dot(vec_goal_to_robot) / - (vec_goal_to_center.mag() * vec_goal_to_robot.mag()); + (vec_goal_to_center.mag() * vec_goal_to_robot.mag()); if (cosTheta > 1.0) cosTheta = 1.0; if (cosTheta < -1.0) cosTheta = -1.0; - double central_angle = std::abs(std::acos(cosTheta)); // [0, PI/2] + double central_angle = std::abs(std::acos(cosTheta)); // [0, PI/2] // Normalize double normalized_danger = (M_PI_2 - central_angle) / M_PI_2; - if (normalized_danger < 0.0) normalized_danger = 0.0; // Clamp + if (normalized_danger < 0.0) normalized_danger = 0.0; // Clamp - // Scales angles so that more central angles close together and more sideline are futher apart + // Scales angles so that more central angles close together and more sideline are futher + // apart const double kDangerAngleExponent = 0.25; angle_between = std::pow(normalized_danger, kDangerAngleExponent); } - double danger_score = dist_to_ball * kDangerDistToBall + dist_to_goal * kDangerDistToGoal - min * kDangerDistToOurRobots - angle_between * kDangerAngle; + double danger_score = dist_to_ball * kDangerDistToBall + dist_to_goal * kDangerDistToGoal - + min * kDangerDistToOurRobots - angle_between * kDangerAngle; danger_score_[i] = danger_score; } @@ -246,7 +253,6 @@ double Marking::find_their_robot_in_possession() { return robotInPossession; } - } // namespace strategy int main(int argc, char* argv[]) { diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp index f777cd24a54..5dbef77fac6 100644 --- a/src/rj_strategy/src/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -9,12 +9,13 @@ namespace strategy { */ MarkingClient::MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) - : node_{std::move(node)}, robot_id_{robot_id}, selected_robot_marking_id_{MarkingClient::kInvalidRobotId} { + : node_{std::move(node)}, + robot_id_{robot_id}, + selected_robot_marking_id_{MarkingClient::kInvalidRobotId} { client_ = node_->create_client("marking_srv"); } void MarkingClient::join_group(StatusCallback callback) { - if (am_i_member_) { return; } From 3073ef6027bc29ca195793a1025e0601bda4f698 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 2 Nov 2025 21:42:18 -0500 Subject: [PATCH 32/44] Resolve merge comments --- rj_gameplay/rj_gameplay.egg-info/PKG-INFO | 12 --- rj_gameplay/rj_gameplay.egg-info/SOURCES.txt | 90 ------------------- .../rj_gameplay.egg-info/dependency_links.txt | 1 - .../rj_gameplay.egg-info/entry_points.txt | 3 - rj_gameplay/rj_gameplay.egg-info/requires.txt | 1 - .../rj_gameplay.egg-info/top_level.txt | 2 - rj_gameplay/rj_gameplay.egg-info/zip-safe | 1 - .../src/agent/position/defense.cpp | 2 - src/rj_strategy/src/coordinator/marking.cpp | 17 +++- 9 files changed, 13 insertions(+), 116 deletions(-) delete mode 100644 rj_gameplay/rj_gameplay.egg-info/PKG-INFO delete mode 100644 rj_gameplay/rj_gameplay.egg-info/SOURCES.txt delete mode 100644 rj_gameplay/rj_gameplay.egg-info/dependency_links.txt delete mode 100644 rj_gameplay/rj_gameplay.egg-info/entry_points.txt delete mode 100644 rj_gameplay/rj_gameplay.egg-info/requires.txt delete mode 100644 rj_gameplay/rj_gameplay.egg-info/top_level.txt delete mode 100644 rj_gameplay/rj_gameplay.egg-info/zip-safe diff --git a/rj_gameplay/rj_gameplay.egg-info/PKG-INFO b/rj_gameplay/rj_gameplay.egg-info/PKG-INFO deleted file mode 100644 index 85e86975ea7..00000000000 --- a/rj_gameplay/rj_gameplay.egg-info/PKG-INFO +++ /dev/null @@ -1,12 +0,0 @@ -Metadata-Version: 2.1 -Name: rj-gameplay -Version: 0.0.0 -Summary: Rewrite of the gameplay library. -Home-page: UNKNOWN -Maintainer: oswinso -Maintainer-email: oswinso@gmail.com -License: UNKNOWN -Platform: UNKNOWN - -UNKNOWN - diff --git a/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt b/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt deleted file mode 100644 index 83aa7c4406b..00000000000 --- a/rj_gameplay/rj_gameplay.egg-info/SOURCES.txt +++ /dev/null @@ -1,90 +0,0 @@ -README.md -setup.cfg -setup.py -rj_gameplay/__init__.py -rj_gameplay/gameplay_node.py -rj_gameplay/play_selector.py -rj_gameplay.egg-info/PKG-INFO -rj_gameplay.egg-info/SOURCES.txt -rj_gameplay.egg-info/dependency_links.txt -rj_gameplay.egg-info/entry_points.txt -rj_gameplay.egg-info/requires.txt -rj_gameplay.egg-info/top_level.txt -rj_gameplay.egg-info/zip-safe -rj_gameplay/eval/__init__.py -rj_gameplay/play/__init__.py -rj_gameplay/play/defend_restart.py -rj_gameplay/play/defense.py -rj_gameplay/play/defensive_clear.py -rj_gameplay/play/keepaway.py -rj_gameplay/play/kickoff_play.py -rj_gameplay/play/offense.py -rj_gameplay/play/penalty_defense.py -rj_gameplay/play/penalty_offense.py -rj_gameplay/play/prep_penalty_offense.py -rj_gameplay/play/restart.py -rj_gameplay/play/test_motion_planning.py -rj_gameplay/role/__init__.py -rj_gameplay/role/capture_role.py -rj_gameplay/role/dumb_move.py -rj_gameplay/role/goalie_role.py -rj_gameplay/role/marker.py -rj_gameplay/role/passer.py -rj_gameplay/role/receiver.py -rj_gameplay/role/seeker.py -rj_gameplay/role/striker.py -rj_gameplay/situation/__init__.py -rj_gameplay/situation/decision_tree/__init__.py -rj_gameplay/situation/decision_tree/plays.py -rj_gameplay/skill/__init__.py -rj_gameplay/skill/capture.py -rj_gameplay/skill/dribble.py -rj_gameplay/skill/intercept.py -rj_gameplay/skill/kick.py -rj_gameplay/skill/line_kick.py -rj_gameplay/skill/mark.py -rj_gameplay/skill/move.py -rj_gameplay/skill/pivot.py -rj_gameplay/skill/pivot_kick.py -rj_gameplay/skill/receive.py -rj_gameplay/skill/settle.py -rj_gameplay/tactic/__init__.py -rj_gameplay/tactic/clear_tactic.py -rj_gameplay/tactic/dumb_tactic.py -rj_gameplay/tactic/goalie_tactic.py -rj_gameplay/tactic/line_tactic.py -rj_gameplay/tactic/move_tactic.py -rj_gameplay/tactic/nmark_tactic.py -rj_gameplay/tactic/pass_tactic.py -rj_gameplay/tactic/prep_move.py -rj_gameplay/tactic/seek.py -rj_gameplay/tactic/striker_tactic.py -rj_gameplay/tactic/wall_tactic.py -stp/__init__.py -stp/global_parameters.py -stp/local_parameters.py -stp/pylint_stp.py -stp/rc.py -stp/testing.py -stp/action/__init__.py -stp/formations/__init__.py -stp/formations/diamond_formation.py -stp/formations/x_formation.py -stp/play/__init__.py -stp/play/pure_play.py -stp/role/__init__.py -stp/role/constraint.py -stp/role/cost.py -stp/role/assignment/__init__.py -stp/role/assignment/naive.py -stp/situation/__init__.py -stp/skill/__init__.py -stp/skill/action_behavior.py -stp/tactic/__init__.py -stp/utils/__init__.py -stp/utils/constants.py -stp/utils/enum.py -stp/utils/fsm.py -stp/utils/pass_seeker_optimizer.py -stp/utils/typed_key_dict.py -stp/utils/world_state_converter.py \ No newline at end of file diff --git a/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt b/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt deleted file mode 100644 index 8b137891791..00000000000 --- a/rj_gameplay/rj_gameplay.egg-info/dependency_links.txt +++ /dev/null @@ -1 +0,0 @@ - diff --git a/rj_gameplay/rj_gameplay.egg-info/entry_points.txt b/rj_gameplay/rj_gameplay.egg-info/entry_points.txt deleted file mode 100644 index a641d186309..00000000000 --- a/rj_gameplay/rj_gameplay.egg-info/entry_points.txt +++ /dev/null @@ -1,3 +0,0 @@ -[console_scripts] -gameplay_node = rj_gameplay.gameplay_node:main - diff --git a/rj_gameplay/rj_gameplay.egg-info/requires.txt b/rj_gameplay/rj_gameplay.egg-info/requires.txt deleted file mode 100644 index 49fe098d9e6..00000000000 --- a/rj_gameplay/rj_gameplay.egg-info/requires.txt +++ /dev/null @@ -1 +0,0 @@ -setuptools diff --git a/rj_gameplay/rj_gameplay.egg-info/top_level.txt b/rj_gameplay/rj_gameplay.egg-info/top_level.txt deleted file mode 100644 index 9efb5b023a4..00000000000 --- a/rj_gameplay/rj_gameplay.egg-info/top_level.txt +++ /dev/null @@ -1,2 +0,0 @@ -rj_gameplay -stp diff --git a/rj_gameplay/rj_gameplay.egg-info/zip-safe b/rj_gameplay/rj_gameplay.egg-info/zip-safe deleted file mode 100644 index 8b137891791..00000000000 --- a/rj_gameplay/rj_gameplay.egg-info/zip-safe +++ /dev/null @@ -1 +0,0 @@ - diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 6d18575b3fe..ae4e08f4667 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -49,7 +49,6 @@ Defense::State Defense::update_state() { clientHandles_->markingClient->am_i_marking()) { return MARKING; } else { - SPDLOG_INFO("Robot {}: After pending marking, not a member so idling", robot_id_); return IDLING; } } @@ -64,7 +63,6 @@ Defense::State Defense::update_state() { switch (current_state_) { case IDLING: - // SPDLOG_INFO("Robot {}: idling", robot_id_); next_state = JOINING_WALL; break; case JOINING_WALL: diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 034eb73f6c1..33501a0ab15 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -25,10 +25,13 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { marking_list_[request->robot_id] = kInvalidRobotId; enemey_to_friends_[enemey_id] = kInvalidRobotId; num_markers_--; - // replace if possible + // The robot is leaving so replace the robot its marking if possible const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); double min = std::numeric_limits::infinity(); uint8_t waiting_robot_id = kInvalidRobotId; + // Find closest robot in queue for replacement + // Queue is queue of robots that want to join marking but aren't good enough + // Not close enough to mark or we exceeed the max num of markers for (size_t i = 0; i < queue_.size(); ++i) { const auto& i_robot = last_world_state_.get_robot(true, queue_[i]); double distance = i_robot.pose.position().dist_to(enemey_robot.pose.position()); @@ -52,6 +55,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { return; } if (num_markers_ < kMaxMarkers) { + // this means we can add this prospective marker as a marker uint8_t robotInPossession = find_their_robot_in_possession(); uint8_t most_dangerous = kInvalidRobotId; @@ -67,6 +71,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { } } if (most_dangerous != kInvalidRobotId) { + // Assign most dangerous unmarked robot without ball enemey_to_friends_[most_dangerous] = request->robot_id; marking_list_[request->robot_id] = most_dangerous; num_markers_++; @@ -74,10 +79,11 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { queue_.push_back(request->robot_id); } } else { - // should we kick someone out + // should we kick someone out (is this new robot a better marker) double better_distance = 0; uint8_t kick_out_this_robot_id = kInvalidRobotId; const auto& robot_requesting = last_world_state_.get_robot(true, request->robot_id); + // Kicked out robot is one that is furthest from its marker and new robot is closer than it for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] != kInvalidRobotId) { uint8_t enemey_id = marking_list_[i]; @@ -126,9 +132,9 @@ void Marking::publish_marking_list() { } } - // checking if anyone we are marking has possession and if they are, then they should take the - // most dangerous guy then he would have been assigned to someone so no need to check the others bool assigned = false; + // check if anyone being marked has the ball + // if so, remove them from being marked and assign them the most dangerous robot for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] == robotInPossession && robotInPossession != kInvalidRobotId) { enemey_to_friends_[robotInPossession] = kInvalidRobotId; @@ -138,6 +144,7 @@ void Marking::publish_marking_list() { } } + // If the most dangerous is not assigned then find the robot that is assigned to least dangerous if (most_dangerous != kInvalidRobotId && !assigned) { uint8_t not_dangerous_robot_id = kInvalidRobotId; double max_danger_sub = 0.0; @@ -153,6 +160,8 @@ void Marking::publish_marking_list() { } // seeing if most dangerous of non-marked robots is significantly more dangerous than any // marked robot + // This only gets rid of the most dangerous non-marked robot + // , subsequent runs will pick up next most dangerous if (max_danger_sub > kSuperDangerSub && not_dangerous_robot_id != kInvalidRobotId) { uint8_t friend_id = enemey_to_friends_[not_dangerous_robot_id]; enemey_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; From 9207a0c4b9e9c1157ed10ad4a819930c5100c217 Mon Sep 17 00:00:00 2001 From: petergarud Date: Sun, 9 Nov 2025 21:00:43 -0500 Subject: [PATCH 33/44] Resolve comments --- .../include/rj_strategy/agent/position/defense.hpp | 3 +-- src/rj_strategy/include/rj_strategy/coordinator/marking.hpp | 6 +++--- src/rj_strategy/src/coordinator/kicker_picker_client.cpp | 1 - 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 73bb3b7bc3d..d315c753ce9 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -46,7 +46,6 @@ class Defense : public Position { private: // static constexpr int kMaxWallers{6}; static constexpr int kMaxWallers{2}; - // static_cast(kNumShells)}; // This effectively turns off marking /** * @brief The derived_get_task method returns the task for the defensive robot @@ -122,7 +121,7 @@ class Defense : public Position { bool sent_join_marking_group_request_ = false; RJ::Time request_time_; - RJ::Seconds kMarkingGroupJoinTimeout{2.0}; // 2 seconds + RJ::Seconds kMarkingGroupJoinTimeout{2.0}; bool pending_marking_state_ = false; diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index 29522beeea0..e3f752901a0 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -1,6 +1,6 @@ #pragma once -#include // for std::any_of +#include #include #include @@ -48,9 +48,9 @@ class Marking : public Coordinator - marking_list_{}; // Initialize it to invalid robot id in constructor + marking_list_{}; std::array - danger_score_{}; // infinity initialized in constructor, no one is a valid target initially + danger_score_{}; std::array enemey_to_friends_{}; std::vector queue_; WorldState last_world_state_; diff --git a/src/rj_strategy/src/coordinator/kicker_picker_client.cpp b/src/rj_strategy/src/coordinator/kicker_picker_client.cpp index 772e08f1031..08820d0f904 100644 --- a/src/rj_strategy/src/coordinator/kicker_picker_client.cpp +++ b/src/rj_strategy/src/coordinator/kicker_picker_client.cpp @@ -14,7 +14,6 @@ KickerPickerClient::KickerPickerClient(rclcpp::Node::SharedPtr node, uint8_t rob } void KickerPickerClient::join_group(StatusCallback callback) { - SPDLOG_INFO("Hello, I'm trying to join"); if (am_i_member_) { return; } From d916b5a4b320acedfd5f88916c68c402da4bda4d Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 9 Nov 2025 21:02:58 -0500 Subject: [PATCH 34/44] Fix Code Style On marking-coordinator (#2452) automated style fixes Co-authored-by: petergarud --- src/rj_strategy/include/rj_strategy/coordinator/marking.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index e3f752901a0..e9e86a80d6f 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -47,10 +47,8 @@ class Marking : public Coordinator - marking_list_{}; - std::array - danger_score_{}; + std::array marking_list_{}; + std::array danger_score_{}; std::array enemey_to_friends_{}; std::vector queue_; WorldState last_world_state_; From 8de5768be496f4ed4ee7ef3e9112f020a097c8fd Mon Sep 17 00:00:00 2001 From: sdhanyamraju6 Date: Tue, 11 Nov 2025 19:57:33 -0500 Subject: [PATCH 35/44] fix client handles --- .../include/rj_strategy/agent/position.hpp | 8 ++++- .../rj_strategy/agent/position/defense.hpp | 5 +-- .../agent/position/free_kicker.hpp | 5 +-- .../agent/position/goal_kicker.hpp | 3 -- .../rj_strategy/agent/position/goalie.hpp | 6 +--- .../rj_strategy/agent/position/idle.hpp | 6 +--- .../rj_strategy/agent/position/line.hpp | 2 +- .../rj_strategy/agent/position/offense.hpp | 6 +--- .../agent/position/penalty_non_kicker.hpp | 6 +--- .../agent/position/penalty_player.hpp | 6 +--- .../rj_strategy/agent/position/pivot_test.hpp | 2 -- .../agent/position/robot_factory_position.hpp | 7 ++-- .../rj_strategy/agent/position/seeker.hpp | 4 --- .../agent/position/smartidling.hpp | 6 +--- .../agent/position/solo_offense.hpp | 6 +--- .../rj_strategy/agent/position/zoner.hpp | 6 +--- src/rj_strategy/src/agent/position.cpp | 8 +++-- .../src/agent/position/defense.cpp | 25 +++++---------- .../src/agent/position/free_kicker.cpp | 9 +----- .../src/agent/position/goal_kicker.cpp | 2 -- src/rj_strategy/src/agent/position/goalie.cpp | 8 +---- src/rj_strategy/src/agent/position/idle.cpp | 8 +---- src/rj_strategy/src/agent/position/line.cpp | 2 +- .../src/agent/position/offense.cpp | 10 +----- .../src/agent/position/penalty_non_kicker.cpp | 9 +----- .../src/agent/position/penalty_player.cpp | 8 +---- .../src/agent/position/pivot_test.cpp | 3 -- .../agent/position/robot_factory_position.cpp | 32 +++++++++++-------- src/rj_strategy/src/agent/position/seeker.cpp | 5 --- .../src/agent/position/smartidling.cpp | 8 +---- .../src/agent/position/solo_offense.cpp | 10 +----- src/rj_strategy/src/agent/position/zoner.cpp | 10 +----- 32 files changed, 62 insertions(+), 179 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position.hpp b/src/rj_strategy/include/rj_strategy/agent/position.hpp index 3d88301f419..044e4fc315b 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position.hpp @@ -69,7 +69,7 @@ class Position { public: Position(int r_id); virtual ~Position() = default; - Position(const Position& other) = default; + Position(Position&& other) = default; /** * @brief return a RobotIntent to be sent to PlannerNode by AC; nullopt @@ -238,6 +238,9 @@ class Position { */ virtual void set_goalie_id(int goalie_id); + // Allow external code (e.g., RobotFactoryPosition) to inject shared client handles + void set_client_handles(std::shared_ptr client_handles); + protected: Position(int r_id, std::string position_name); @@ -314,6 +317,9 @@ class Position { // protected to allow WorldState to be accessed directly by deriveed WorldState* last_world_state_; + // Client Handles + std::shared_ptr client_handles_; + // Current goalie int goalie_id_; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index d315c753ce9..722b98e9ea8 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -26,10 +26,8 @@ namespace strategy { class Defense : public Position { public: Defense(int r_id); - Defense(int r_id, std::shared_ptr clientHandles); - Defense(const Position& other, std::shared_ptr clientHandles); ~Defense() override = default; - Defense(const Position& other); + Defense(Position&& other); void receive_communication_response(communication::AgentPosResponseWrapper response) override; communication::PosAgentResponseWrapper receive_communication_request( @@ -125,7 +123,6 @@ class Defense : public Position { bool pending_marking_state_ = false; - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp index 1df127c5941..67ad761e4a3 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp @@ -18,9 +18,7 @@ namespace strategy { class FreeKicker : public Position { public: FreeKicker(int r_id); - FreeKicker(int r_id, std::shared_ptr clientHandles); - FreeKicker(const Position& other, std::shared_ptr clientHandles); - FreeKicker(const Position&); + FreeKicker(Position&&); ~FreeKicker() = default; /** @@ -40,7 +38,6 @@ class FreeKicker : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp index 3dd06109628..c1051c302dd 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp @@ -21,7 +21,6 @@ class GoalKicker : public Position { public: GoalKicker(int r_id); ~GoalKicker() = default; - GoalKicker(int r_id, std::shared_ptr clientHandles); /** * @brief Does nothing; this position is a special case @@ -40,8 +39,6 @@ class GoalKicker : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp b/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp index bede3596c84..f57596e2988 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp @@ -23,10 +23,8 @@ namespace strategy { class Goalie : public Position { public: Goalie(int r_id); - Goalie(int r_id, std::shared_ptr clientHandles); - Goalie(const Position& other, std::shared_ptr clientHandles); ~Goalie() override = default; - Goalie(const Position& other); + Goalie(Position&& other); void derived_acknowledge_pass() override; void derived_pass_ball() override; @@ -89,8 +87,6 @@ class Goalie : public Position { State latest_state_ = IDLING; rj_geometry::Point penalty_location(); - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp b/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp index f192ac0768f..652a97000de 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/idle.hpp @@ -9,9 +9,7 @@ class Idle : public Position { public: Idle(int r_id); ~Idle() = default; - Idle(const Position& other); - Idle(int r_id, std::shared_ptr clientHandles); - Idle(const Position& other, std::shared_ptr clientHandles); + Idle(Position&& other); /** * @brief Does nothing; this position is a special case @@ -30,7 +28,5 @@ class Idle : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/line.hpp b/src/rj_strategy/include/rj_strategy/agent/position/line.hpp index db73f1acfe7..660c94cacdb 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/line.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/line.hpp @@ -8,7 +8,7 @@ namespace strategy { class Line : public Position { public: - Line(const Position& other); + Line(Position&& other); Line(int r_id); Line(int r_id, bool forward); ~Line() override = default; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp index b724bbd70dc..99d3543b363 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/offense.hpp @@ -29,9 +29,7 @@ class Offense : public Position { public: Offense(int r_id); ~Offense() override = default; - Offense(const Position& other); - Offense(int r_id, std::shared_ptr clientHandles); - Offense(const Position& other, std::shared_ptr clientHandles); + Offense(Position&& other); communication::PosAgentResponseWrapper receive_communication_request( communication::AgentPosRequestWrapper request) override; @@ -233,8 +231,6 @@ class Offense : public Position { void broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding); std::unordered_map seeker_points_; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp index 8244e3132aa..84b0f42cf7e 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/penalty_non_kicker.hpp @@ -19,9 +19,7 @@ class PenaltyNonKicker : public Position { public: PenaltyNonKicker(int r_id); ~PenaltyNonKicker() = default; - PenaltyNonKicker(const Position& other); - PenaltyNonKicker(int r_id, std::shared_ptr clientHandles); - PenaltyNonKicker(const Position& other, std::shared_ptr clientHandles); + PenaltyNonKicker(Position&& other); /** * @brief Does nothing; this position is a special case @@ -53,7 +51,5 @@ class PenaltyNonKicker : public Position { std::optional state_to_task(RobotIntent intent); // State latest_state_ = GET_AWAY; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp b/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp index 023bb862d1f..a2521eac5ab 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/penalty_player.hpp @@ -22,9 +22,7 @@ class PenaltyPlayer : public Position { public: PenaltyPlayer(int r_id); ~PenaltyPlayer() = default; - PenaltyPlayer(const Position& other); - PenaltyPlayer(int r_id, std::shared_ptr clientHandles); - PenaltyPlayer(const Position& other, std::shared_ptr clientHandles); + PenaltyPlayer(Position&& other); /** * @brief Does nothing; this position is a special case @@ -90,8 +88,6 @@ class PenaltyPlayer : public Position { // current state of Goalie (state machine) State latest_state_ = START; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp b/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp index a2564f4e224..45532449788 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/pivot_test.hpp @@ -26,8 +26,6 @@ class Pivot : public Position { ~Pivot() override = default; Pivot(const Pivot& other) = default; Pivot(Pivot&& other) = default; - Pivot(int r_id, std::shared_ptr clientHandles); - Pivot(const Position& other, std::shared_ptr clientHandles); Pivot& operator=(const Pivot& other) = delete; Pivot& operator=(Pivot&& other) = delete; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp b/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp index 112c4c1a846..06cd2b17038 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/robot_factory_position.hpp @@ -144,17 +144,14 @@ class RobotFactoryPosition : public Position { template void set_current_position() { if (dynamic_cast(current_position_.get()) == nullptr) { - // This line requires Pos to implement the constructor Pos(const - // Position&) + // This line requires Pos to implement the constructor Pos(Position&&) current_position_->die(); - current_position_ = std::make_unique(*current_position_, clientHandles_); + current_position_ = std::make_unique(std::move(*current_position_)); SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name()); } } bool set_position_override_if_requested(); - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp index 7fc88b69cf7..67df9c227fd 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/seeker.hpp @@ -32,8 +32,6 @@ class Seeker : public RoleInterface { Seeker(Seeker&& other) = default; Seeker& operator=(const Seeker& other) = default; Seeker& operator=(Seeker&& other) = default; - Seeker(int r_id, std::shared_ptr clientHandles); - Seeker(const Position& other, std::shared_ptr clientHandles); /** * @brief Returns a seeker behavior which aims to get open @@ -115,8 +113,6 @@ class Seeker : public RoleInterface { const FieldDimensions& field_dimensions) const; std::unordered_map seeker_points_{}; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp b/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp index c25b988a974..85652cc2f7f 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/smartidling.hpp @@ -9,9 +9,7 @@ class SmartIdle : public Position { public: SmartIdle(int r_id); ~SmartIdle() = default; - SmartIdle(const Position& other); - SmartIdle(int r_id, std::shared_ptr clientHandles); - SmartIdle(const Position& other, std::shared_ptr clientHandles); + SmartIdle(Position&& other); /** * @brief Does nothing; this position is a special case @@ -30,7 +28,5 @@ class SmartIdle : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp index aab7ced26b2..5f565fd3b78 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp @@ -22,13 +22,11 @@ namespace strategy { class SoloOffense : public Position { public: - SoloOffense(const Position& other); + SoloOffense(Position&& other); SoloOffense(int r_id); ~SoloOffense() override = default; SoloOffense(const SoloOffense& other) = default; SoloOffense(SoloOffense&& other) = default; - SoloOffense(int r_id, std::shared_ptr clientHandles); - SoloOffense(const Position& other, std::shared_ptr clientHandles); std::string get_current_state() override; @@ -61,8 +59,6 @@ class SoloOffense : public Position { rj_geometry::Point calculate_best_shot() const; double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp b/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp index 6614b42559c..2d843605f79 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/zoner.hpp @@ -28,12 +28,10 @@ class Zoner : public Position { public: Zoner(int r_id); ~Zoner() override = default; - Zoner(const Position& other); + Zoner(Position&& other); Zoner(Zoner&& other) = default; Zoner& operator=(const Zoner& other) = default; Zoner& operator=(Zoner&& other) = default; - Zoner(int r_id, std::shared_ptr clientHandles); - Zoner(const Position& other, std::shared_ptr clientHandles); private: std::optional derived_get_task(RobotIntent intent) override; @@ -58,8 +56,6 @@ class Zoner : public Position { std::string get_current_state() override; static constexpr double kZonerRadius = 1.5; - - std::shared_ptr clientHandles_; }; } // namespace strategy diff --git a/src/rj_strategy/src/agent/position.cpp b/src/rj_strategy/src/agent/position.cpp index e058924a293..91fd9ef433c 100644 --- a/src/rj_strategy/src/agent/position.cpp +++ b/src/rj_strategy/src/agent/position.cpp @@ -2,10 +2,10 @@ namespace strategy { -Position::Position(int r_id) : robot_id_(r_id) {} +Position::Position(int r_id) : robot_id_(r_id), client_handles_(std::make_shared()) {} Position::Position(int r_id, std::string position_name) - : position_name_{std::move(position_name)}, robot_id_{r_id} {}; + : position_name_{std::move(position_name)}, robot_id_{r_id}, client_handles_(std::make_shared()) {}; std::optional Position::get_task(WorldState& world_state, FieldDimensions& field_dimensions, @@ -36,6 +36,10 @@ void Position::set_goal_canceled() { goal_canceled_ = true; } void Position::set_goalie_id(int goalie_id) { goalie_id_ = goalie_id; } +void Position::set_client_handles(std::shared_ptr client_handles) { + client_handles_ = std::move(client_handles); +} + bool Position::check_is_done() { if (is_done_) { is_done_ = false; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index ae4e08f4667..cecfd82923b 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -4,16 +4,7 @@ namespace strategy { Defense::Defense(int r_id) : Position(r_id, "Defense") {} -Defense::Defense(const Position& other) : Position{other} { - position_name_ = "Defense"; - walling_robots_ = {}; -} - -Defense::Defense(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "Defense"), clientHandles_{clientHandles} {} - -Defense::Defense(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} { +Defense::Defense(Position&& other) : Position{std::move(other)} { position_name_ = "Defense"; walling_robots_ = {}; } @@ -45,8 +36,8 @@ Defense::State Defense::update_state() { return IDLING; } - if (clientHandles_->markingClient->am_i_member() && - clientHandles_->markingClient->am_i_marking()) { + if (client_handles_->markingClient->am_i_member() && + client_handles_->markingClient->am_i_marking()) { return MARKING; } else { return IDLING; @@ -106,8 +97,8 @@ Defense::State Defense::update_state() { next_state = IDLING; } case MARKING: - if (!clientHandles_->markingClient->am_i_member() || - !clientHandles_->markingClient->am_i_marking()) { + if (!client_handles_->markingClient->am_i_member() || + !client_handles_->markingClient->am_i_marking()) { next_state = IDLING; } break; @@ -118,7 +109,7 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = true; request_time_ = RJ::now(); - clientHandles_->markingClient->join_group([this](const MarkingClient::Result& res) { + client_handles_->markingClient->join_group([this](const MarkingClient::Result& res) { if (res.am_i_member && res.am_i_marking) { pending_marking_state_ = true; } @@ -129,7 +120,7 @@ Defense::State Defense::update_state() { // reset flag sent_join_marking_group_request_ = false; // ensure not in coordinator group - clientHandles_->markingClient->leave_group(); + client_handles_->markingClient->leave_group(); SPDLOG_INFO("Robot {}: Timeout on join group, IDLING now", robot_id_); next_state = IDLING; } @@ -207,7 +198,7 @@ std::optional Defense::state_to_task(RobotIntent intent) { return intent; } else if (current_state_ == MARKING) { rj_geometry::Point targetPoint = - last_world_state_->get_robot(false, clientHandles_->markingClient->who_am_i_marking()) + last_world_state_->get_robot(false, client_handles_->markingClient->who_am_i_marking()) .pose.position(); rj_geometry::Point ballPoint = last_world_state_->ball.position; diff --git a/src/rj_strategy/src/agent/position/free_kicker.cpp b/src/rj_strategy/src/agent/position/free_kicker.cpp index 5e864c3bf86..5513ffc2e3d 100644 --- a/src/rj_strategy/src/agent/position/free_kicker.cpp +++ b/src/rj_strategy/src/agent/position/free_kicker.cpp @@ -4,14 +4,7 @@ namespace strategy { FreeKicker::FreeKicker(int r_id) : Position(r_id, "FreeKicker") {} -FreeKicker::FreeKicker(const Position& other) : Position{other} { position_name_ = "FreeKicker"; } -FreeKicker::FreeKicker(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "FreeKicker"), clientHandles_{clientHandles} {} - -FreeKicker::FreeKicker(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} { - position_name_ = "FreeKicker"; -} +FreeKicker::FreeKicker(Position&& other) : Position{std::move(other)} { position_name_ = "FreeKicker"; } std::optional FreeKicker::derived_get_task(RobotIntent intent) { // Penalty Kicker kicks the ball into the goal diff --git a/src/rj_strategy/src/agent/position/goal_kicker.cpp b/src/rj_strategy/src/agent/position/goal_kicker.cpp index fb621958db7..00f03ce9367 100644 --- a/src/rj_strategy/src/agent/position/goal_kicker.cpp +++ b/src/rj_strategy/src/agent/position/goal_kicker.cpp @@ -3,8 +3,6 @@ namespace strategy { GoalKicker::GoalKicker(int r_id) : Position(r_id, "GoalKicker") {} -GoalKicker::GoalKicker(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "GoalKicker"), clientHandles_{clientHandles} {} std::optional GoalKicker::derived_get_task(RobotIntent intent) { // Penalty Kicker kicks the ball into the goal diff --git a/src/rj_strategy/src/agent/position/goalie.cpp b/src/rj_strategy/src/agent/position/goalie.cpp index 119686e3b8b..8fc9cc9d4cb 100644 --- a/src/rj_strategy/src/agent/position/goalie.cpp +++ b/src/rj_strategy/src/agent/position/goalie.cpp @@ -4,13 +4,7 @@ namespace strategy { Goalie::Goalie(int r_id) : Position(r_id, "Goalie") {} -Goalie::Goalie(const Position& other) : Position{other} {} - -Goalie::Goalie(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "Goalie"), clientHandles_{clientHandles} {} - -Goalie::Goalie(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} {} +Goalie::Goalie(Position&& other) : Position{std::move(other)} {} std::optional Goalie::derived_get_task(RobotIntent intent) { latest_state_ = update_state(); diff --git a/src/rj_strategy/src/agent/position/idle.cpp b/src/rj_strategy/src/agent/position/idle.cpp index 4c4658f2abe..3ab92a63ca7 100644 --- a/src/rj_strategy/src/agent/position/idle.cpp +++ b/src/rj_strategy/src/agent/position/idle.cpp @@ -4,13 +4,7 @@ namespace strategy { Idle::Idle(int r_id) : Position{r_id, "Idle"} {} -Idle::Idle(const Position& other) : Position{other} {} - -Idle::Idle(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "Idle"), clientHandles_{clientHandles} {} - -Idle::Idle(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} {} +Idle::Idle(Position&& other) : Position{std::move(other)} {} std::string Idle::get_current_state() { return "Idle"; } diff --git a/src/rj_strategy/src/agent/position/line.cpp b/src/rj_strategy/src/agent/position/line.cpp index 40e63dcb3bf..0405441c6e7 100644 --- a/src/rj_strategy/src/agent/position/line.cpp +++ b/src/rj_strategy/src/agent/position/line.cpp @@ -2,7 +2,7 @@ namespace strategy { -Line::Line(const Position& other) : Position{other} { position_name_ = "Line"; } +Line::Line(Position&& other) : Position{std::move(other)} { position_name_ = "Line"; } Line::Line(int r_id) : Position{r_id, "Line"} {} diff --git a/src/rj_strategy/src/agent/position/offense.cpp b/src/rj_strategy/src/agent/position/offense.cpp index 1162bcfb5c1..0c5e722e948 100644 --- a/src/rj_strategy/src/agent/position/offense.cpp +++ b/src/rj_strategy/src/agent/position/offense.cpp @@ -4,15 +4,7 @@ namespace strategy { Offense::Offense(int r_id) : Position{r_id, "Offense"}, seeker_{r_id} {} -Offense::Offense(const Position& other) : Position{other}, seeker_{robot_id_} { - position_name_ = "Offense"; -} - -Offense::Offense(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "Offense"), seeker_{r_id}, clientHandles_{clientHandles} {} - -Offense::Offense(const Position& other, std::shared_ptr clientHandles) - : Position{other}, seeker_{robot_id_}, clientHandles_{clientHandles} { +Offense::Offense(Position&& other) : Position{std::move(other)}, seeker_{robot_id_} { position_name_ = "Offense"; } diff --git a/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp b/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp index e36b1a995db..c2e2d7713c0 100644 --- a/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp +++ b/src/rj_strategy/src/agent/position/penalty_non_kicker.cpp @@ -4,14 +4,7 @@ namespace strategy { PenaltyNonKicker::PenaltyNonKicker(int r_id) : Position{r_id, "PenaltyNonKicker"} {} -PenaltyNonKicker::PenaltyNonKicker(const Position& other) : Position{other} {} - -PenaltyNonKicker::PenaltyNonKicker(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "PenaltyNonKicker"), clientHandles_{clientHandles} {} - -PenaltyNonKicker::PenaltyNonKicker(const Position& other, - std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} {} +PenaltyNonKicker::PenaltyNonKicker(Position&& other) : Position{std::move(other)} {} std::string PenaltyNonKicker::get_current_state() { return "PenaltyNonKicker"; } diff --git a/src/rj_strategy/src/agent/position/penalty_player.cpp b/src/rj_strategy/src/agent/position/penalty_player.cpp index 33b3e5f4f93..7ac88bacbb9 100644 --- a/src/rj_strategy/src/agent/position/penalty_player.cpp +++ b/src/rj_strategy/src/agent/position/penalty_player.cpp @@ -5,13 +5,7 @@ namespace strategy { // ALSO USED AS KickoffKicker PenaltyPlayer::PenaltyPlayer(int r_id) : Position(r_id, "PenaltyPlayer") {} -PenaltyPlayer::PenaltyPlayer(const Position& other) : Position{other} {} - -PenaltyPlayer::PenaltyPlayer(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "PenaltyPlayer"), clientHandles_{clientHandles} {} - -PenaltyPlayer::PenaltyPlayer(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} {} +PenaltyPlayer::PenaltyPlayer(Position&& other) : Position{std::move(other)} {} std::optional PenaltyPlayer::derived_get_task(RobotIntent intent) { latest_state_ = update_state(); diff --git a/src/rj_strategy/src/agent/position/pivot_test.cpp b/src/rj_strategy/src/agent/position/pivot_test.cpp index bfa46e60b3d..76ea01f1d2b 100644 --- a/src/rj_strategy/src/agent/position/pivot_test.cpp +++ b/src/rj_strategy/src/agent/position/pivot_test.cpp @@ -3,9 +3,6 @@ namespace strategy { Pivot::Pivot(int r_id) : Position{r_id, "Pivot"} {} -Pivot::Pivot(int r_id, std::shared_ptr clientHandles) : Position{r_id, "Pivot"} {} -Pivot::Pivot(const Position& other, std::shared_ptr clientHandles) - : Position{other} {} std::optional Pivot::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock diff --git a/src/rj_strategy/src/agent/position/robot_factory_position.cpp b/src/rj_strategy/src/agent/position/robot_factory_position.cpp index bc6bda4c0a1..37589c5ca92 100644 --- a/src/rj_strategy/src/agent/position/robot_factory_position.cpp +++ b/src/rj_strategy/src/agent/position/robot_factory_position.cpp @@ -3,15 +3,19 @@ namespace strategy { RobotFactoryPosition::RobotFactoryPosition(int r_id, rclcpp::Node::SharedPtr node) - : Position(r_id, "RobotFactoryPosition"), clientHandles_(std::make_shared()) { - clientHandles_->kickerPickerClient = std::make_unique(node, r_id); - clientHandles_->markingClient = std::make_unique(node, r_id); + : Position(r_id, "RobotFactoryPosition") { + client_handles_->kickerPickerClient = std::make_unique(node, r_id); + client_handles_->markingClient = std::make_unique(node, r_id); if (robot_id_ == 0) { - current_position_ = std::make_unique(robot_id_, clientHandles_); + current_position_ = std::make_unique(robot_id_); } else if (robot_id_ == 1 || robot_id_ == 2) { - current_position_ = std::make_unique(robot_id_, clientHandles_); + current_position_ = std::make_unique(robot_id_); } else { - current_position_ = std::make_unique(robot_id_, clientHandles_); + current_position_ = std::make_unique(robot_id_); + } + // Make sure the child position uses the same client handles instance + if (current_position_) { + current_position_->set_client_handles(client_handles_); } } @@ -41,7 +45,7 @@ void RobotFactoryPosition::process_play_state() { case PlayState::State::Playing: { // We just became regular playing. // set_default_position(); - clientHandles_->kickerPickerClient->leave_group(); + client_handles_->kickerPickerClient->leave_group(); break; } @@ -71,7 +75,7 @@ void RobotFactoryPosition::process_play_state() { case PlayState::State::Halt: { // The game has been stopped or halted. In this case, we typically want to keep // our current position. The rules for movement should be handled at a lower level. - clientHandles_->kickerPickerClient->leave_group(); + client_handles_->kickerPickerClient->leave_group(); handle_stop(); break; } @@ -83,8 +87,8 @@ void RobotFactoryPosition::process_play_state() { void RobotFactoryPosition::handle_stop() { set_default_position(); } void RobotFactoryPosition::handle_penalty_playing() { - if (!(clientHandles_->kickerPickerClient->am_i_member() && - clientHandles_->kickerPickerClient->is_selected())) { + if (!(client_handles_->kickerPickerClient->am_i_member() && + client_handles_->kickerPickerClient->is_selected())) { set_current_position(); } } @@ -95,8 +99,8 @@ void RobotFactoryPosition::handle_setup() { // Set up our restart if ((current_play_state_.is_kickoff() || current_play_state_.is_penalty()) && - !clientHandles_->kickerPickerClient->am_i_member()) { - clientHandles_->kickerPickerClient->join_group([this]( + !client_handles_->kickerPickerClient->am_i_member()) { + client_handles_->kickerPickerClient->join_group([this]( KickerPickerClient::Result result) { if (result.am_i_member && result.kicker_id == robot_id_ && current_play_state_.is_kickoff()) { @@ -121,9 +125,9 @@ void RobotFactoryPosition::handle_ready() { // Time to kick if (current_play_state_.is_our_restart() && current_play_state_.is_free_kick() && - !clientHandles_->kickerPickerClient->am_i_member()) { + !client_handles_->kickerPickerClient->am_i_member()) { // There is no "Setup" stage for free kicks, so this is when we choose kicker - clientHandles_->kickerPickerClient->join_group([this](KickerPickerClient::Result result) { + client_handles_->kickerPickerClient->join_group([this](KickerPickerClient::Result result) { if (result.am_i_member && result.kicker_id == robot_id_) { set_current_position(); } else { diff --git a/src/rj_strategy/src/agent/position/seeker.cpp b/src/rj_strategy/src/agent/position/seeker.cpp index 51ccb464654..64d748b02e1 100644 --- a/src/rj_strategy/src/agent/position/seeker.cpp +++ b/src/rj_strategy/src/agent/position/seeker.cpp @@ -3,11 +3,6 @@ namespace strategy { Seeker::Seeker(int robot_id) { robot_id_ = robot_id; } -Seeker::Seeker(int r_id, std::shared_ptr clientHandles) - : clientHandles_{clientHandles} { - robot_id_ = r_id; -} -Seeker::Seeker(const Position& other, std::shared_ptr clientHandles) {} std::optional Seeker::get_task(RobotIntent intent, const WorldState* last_world_state, FieldDimensions field_dimensions) { diff --git a/src/rj_strategy/src/agent/position/smartidling.cpp b/src/rj_strategy/src/agent/position/smartidling.cpp index 3c7b2a82ceb..830a908d78b 100644 --- a/src/rj_strategy/src/agent/position/smartidling.cpp +++ b/src/rj_strategy/src/agent/position/smartidling.cpp @@ -4,13 +4,7 @@ namespace strategy { SmartIdle::SmartIdle(int r_id) : Position{r_id, "SmartIdle"} {} -SmartIdle::SmartIdle(const Position& other) : Position{other} {} - -SmartIdle::SmartIdle(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "SmartIdle"), clientHandles_{clientHandles} {} - -SmartIdle::SmartIdle(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} {} +SmartIdle::SmartIdle(Position&& other) : Position{std::move(other)} {} std::string SmartIdle::get_current_state() { return "SmartIdle"; } diff --git a/src/rj_strategy/src/agent/position/solo_offense.cpp b/src/rj_strategy/src/agent/position/solo_offense.cpp index a2ed0e10fe6..71c793d8f6a 100644 --- a/src/rj_strategy/src/agent/position/solo_offense.cpp +++ b/src/rj_strategy/src/agent/position/solo_offense.cpp @@ -2,20 +2,12 @@ namespace strategy { -SoloOffense::SoloOffense(const Position& other) : Position{other} { +SoloOffense::SoloOffense(Position&& other) : Position{std::move(other)} { position_name_ = "SoloOffense"; } SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} -SoloOffense::SoloOffense(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "SoloOffense"), clientHandles_{clientHandles} {} - -SoloOffense::SoloOffense(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} { - position_name_ = "SoloOffense"; -} - std::optional SoloOffense::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock State new_state = next_state(); diff --git a/src/rj_strategy/src/agent/position/zoner.cpp b/src/rj_strategy/src/agent/position/zoner.cpp index e5d3ccd8443..b0404cbeeac 100644 --- a/src/rj_strategy/src/agent/position/zoner.cpp +++ b/src/rj_strategy/src/agent/position/zoner.cpp @@ -4,15 +4,7 @@ namespace strategy { Zoner::Zoner(int r_id) : Position(r_id, "Zoner") {} -Zoner::Zoner(const Position& other) : Position{other} { position_name_ = "Zoner"; } - -Zoner::Zoner(int r_id, std::shared_ptr clientHandles) - : Position(r_id, "Zoner"), clientHandles_{clientHandles} {} - -Zoner::Zoner(const Position& other, std::shared_ptr clientHandles) - : Position{other}, clientHandles_{clientHandles} { - position_name_ = "Zoner"; -} +Zoner::Zoner(Position&& other) : Position{std::move(other)} { position_name_ = "Zoner"; } std::optional Zoner::derived_get_task(RobotIntent intent) { current_state_ = next_state(); From 0ce1e250f089727bad79d82abd1207594892ccaf Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 11 Nov 2025 20:06:01 -0500 Subject: [PATCH 36/44] Fix Code Style On marking-coordinator (#2454) automated style fixes Co-authored-by: sanatd33 --- .../include/rj_strategy/agent/position/defense.hpp | 1 - src/rj_strategy/src/agent/position.cpp | 7 +++++-- src/rj_strategy/src/agent/position/defense.cpp | 11 ++++++----- src/rj_strategy/src/agent/position/free_kicker.cpp | 4 +++- .../src/agent/position/robot_factory_position.cpp | 2 +- 5 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 722b98e9ea8..f82b6b11ca3 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -122,7 +122,6 @@ class Defense : public Position { RJ::Seconds kMarkingGroupJoinTimeout{2.0}; bool pending_marking_state_ = false; - }; } // namespace strategy diff --git a/src/rj_strategy/src/agent/position.cpp b/src/rj_strategy/src/agent/position.cpp index 91fd9ef433c..750b4a03c97 100644 --- a/src/rj_strategy/src/agent/position.cpp +++ b/src/rj_strategy/src/agent/position.cpp @@ -2,10 +2,13 @@ namespace strategy { -Position::Position(int r_id) : robot_id_(r_id), client_handles_(std::make_shared()) {} +Position::Position(int r_id) + : robot_id_(r_id), client_handles_(std::make_shared()) {} Position::Position(int r_id, std::string position_name) - : position_name_{std::move(position_name)}, robot_id_{r_id}, client_handles_(std::make_shared()) {}; + : position_name_{std::move(position_name)}, + robot_id_{r_id}, + client_handles_(std::make_shared()){}; std::optional Position::get_task(WorldState& world_state, FieldDimensions& field_dimensions, diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index cecfd82923b..335e1f47384 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -109,11 +109,12 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = true; request_time_ = RJ::now(); - client_handles_->markingClient->join_group([this](const MarkingClient::Result& res) { - if (res.am_i_member && res.am_i_marking) { - pending_marking_state_ = true; - } - }); + client_handles_->markingClient->join_group( + [this](const MarkingClient::Result& res) { + if (res.am_i_member && res.am_i_marking) { + pending_marking_state_ = true; + } + }); } auto elapsed = RJ::now() - request_time_; if (elapsed > kMarkingGroupJoinTimeout) { diff --git a/src/rj_strategy/src/agent/position/free_kicker.cpp b/src/rj_strategy/src/agent/position/free_kicker.cpp index 5513ffc2e3d..d2d70012774 100644 --- a/src/rj_strategy/src/agent/position/free_kicker.cpp +++ b/src/rj_strategy/src/agent/position/free_kicker.cpp @@ -4,7 +4,9 @@ namespace strategy { FreeKicker::FreeKicker(int r_id) : Position(r_id, "FreeKicker") {} -FreeKicker::FreeKicker(Position&& other) : Position{std::move(other)} { position_name_ = "FreeKicker"; } +FreeKicker::FreeKicker(Position&& other) : Position{std::move(other)} { + position_name_ = "FreeKicker"; +} std::optional FreeKicker::derived_get_task(RobotIntent intent) { // Penalty Kicker kicks the ball into the goal diff --git a/src/rj_strategy/src/agent/position/robot_factory_position.cpp b/src/rj_strategy/src/agent/position/robot_factory_position.cpp index 37589c5ca92..6a0a772c730 100644 --- a/src/rj_strategy/src/agent/position/robot_factory_position.cpp +++ b/src/rj_strategy/src/agent/position/robot_factory_position.cpp @@ -101,7 +101,7 @@ void RobotFactoryPosition::handle_setup() { if ((current_play_state_.is_kickoff() || current_play_state_.is_penalty()) && !client_handles_->kickerPickerClient->am_i_member()) { client_handles_->kickerPickerClient->join_group([this]( - KickerPickerClient::Result result) { + KickerPickerClient::Result result) { if (result.am_i_member && result.kicker_id == robot_id_ && current_play_state_.is_kickoff()) { set_current_position(); From e79fb8ecc654879db148e6b9ee51e43146c1c56f Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 11 Nov 2025 21:56:49 -0500 Subject: [PATCH 37/44] addressed most comments but still missing some callback stuff (check lines 50-58) --- .../rj_strategy/agent/position/defense.hpp | 4 +- .../rj_strategy/coordinator/marking.hpp | 8 +- .../coordinator/marking_client.hpp | 3 +- .../src/agent/position/defense.cpp | 11 +- src/rj_strategy/src/coordinator/marking.cpp | 121 +++++++++--------- .../src/coordinator/marking_client.cpp | 3 +- 6 files changed, 77 insertions(+), 73 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index f82b6b11ca3..372959586bc 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -42,8 +42,8 @@ class Defense : public Position { void revive() override; private: - // static constexpr int kMaxWallers{6}; static constexpr int kMaxWallers{2}; + static constexpr RJ::Seconds kMarkingGroupJoinTimeout{2.0}; /** * @brief The derived_get_task method returns the task for the defensive robot @@ -119,7 +119,7 @@ class Defense : public Position { bool sent_join_marking_group_request_ = false; RJ::Time request_time_; - RJ::Seconds kMarkingGroupJoinTimeout{2.0}; + bool pending_marking_state_ = false; }; diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index e9e86a80d6f..b4c7e60a3c0 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -36,21 +36,23 @@ class Marking : public Coordinator marking_list_{}; std::array danger_score_{}; - std::array enemey_to_friends_{}; - std::vector queue_; + std::array enemy_to_friends_{}; + std::vector unassigned_markers_queue_; WorldState last_world_state_; FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; rclcpp::Subscription::SharedPtr world_state_sub_; diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp index d9b745fe6e8..db26bc25dfe 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp @@ -22,8 +22,7 @@ class MarkingClient { static constexpr uint8_t kInvalidRobotId = kNumShells; struct Result { bool am_i_member{false}; // Whether this robot is currently a member of the kicker group. - std::optional am_i_marking{false}; - std::optional who_i_am_marking{kInvalidRobotId}; // ID of Kicker id + std::optional who_am_i_marking; // ID of Kicker id }; using StatusCallback = std::function; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 335e1f47384..bff21e3c0b1 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -111,7 +111,16 @@ Defense::State Defense::update_state() { client_handles_->markingClient->join_group( [this](const MarkingClient::Result& res) { - if (res.am_i_member && res.am_i_marking) { + if (res.am_i_member) { + // if (res.who_am_i_marking.has_value()) { + // SPDLOG_INFO("Robot {}: Joined marking group and assigned to mark " + // "robot {}", + // robot_id_, res.who_am_i_marking.value()); + // } else { + // SPDLOG_INFO("Robot {}: Joined marking group but not assigned to " + // "mark any robot", + // robot_id_); + // } pending_marking_state_ = true; } }); diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 33501a0ab15..3b51b820e76 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -5,7 +5,7 @@ namespace strategy { Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state marking_list_.fill(kInvalidRobotId); // initializes to no valid markers - enemey_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them + enemy_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them danger_score_.fill( std::numeric_limits::infinity()); // everyone starts with an infinite danger score num_markers_ = 0; @@ -21,35 +21,35 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { if (request->join == false) { bool marking = (marking_list_[request->robot_id] != kInvalidRobotId); if (marking) { - uint8_t enemey_id = marking_list_[request->robot_id]; + uint8_t enemy_id = marking_list_[request->robot_id]; marking_list_[request->robot_id] = kInvalidRobotId; - enemey_to_friends_[enemey_id] = kInvalidRobotId; + enemy_to_friends_[enemy_id] = kInvalidRobotId; num_markers_--; // The robot is leaving so replace the robot its marking if possible - const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); + const auto& enemy_robot = last_world_state_.get_robot(false, enemy_id); double min = std::numeric_limits::infinity(); uint8_t waiting_robot_id = kInvalidRobotId; // Find closest robot in queue for replacement // Queue is queue of robots that want to join marking but aren't good enough // Not close enough to mark or we exceeed the max num of markers - for (size_t i = 0; i < queue_.size(); ++i) { - const auto& i_robot = last_world_state_.get_robot(true, queue_[i]); - double distance = i_robot.pose.position().dist_to(enemey_robot.pose.position()); + for (size_t i = 0; i < unassigned_markers_queue_.size(); ++i) { + const auto& i_robot = last_world_state_.get_robot(true, unassigned_markers_queue_[i]); + double distance = i_robot.pose.position().dist_to(enemy_robot.pose.position()); if (distance < min) { min = distance; - waiting_robot_id = queue_[i]; + waiting_robot_id = unassigned_markers_queue_[i]; } } if (kInvalidRobotId != waiting_robot_id) { - queue_.erase(std::remove(queue_.begin(), queue_.end(), waiting_robot_id), - queue_.end()); - marking_list_[waiting_robot_id] = enemey_id; - enemey_to_friends_[enemey_id] = waiting_robot_id; + unassigned_markers_queue_.erase(std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), waiting_robot_id), + unassigned_markers_queue_.end()); + marking_list_[waiting_robot_id] = enemy_id; + enemy_to_friends_[enemy_id] = waiting_robot_id; num_markers_++; } } else { - queue_.erase(std::remove(queue_.begin(), queue_.end(), request->robot_id), - queue_.end()); + unassigned_markers_queue_.erase(std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), request->robot_id), + unassigned_markers_queue_.end()); } response->success = true; return; @@ -58,25 +58,14 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { // this means we can add this prospective marker as a marker uint8_t robotInPossession = find_their_robot_in_possession(); - uint8_t most_dangerous = kInvalidRobotId; - double min = std::numeric_limits::infinity(); - for (size_t i = 0; i < danger_score_.size(); ++i) { - // don't include marked robots or the guy with the ball in most dangerous calculation - if (enemey_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { - continue; - } - if (danger_score_[i] < min) { - most_dangerous = i; - min = danger_score_[i]; - } - } + uint8_t most_dangerous = most_dangerous_robot(robotInPossession); if (most_dangerous != kInvalidRobotId) { // Assign most dangerous unmarked robot without ball - enemey_to_friends_[most_dangerous] = request->robot_id; + enemy_to_friends_[most_dangerous] = request->robot_id; marking_list_[request->robot_id] = most_dangerous; num_markers_++; } else { - queue_.push_back(request->robot_id); + unassigned_markers_queue_.push_back(request->robot_id); } } else { // should we kick someone out (is this new robot a better marker) @@ -86,12 +75,12 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { // Kicked out robot is one that is furthest from its marker and new robot is closer than it for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] != kInvalidRobotId) { - uint8_t enemey_id = marking_list_[i]; + uint8_t enemy_id = marking_list_[i]; const auto& i_robot = last_world_state_.get_robot(true, i); - const auto& enemey_robot = last_world_state_.get_robot(false, enemey_id); + const auto& enemy_robot = last_world_state_.get_robot(false, enemy_id); double dist = - (i_robot.pose.position().dist_to(enemey_robot.pose.position())) - - (robot_requesting.pose.position().dist_to(enemey_robot.pose.position())); + (i_robot.pose.position().dist_to(enemy_robot.pose.position())) - + (robot_requesting.pose.position().dist_to(enemy_robot.pose.position())); if (dist > better_distance) { better_distance = dist; kick_out_this_robot_id = i; @@ -99,12 +88,12 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { } } if (kick_out_this_robot_id != kInvalidRobotId) { - uint8_t enemey_id = marking_list_[kick_out_this_robot_id]; + uint8_t enemy_id = marking_list_[kick_out_this_robot_id]; marking_list_[kick_out_this_robot_id] = kInvalidRobotId; - enemey_to_friends_[enemey_id] = request->robot_id; - marking_list_[request->robot_id] = enemey_id; + enemy_to_friends_[enemy_id] = request->robot_id; + marking_list_[request->robot_id] = enemy_id; } else { - queue_.push_back(request->robot_id); + unassigned_markers_queue_.push_back(request->robot_id); } } @@ -119,27 +108,16 @@ void Marking::publish_marking_list() { uint8_t robotInPossession = find_their_robot_in_possession(); // finding most dangerous of non-marked robots - uint8_t most_dangerous = kInvalidRobotId; - double min = std::numeric_limits::infinity(); - for (size_t i = 0; i < danger_score_.size(); ++i) { - // don't include marked robots or the guy with the ball in most dangerous calculation - if (enemey_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { - continue; - } - if (danger_score_[i] < min) { - most_dangerous = i; - min = danger_score_[i]; - } - } + uint8_t most_dangerous = most_dangerous_robot(robotInPossession); bool assigned = false; // check if anyone being marked has the ball // if so, remove them from being marked and assign them the most dangerous robot for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] == robotInPossession && robotInPossession != kInvalidRobotId) { - enemey_to_friends_[robotInPossession] = kInvalidRobotId; + enemy_to_friends_[robotInPossession] = kInvalidRobotId; marking_list_[i] = most_dangerous; - enemey_to_friends_[most_dangerous] = i; + enemy_to_friends_[most_dangerous] = i; assigned = true; } } @@ -150,11 +128,11 @@ void Marking::publish_marking_list() { double max_danger_sub = 0.0; for (size_t i = 0; i < marking_list_.size(); ++i) { if (marking_list_[i] != kInvalidRobotId) { - uint8_t enemey_id = marking_list_[i]; - double danger_sub = danger_score_[enemey_id] - danger_score_[most_dangerous]; + uint8_t enemy_id = marking_list_[i]; + double danger_sub = danger_score_[enemy_id] - danger_score_[most_dangerous]; if (danger_sub > max_danger_sub) { max_danger_sub = danger_sub; - not_dangerous_robot_id = enemey_id; + not_dangerous_robot_id = enemy_id; } } } @@ -163,10 +141,10 @@ void Marking::publish_marking_list() { // This only gets rid of the most dangerous non-marked robot // , subsequent runs will pick up next most dangerous if (max_danger_sub > kSuperDangerSub && not_dangerous_robot_id != kInvalidRobotId) { - uint8_t friend_id = enemey_to_friends_[not_dangerous_robot_id]; - enemey_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; + uint8_t friend_id = enemy_to_friends_[not_dangerous_robot_id]; + enemy_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; marking_list_[friend_id] = most_dangerous; - enemey_to_friends_[most_dangerous] = friend_id; + enemy_to_friends_[most_dangerous] = friend_id; } } @@ -205,14 +183,14 @@ void Marking::update_danger_scores() { double angle_between = 0.0; // Default to 0 (not dangerous) // Check if beyond midfield - bool onOurSide = false; - if (goal_loc.y() < field_center.y()) { - onOurSide = robot.pose.position().y() < field_center.y(); - } else { - onOurSide = robot.pose.position().y() > field_center.y(); - } + // bool onOurSide = false; + // if (goal_loc.y() < field_center.y()) { + // onOurSide = robot.pose.position().y() < field_center.y(); + // } else { + // onOurSide = robot.pose.position().y() > field_center.y(); + // } - if (onOurSide) { + if (field_dimensions_.our_half().hit(robot.pose.position())) { const auto& vec_goal_to_center = field_center - goal_loc; const auto& vec_goal_to_robot = robot.pose.position() - goal_loc; @@ -253,7 +231,7 @@ double Marking::find_their_robot_in_possession() { continue; } double dist_to_ball = ball_pos.dist_to(robot.pose.position()); - if (dist_to_ball < min_dist_to_ball && dist_to_ball < 0.3) { + if (dist_to_ball < min_dist_to_ball && dist_to_ball < kPossessionThreshold) { robotInPossession = i; min_dist_to_ball = dist_to_ball; } @@ -262,6 +240,21 @@ double Marking::find_their_robot_in_possession() { return robotInPossession; } +uint8_t Marking::most_dangerous_robot(uint8_t robotInPossession) { + uint8_t most_dangerous = kInvalidRobotId; + double min = std::numeric_limits::infinity(); + for (size_t i = 0; i < danger_score_.size(); ++i) { + // don't include marked robots or the guy with the ball in most dangerous calculation + if (enemy_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { + continue; + } + if (danger_score_[i] < min) { + most_dangerous = i; + min = danger_score_[i]; + } + } + return most_dangerous; +} } // namespace strategy int main(int argc, char* argv[]) { diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp index 5dbef77fac6..24936ba58cc 100644 --- a/src/rj_strategy/src/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -49,9 +49,10 @@ void MarkingClient::join_group(StatusCallback callback) { // Create subscription to track selected kicker. subscription_ = node_->create_subscription( "marking_data", rclcpp::QoS(1).transient_local(), - [this](const rj_msgs::msg::Marking::SharedPtr msg) { + [this](const rj_msgs::msg::Marking::SharedPtr msg) { // callback=std::move(callback) selected_robot_marking_id_ = msg->mark_robot_ids[robot_id_]; am_i_marking_ = (selected_robot_marking_id_ != kInvalidRobotId); + // callback(Result{true, selected_robot_marking_id_}); }); callback(Result{true}); From 1fc48317883bd2b00800da5248ad5af0cc29cd3c Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 16 Nov 2025 08:47:25 -0500 Subject: [PATCH 38/44] Fix Code Style On marking-coordinator (#2457) automated style fixes Co-authored-by: shourikb --- .../rj_strategy/agent/position/defense.hpp | 1 - src/rj_strategy/src/agent/position/defense.cpp | 3 ++- src/rj_strategy/src/coordinator/marking.cpp | 17 +++++++++++------ .../src/coordinator/marking_client.cpp | 3 ++- 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 372959586bc..e3989e646a4 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -119,7 +119,6 @@ class Defense : public Position { bool sent_join_marking_group_request_ = false; RJ::Time request_time_; - bool pending_marking_state_ = false; }; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index bff21e3c0b1..214e4dff077 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -113,7 +113,8 @@ Defense::State Defense::update_state() { [this](const MarkingClient::Result& res) { if (res.am_i_member) { // if (res.who_am_i_marking.has_value()) { - // SPDLOG_INFO("Robot {}: Joined marking group and assigned to mark " + // SPDLOG_INFO("Robot {}: Joined marking group and assigned to mark + // " // "robot {}", // robot_id_, res.who_am_i_marking.value()); // } else { diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 3b51b820e76..9c03e04121a 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -4,7 +4,7 @@ namespace strategy { Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { // Subscribe to world state - marking_list_.fill(kInvalidRobotId); // initializes to no valid markers + marking_list_.fill(kInvalidRobotId); // initializes to no valid markers enemy_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them danger_score_.fill( std::numeric_limits::infinity()); // everyone starts with an infinite danger score @@ -33,7 +33,8 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { // Queue is queue of robots that want to join marking but aren't good enough // Not close enough to mark or we exceeed the max num of markers for (size_t i = 0; i < unassigned_markers_queue_.size(); ++i) { - const auto& i_robot = last_world_state_.get_robot(true, unassigned_markers_queue_[i]); + const auto& i_robot = + last_world_state_.get_robot(true, unassigned_markers_queue_[i]); double distance = i_robot.pose.position().dist_to(enemy_robot.pose.position()); if (distance < min) { min = distance; @@ -41,15 +42,19 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { } } if (kInvalidRobotId != waiting_robot_id) { - unassigned_markers_queue_.erase(std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), waiting_robot_id), - unassigned_markers_queue_.end()); + unassigned_markers_queue_.erase( + std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), + waiting_robot_id), + unassigned_markers_queue_.end()); marking_list_[waiting_robot_id] = enemy_id; enemy_to_friends_[enemy_id] = waiting_robot_id; num_markers_++; } } else { - unassigned_markers_queue_.erase(std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), request->robot_id), - unassigned_markers_queue_.end()); + unassigned_markers_queue_.erase( + std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), + request->robot_id), + unassigned_markers_queue_.end()); } response->success = true; return; diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp index 24936ba58cc..8f4b38ecb41 100644 --- a/src/rj_strategy/src/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -49,7 +49,8 @@ void MarkingClient::join_group(StatusCallback callback) { // Create subscription to track selected kicker. subscription_ = node_->create_subscription( "marking_data", rclcpp::QoS(1).transient_local(), - [this](const rj_msgs::msg::Marking::SharedPtr msg) { // callback=std::move(callback) + [this]( + const rj_msgs::msg::Marking::SharedPtr msg) { // callback=std::move(callback) selected_robot_marking_id_ = msg->mark_robot_ids[robot_id_]; am_i_marking_ = (selected_robot_marking_id_ != kInvalidRobotId); // callback(Result{true, selected_robot_marking_id_}); From 22d0b1bb6bfb85b3692db805183577d01a1002df Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 18 Nov 2025 21:20:08 -0500 Subject: [PATCH 39/44] fixed some more comments --- .../include/rj_strategy/coordinator/marking.hpp | 2 +- src/rj_strategy/src/agent/position/defense.cpp | 1 + src/rj_strategy/src/coordinator/marking.cpp | 7 +++---- src/rj_strategy/src/coordinator/marking_client.cpp | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index b4c7e60a3c0..c327a7fb013 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -35,7 +35,7 @@ class Marking : public CoordinatormarkingClient->am_i_member() || !client_handles_->markingClient->am_i_marking()) { diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 3b51b820e76..b7fa0873ef2 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -2,13 +2,12 @@ namespace strategy { -Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node") { +Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node"), num_markers_{0} { // Subscribe to world state marking_list_.fill(kInvalidRobotId); // initializes to no valid markers enemy_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them danger_score_.fill( std::numeric_limits::infinity()); // everyone starts with an infinite danger score - num_markers_ = 0; world_state_sub_ = this->create_subscription( vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1), [this](rj_msgs::msg::WorldState::SharedPtr world_state) { // NOLINT @@ -171,7 +170,7 @@ void Marking::update_danger_scores() { double min = std::numeric_limits::infinity(); for (uint8_t j = 0; j < kNumShells; j++) { const auto& i_friend = last_world_state_.get_robot(true, j); - if (!robot.visible) { + if (!i_friend.visible) { continue; } double dist = robot.pose.position().dist_to(i_friend.pose.position()); @@ -221,7 +220,7 @@ void Marking::update_danger_scores() { // } } -double Marking::find_their_robot_in_possession() { +uint8_t Marking::find_their_robot_in_possession() { uint8_t robotInPossession = kInvalidRobotId; double min_dist_to_ball = std::numeric_limits::infinity(); const auto& ball_pos = last_world_state_.ball.position; diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp index 24936ba58cc..0f89e2e23ac 100644 --- a/src/rj_strategy/src/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -3,7 +3,7 @@ namespace strategy { /** - * @brief Client for interacting with the KickerPicker coordinator. + * @brief Client for interacting with the Marking coordinator. * * Manages membership in the kicker group and tracks the currently selected kicker. */ @@ -95,7 +95,7 @@ void MarkingClient::leave_group(StatusCallback callback) { selected_robot_marking_id_ = kInvalidRobotId; if (callback) { - callback(Result{false}); + callback(Result{true, selected_robot_marking_id_}); } }); } From 7b1c4683ce3afac306d5d6aea0257dc30a4c8a0f Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 18 Nov 2025 21:43:55 -0500 Subject: [PATCH 40/44] More fixes --- .../include/rj_strategy/coordinator/marking.hpp | 2 +- src/rj_strategy/src/coordinator/marking.cpp | 16 +++++----------- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp index c327a7fb013..0350ef0f3b8 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -52,7 +52,7 @@ class Marking : public Coordinator marking_list_{}; std::array danger_score_{}; std::array enemy_to_friends_{}; - std::vector unassigned_markers_queue_; + std::unordered_set unassigned_markers_queue_; WorldState last_world_state_; FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; rclcpp::Subscription::SharedPtr world_state_sub_; diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 2d73a541c77..5edeea92aed 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -31,29 +31,23 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { // Find closest robot in queue for replacement // Queue is queue of robots that want to join marking but aren't good enough // Not close enough to mark or we exceeed the max num of markers - for (size_t i = 0; i < unassigned_markers_queue_.size(); ++i) { + for (uint8_t id : unassigned_markers_queue_) { const auto& i_robot = - last_world_state_.get_robot(true, unassigned_markers_queue_[i]); + last_world_state_.get_robot(true, id); double distance = i_robot.pose.position().dist_to(enemy_robot.pose.position()); if (distance < min) { min = distance; - waiting_robot_id = unassigned_markers_queue_[i]; + waiting_robot_id = id; } } if (kInvalidRobotId != waiting_robot_id) { - unassigned_markers_queue_.erase( - std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), - waiting_robot_id), - unassigned_markers_queue_.end()); + unassigned_markers_.erase(waiting_robot_id); marking_list_[waiting_robot_id] = enemy_id; enemy_to_friends_[enemy_id] = waiting_robot_id; num_markers_++; } } else { - unassigned_markers_queue_.erase( - std::remove(unassigned_markers_queue_.begin(), unassigned_markers_queue_.end(), - request->robot_id), - unassigned_markers_queue_.end()); + unassigned_markers_.erase(request->robot_id); } response->success = true; return; From 498f016635541f1bc76b465a2de934451aabf20e Mon Sep 17 00:00:00 2001 From: shourikb Date: Sun, 23 Nov 2025 20:26:34 -0500 Subject: [PATCH 41/44] addressed all comments --- install/setup.bash | 5 ----- install/setup.zsh | 5 ----- .../rj_strategy/coordinator/marking_client.hpp | 6 +----- src/rj_strategy/src/agent/position/defense.cpp | 14 ++------------ src/rj_strategy/src/coordinator/marking.cpp | 11 ++++------- .../src/coordinator/marking_client.cpp | 15 +++++++-------- 6 files changed, 14 insertions(+), 42 deletions(-) diff --git a/install/setup.bash b/install/setup.bash index 10ea0f7c07f..4e927c619c5 100644 --- a/install/setup.bash +++ b/install/setup.bash @@ -17,11 +17,6 @@ _colcon_prefix_chain_bash_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - # source this prefix # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" diff --git a/install/setup.zsh b/install/setup.zsh index 54799fde6f8..2901efe685b 100644 --- a/install/setup.zsh +++ b/install/setup.zsh @@ -17,11 +17,6 @@ _colcon_prefix_chain_zsh_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - # source this prefix # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp index db26bc25dfe..293d2296dc3 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp @@ -20,12 +20,8 @@ namespace strategy { class MarkingClient { public: static constexpr uint8_t kInvalidRobotId = kNumShells; - struct Result { - bool am_i_member{false}; // Whether this robot is currently a member of the kicker group. - std::optional who_am_i_marking; // ID of Kicker id - }; - using StatusCallback = std::function; + using StatusCallback = std::function; explicit MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id); ~MarkingClient() = default; diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 869bf8f4a92..1c0744b955c 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -111,18 +111,8 @@ Defense::State Defense::update_state() { request_time_ = RJ::now(); client_handles_->markingClient->join_group( - [this](const MarkingClient::Result& res) { - if (res.am_i_member) { - // if (res.who_am_i_marking.has_value()) { - // SPDLOG_INFO("Robot {}: Joined marking group and assigned to mark - // " - // "robot {}", - // robot_id_, res.who_am_i_marking.value()); - // } else { - // SPDLOG_INFO("Robot {}: Joined marking group but not assigned to " - // "mark any robot", - // robot_id_); - // } + [this](const bool is_member) { + if (is_member) { pending_marking_state_ = true; } }); diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 5edeea92aed..246a86f8313 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -41,13 +41,13 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { } } if (kInvalidRobotId != waiting_robot_id) { - unassigned_markers_.erase(waiting_robot_id); + unassigned_markers_queue_.erase(waiting_robot_id); marking_list_[waiting_robot_id] = enemy_id; enemy_to_friends_[enemy_id] = waiting_robot_id; num_markers_++; } } else { - unassigned_markers_.erase(request->robot_id); + unassigned_markers_queue_.erase(request->robot_id); } response->success = true; return; @@ -63,7 +63,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { marking_list_[request->robot_id] = most_dangerous; num_markers_++; } else { - unassigned_markers_queue_.push_back(request->robot_id); + unassigned_markers_queue_.insert(request->robot_id); } } else { // should we kick someone out (is this new robot a better marker) @@ -91,7 +91,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { enemy_to_friends_[enemy_id] = request->robot_id; marking_list_[request->robot_id] = enemy_id; } else { - unassigned_markers_queue_.push_back(request->robot_id); + unassigned_markers_queue_.insert(request->robot_id); } } @@ -214,9 +214,6 @@ void Marking::update_danger_scores() { danger_score_[i] = danger_score; } - // for (size_t i = 0; i < 6; ++i) { - // SPDLOG_INFO("Robot {} has danger score {}", i, danger_score_[i]); - // } } uint8_t Marking::find_their_robot_in_possession() { diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp index 03107cdb788..56d274719ed 100644 --- a/src/rj_strategy/src/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -23,7 +23,7 @@ void MarkingClient::join_group(StatusCallback callback) { if (!client_->wait_for_service(std::chrono::seconds(1))) { SPDLOG_ERROR("Marking service not available."); if (callback) { - callback(Result{false}); + callback(false); } return; } @@ -39,7 +39,7 @@ void MarkingClient::join_group(StatusCallback callback) { // ROS2 async callbacks require value capture. if (!future.valid() || !future.get()->success) { if (callback) { - callback(Result{false}); + callback(false); } return; } @@ -50,13 +50,12 @@ void MarkingClient::join_group(StatusCallback callback) { subscription_ = node_->create_subscription( "marking_data", rclcpp::QoS(1).transient_local(), [this]( - const rj_msgs::msg::Marking::SharedPtr msg) { // callback=std::move(callback) + const rj_msgs::msg::Marking::SharedPtr msg) { selected_robot_marking_id_ = msg->mark_robot_ids[robot_id_]; am_i_marking_ = (selected_robot_marking_id_ != kInvalidRobotId); - // callback(Result{true, selected_robot_marking_id_}); }); - callback(Result{true}); + callback(true); }); } @@ -64,7 +63,7 @@ void MarkingClient::join_group(StatusCallback callback) { void MarkingClient::leave_group(StatusCallback callback) { if (!am_i_member_) { if (callback) { - callback(Result{false}); + callback(false); } return; } @@ -80,7 +79,7 @@ void MarkingClient::leave_group(StatusCallback callback) { // ROS2 async callbacks require value capture. if (!future.valid() || !future.get()->success) { if (callback) { - callback(Result{false}); + callback(false); } return; } @@ -96,7 +95,7 @@ void MarkingClient::leave_group(StatusCallback callback) { selected_robot_marking_id_ = kInvalidRobotId; if (callback) { - callback(Result{true, selected_robot_marking_id_}); + callback(true); } }); } From 5e2cc4dc3ba3d418902fbb4d945b8431e1d4fb87 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 23 Nov 2025 21:00:46 -0500 Subject: [PATCH 42/44] Fix Code Style On marking-coordinator (#2472) automated style fixes Co-authored-by: petergarud --- src/rj_strategy/src/agent/position/defense.cpp | 14 ++++++-------- src/rj_strategy/src/coordinator/marking.cpp | 4 +--- src/rj_strategy/src/coordinator/marking_client.cpp | 3 +-- 3 files changed, 8 insertions(+), 13 deletions(-) diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 7ba0cdd498f..68df6d15749 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -36,8 +36,7 @@ Defense::State Defense::update_state() { return IDLING; } - if (client_handles_->marking->am_i_member() && - client_handles_->marking->am_i_marking()) { + if (client_handles_->marking->am_i_member() && client_handles_->marking->am_i_marking()) { return MARKING; } else { return IDLING; @@ -110,12 +109,11 @@ Defense::State Defense::update_state() { sent_join_marking_group_request_ = true; request_time_ = RJ::now(); - client_handles_->marking->join_group( - [this](const bool is_member) { - if (is_member) { - pending_marking_state_ = true; - } - }); + client_handles_->marking->join_group([this](const bool is_member) { + if (is_member) { + pending_marking_state_ = true; + } + }); } auto elapsed = RJ::now() - request_time_; if (elapsed > kMarkingGroupJoinTimeout) { diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index 246a86f8313..f6c80e7f0d1 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -32,8 +32,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { // Queue is queue of robots that want to join marking but aren't good enough // Not close enough to mark or we exceeed the max num of markers for (uint8_t id : unassigned_markers_queue_) { - const auto& i_robot = - last_world_state_.get_robot(true, id); + const auto& i_robot = last_world_state_.get_robot(true, id); double distance = i_robot.pose.position().dist_to(enemy_robot.pose.position()); if (distance < min) { min = distance; @@ -213,7 +212,6 @@ void Marking::update_danger_scores() { danger_score_[i] = danger_score; } - } uint8_t Marking::find_their_robot_in_possession() { diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp index 56d274719ed..87421e15f8d 100644 --- a/src/rj_strategy/src/coordinator/marking_client.cpp +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -49,8 +49,7 @@ void MarkingClient::join_group(StatusCallback callback) { // Create subscription to track selected kicker. subscription_ = node_->create_subscription( "marking_data", rclcpp::QoS(1).transient_local(), - [this]( - const rj_msgs::msg::Marking::SharedPtr msg) { + [this](const rj_msgs::msg::Marking::SharedPtr msg) { selected_robot_marking_id_ = msg->mark_robot_ids[robot_id_]; am_i_marking_ = (selected_robot_marking_id_ != kInvalidRobotId); }); From 7c729e4ff792dd9e0997cec2deac265346824317 Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 3 Feb 2026 19:35:52 -0500 Subject: [PATCH 43/44] last few fixes --- .../rj_strategy/agent/position/defense.hpp | 1 + .../rj_strategy/coordinator/marking_client.hpp | 2 +- src/rj_strategy/src/agent/position/defense.cpp | 5 ++--- src/rj_strategy/src/coordinator/marking.cpp | 18 ++++++------------ 4 files changed, 10 insertions(+), 16 deletions(-) diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index e3989e646a4..7d8cec1e6db 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -44,6 +44,7 @@ class Defense : public Position { private: static constexpr int kMaxWallers{2}; static constexpr RJ::Seconds kMarkingGroupJoinTimeout{2.0}; + static constexpr float kMarkingDistanceFactor{0.55f}; /** * @brief The derived_get_task method returns the task for the defensive robot diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp index 293d2296dc3..5a42eb8a7c8 100644 --- a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp @@ -13,7 +13,7 @@ namespace strategy { /** - * @brief Client for interacting with the KickerPicker coordinator. + * @brief Client for interacting with the marking coordinator. * * Manages membership in the kicker group and tracks the currently selected kicker. */ diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index 68df6d15749..2df06e0e71a 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -202,9 +202,8 @@ std::optional Defense::state_to_task(RobotIntent intent) { .pose.position(); rj_geometry::Point ballPoint = last_world_state_->ball.position; - rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); - planning::LinearMotionInstant goal{targetPoint + targetToBall, - rj_geometry::Point{0.0, 0.0}}; + rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(kMarkingDistanceFactor); + planning::LinearMotionInstant goal{targetPoint + targetToBall}; // SPDLOG_INFO("Location to mark: {}, {}", (targetPoint + targetToBall).x(), (targetPoint + // targetToBall).y()); intent.motion_command = diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index f6c80e7f0d1..b29a59b741e 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -69,15 +69,16 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { double better_distance = 0; uint8_t kick_out_this_robot_id = kInvalidRobotId; const auto& robot_requesting = last_world_state_.get_robot(true, request->robot_id); + RJ::geometry::Point robot_requesting_pos = robot_requesting.pose.position(); // Kicked out robot is one that is furthest from its marker and new robot is closer than it - for (size_t i = 0; i < marking_list_.size(); ++i) { + for (size_t i = 0; i < kNumShells; ++i) { if (marking_list_[i] != kInvalidRobotId) { uint8_t enemy_id = marking_list_[i]; const auto& i_robot = last_world_state_.get_robot(true, i); const auto& enemy_robot = last_world_state_.get_robot(false, enemy_id); double dist = (i_robot.pose.position().dist_to(enemy_robot.pose.position())) - - (robot_requesting.pose.position().dist_to(enemy_robot.pose.position())); + (robot_requesting_pos.dist_to(enemy_robot.pose.position())); if (dist > better_distance) { better_distance = dist; kick_out_this_robot_id = i; @@ -110,7 +111,7 @@ void Marking::publish_marking_list() { bool assigned = false; // check if anyone being marked has the ball // if so, remove them from being marked and assign them the most dangerous robot - for (size_t i = 0; i < marking_list_.size(); ++i) { + for (size_t i = 0; i < kNumShells; ++i) { if (marking_list_[i] == robotInPossession && robotInPossession != kInvalidRobotId) { enemy_to_friends_[robotInPossession] = kInvalidRobotId; marking_list_[i] = most_dangerous; @@ -123,7 +124,7 @@ void Marking::publish_marking_list() { if (most_dangerous != kInvalidRobotId && !assigned) { uint8_t not_dangerous_robot_id = kInvalidRobotId; double max_danger_sub = 0.0; - for (size_t i = 0; i < marking_list_.size(); ++i) { + for (size_t i = 0; i < kNumShells; ++i) { if (marking_list_[i] != kInvalidRobotId) { uint8_t enemy_id = marking_list_[i]; double danger_sub = danger_score_[enemy_id] - danger_score_[most_dangerous]; @@ -180,13 +181,6 @@ void Marking::update_danger_scores() { double angle_between = 0.0; // Default to 0 (not dangerous) // Check if beyond midfield - // bool onOurSide = false; - // if (goal_loc.y() < field_center.y()) { - // onOurSide = robot.pose.position().y() < field_center.y(); - // } else { - // onOurSide = robot.pose.position().y() > field_center.y(); - // } - if (field_dimensions_.our_half().hit(robot.pose.position())) { const auto& vec_goal_to_center = field_center - goal_loc; const auto& vec_goal_to_robot = robot.pose.position() - goal_loc; @@ -236,7 +230,7 @@ uint8_t Marking::find_their_robot_in_possession() { uint8_t Marking::most_dangerous_robot(uint8_t robotInPossession) { uint8_t most_dangerous = kInvalidRobotId; double min = std::numeric_limits::infinity(); - for (size_t i = 0; i < danger_score_.size(); ++i) { + for (size_t i = 0; i < kNumShells; ++i) { // don't include marked robots or the guy with the ball in most dangerous calculation if (enemy_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { continue; From 4d7e53a56c3c9fe220f2d77a3770dd4b76bd324e Mon Sep 17 00:00:00 2001 From: shourikb Date: Tue, 3 Feb 2026 19:48:40 -0500 Subject: [PATCH 44/44] minor bug fix --- src/rj_strategy/src/coordinator/marking.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rj_strategy/src/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp index b29a59b741e..6a209e6fc68 100644 --- a/src/rj_strategy/src/coordinator/marking.cpp +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -69,7 +69,7 @@ void Marking::service_callback(RequestPtr request, ResponsePtr response) { double better_distance = 0; uint8_t kick_out_this_robot_id = kInvalidRobotId; const auto& robot_requesting = last_world_state_.get_robot(true, request->robot_id); - RJ::geometry::Point robot_requesting_pos = robot_requesting.pose.position(); + rj_geometry::Point robot_requesting_pos = robot_requesting.pose.position(); // Kicked out robot is one that is furthest from its marker and new robot is closer than it for (size_t i = 0; i < kNumShells; ++i) { if (marking_list_[i] != kInvalidRobotId) {