From 7ebeb436396e256f837e42243283d16434b97633 Mon Sep 17 00:00:00 2001 From: rishiso Date: Wed, 27 Dec 2023 02:14:52 +0000 Subject: [PATCH 1/9] Initial fsm --- .../agent/position/robot_factory_position.cpp | 55 ++++++++++++++++++- 1 file changed, 53 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 6667368a014..700da863bd4 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -16,8 +16,59 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id) { std::optional RobotFactoryPosition::get_task(WorldState& world_state, FieldDimensions& field_dimensions) { - // This is where the current_position can be reassigned based on the - // PlayState + // If keeper, make no changes + if (robot_id_ == 0) { + return current_position_->get_task(world_state, field_dimensions); + } + + // Get sorted positions of all friendly robots + typedef struct std::pair RobotPos; // (robotId, yPosition) + + std::vector robots_copy; + for (int i = 0; i <= 5; i++) { + robots_copy.emplace_back(i, world_state.our_robots[i].pose.position().y()); + } + + std::sort(robots_copy.begin(), robots_copy.end(), + [](RobotPos const& a, RobotPos const& b) { return a.second < b.second; }); + + // Find relative location of current robot + int i = 0; + for (RobotPos r : robots_copy) { + if (r.first == robot_id_) { + break; + } + i++; + } + + // Assigning new position + // Checking which half the ball is on (using 1.99 to avoid undetermined behavior on midline) + if (world_state.ball.position.y() > field_dimensions.length() / 1.99) { + // Offensive mode + // 3 robots on offense, 2 robots on defense + if (i >= 3) { + if (current_position_->get_name().compare("Offense") != 0) { + current_position_ = std::make_unique(robot_id_); + } + } else { + if (current_position_->get_name().compare("Defense") != 0) { + current_position_ = std::make_unique(robot_id_); + } + } + } else { + // Defensive mode + // 4 robots on defense, 1 robot on offense + if (i <= 4) { + if (current_position_->get_name().compare("Defense") != 0) { + current_position_ = std::make_unique(robot_id_); + } + } else { + if (current_position_->get_name().compare("Offense") != 0) { + current_position_ = std::make_unique(robot_id_); + } + } + } + return current_position_->get_task(world_state, field_dimensions); } From bc40cfeca8f1a92d58e1a72d8393eee1b6c8c73f Mon Sep 17 00:00:00 2001 From: rishiso Date: Wed, 27 Dec 2023 02:27:58 +0000 Subject: [PATCH 2/9] Adding possession as a consideration --- .../soccer/strategy/agent/position/robot_factory_position.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 700da863bd4..04c6988e77a 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -42,8 +42,8 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat } // Assigning new position - // Checking which half the ball is on (using 1.99 to avoid undetermined behavior on midline) - if (world_state.ball.position.y() > field_dimensions.length() / 1.99) { + // Checking whether we have possesion or if the ball is on their half (using 1.99 to avoid undetermined behavior on midline) + if (Position::our_possession_ || world_state.ball.position.y() > field_dimensions.length() / 1.99) { // Offensive mode // 3 robots on offense, 2 robots on defense if (i >= 3) { From 97f57a1a519e8c62885373282ae0daee817df4d3 Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Mon, 29 Jan 2024 01:09:42 +0000 Subject: [PATCH 3/9] Style fixes and minor changes --- .../agent/position/robot_factory_position.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 04c6988e77a..895f0c18a3e 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -42,16 +42,18 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat } // Assigning new position - // Checking whether we have possesion or if the ball is on their half (using 1.99 to avoid undetermined behavior on midline) - if (Position::our_possession_ || world_state.ball.position.y() > field_dimensions.length() / 1.99) { + // Checking whether we have possesion or if the ball is on their half (using 1.99 to avoid + // undetermined behavior on midline) + if (Position::our_possession_ || + world_state.ball.position.y() > field_dimensions.length() / 1.99) { // Offensive mode // 3 robots on offense, 2 robots on defense if (i >= 3) { - if (current_position_->get_name().compare("Offense") != 0) { + if (current_position_->get_name() != "Offense") { current_position_ = std::make_unique(robot_id_); } } else { - if (current_position_->get_name().compare("Defense") != 0) { + if (current_position_->get_name() != "Defense") { current_position_ = std::make_unique(robot_id_); } } @@ -59,11 +61,11 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat // Defensive mode // 4 robots on defense, 1 robot on offense if (i <= 4) { - if (current_position_->get_name().compare("Defense") != 0) { + if (current_position_->get_name() != "Defense") { current_position_ = std::make_unique(robot_id_); } } else { - if (current_position_->get_name().compare("Offense") != 0) { + if (current_position_->get_name() != "Offense") { current_position_ = std::make_unique(robot_id_); } } From a50a3628b508e8c93b5e554ed81c1e552648aed2 Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Mon, 29 Jan 2024 01:13:40 +0000 Subject: [PATCH 4/9] Adding todo about race condition --- .../soccer/strategy/agent/position/robot_factory_position.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 895f0c18a3e..7ab6bfc4c24 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -21,6 +21,8 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat return current_position_->get_task(world_state, field_dimensions); } + // TODO (Rishi and Jack): Make this synchronized across all robots to avoid race conditions + // Get sorted positions of all friendly robots typedef struct std::pair RobotPos; // (robotId, yPosition) From 31767b043a026b40a9458701964e9ccb8055f2e3 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 29 Jan 2024 17:22:21 -0500 Subject: [PATCH 5/9] Fix Code Style On position-fsm (#2174) automated style fixes Co-authored-by: rishiso From 9814868449bbd9b96a526bbe6a16e943d57ff58f Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Tue, 30 Jan 2024 19:17:24 -0500 Subject: [PATCH 6/9] Update soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp Co-authored-by: Sid Parikh --- .../soccer/strategy/agent/position/robot_factory_position.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 7ab6bfc4c24..a05fd34f2f2 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -24,7 +24,7 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat // TODO (Rishi and Jack): Make this synchronized across all robots to avoid race conditions // Get sorted positions of all friendly robots - typedef struct std::pair RobotPos; // (robotId, yPosition) + using RobotPos = std::pair; // (robotId, yPosition) std::vector robots_copy; for (int i = 0; i <= 5; i++) { From f828f56e14ac14d02910bec5055c97f5b22a2098 Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Wed, 31 Jan 2024 00:48:17 +0000 Subject: [PATCH 7/9] Working on PR changes (not finished) --- .../agent/position/robot_factory_position.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index a05fd34f2f2..2f4cfa3d475 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -27,7 +27,11 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat using RobotPos = std::pair; // (robotId, yPosition) std::vector robots_copy; - for (int i = 0; i <= 5; i++) { + for (int i = 0; i < world_state.our_robots.size(); i++) { + // Ignore goalie + if (i == 0) { + continue; + } robots_copy.emplace_back(i, world_state.our_robots[i].pose.position().y()); } @@ -45,12 +49,12 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat // Assigning new position // Checking whether we have possesion or if the ball is on their half (using 1.99 to avoid - // undetermined behavior on midline) + // rounding issues on midline) if (Position::our_possession_ || world_state.ball.position.y() > field_dimensions.length() / 1.99) { // Offensive mode - // 3 robots on offense, 2 robots on defense - if (i >= 3) { + // Closest 2 robots on defense, rest on offense + if (i >= 2) { if (current_position_->get_name() != "Offense") { current_position_ = std::make_unique(robot_id_); } @@ -61,8 +65,8 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat } } else { // Defensive mode - // 4 robots on defense, 1 robot on offense - if (i <= 4) { + // Closest 4 robots on defense, rest on offense + if (i <= 3) { if (current_position_->get_name() != "Defense") { current_position_ = std::make_unique(robot_id_); } From 9f0daebf475cfc7003f3621b5bd4bb71dfc3d70a Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Wed, 31 Jan 2024 01:06:27 +0000 Subject: [PATCH 8/9] More fixes (not finished) --- .../strategy/agent/position/robot_factory_position.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 2f4cfa3d475..09edba0086c 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -54,14 +54,14 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat world_state.ball.position.y() > field_dimensions.length() / 1.99) { // Offensive mode // Closest 2 robots on defense, rest on offense - if (i >= 2) { - if (current_position_->get_name() != "Offense") { - current_position_ = std::make_unique(robot_id_); - } - } else { + if (i <= 1) { if (current_position_->get_name() != "Defense") { current_position_ = std::make_unique(robot_id_); } + } else { + if (current_position_->get_name() != "Offense") { + current_position_ = std::make_unique(robot_id_); + } } } else { // Defensive mode From c693e6ff7da7ef7acfbeb2666953d26de6683139 Mon Sep 17 00:00:00 2001 From: Rishi Soni Date: Mon, 5 Feb 2024 02:12:23 +0000 Subject: [PATCH 9/9] Hardcoding number of robots --- .../soccer/strategy/agent/position/robot_factory_position.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 09edba0086c..8f87db3f764 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -27,7 +27,7 @@ std::optional RobotFactoryPosition::get_task(WorldState& world_stat using RobotPos = std::pair; // (robotId, yPosition) std::vector robots_copy; - for (int i = 0; i < world_state.our_robots.size(); i++) { + for (int i = 0; i < 6; i++) { // Ignore goalie if (i == 0) { continue;