From 75edccc354c3c6119d57f7b5f6130cce719c772c Mon Sep 17 00:00:00 2001 From: Cameron Lyon <91966745+CameronLyon@users.noreply.github.com> Date: Mon, 27 Jan 2025 09:37:33 -0500 Subject: [PATCH 1/9] 1/26/2025 --- .../agent/position/penalty_player.cpp | 46 ++++++++++++++++++- .../agent/position/penalty_player.hpp | 7 ++- 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 3328cff29ff..3db3b070055 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -18,10 +18,22 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { // if penalty playing and restart penalty in playstate we switch to shooting if (current_play_state_.is_ready() && (current_play_state_.is_penalty() || current_play_state_.is_kickoff())) { - return SHOOTING_START; + return DRIBBLING_START; } break; } + case DRIBBLING_START: { + if (distance_to_ball() < kOwnBallRadius) { + return DRIBBLING; + } + break; + } + case DRIBBLING: { + // if (distance_to_shooting() < kOwnBallRadius) { + // return SHOOTING_START; + // } + break; + } case SHOOTING_START: { if (check_is_done()) { return SHOOTING; @@ -64,6 +76,38 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; break; } + case DRIBBLING_START: { + target_ = calculate_best_shot(); + rj_geometry::Point ball_position = last_world_state_->ball.position; + auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); + auto move_vector = (current_pos - ball_position).normalized(0.2); + + planning::LinearMotionInstant target{ball_position}; + planning::MotionCommand prep_command{"path_target", target, planning::FaceBall{}}; + + intent.motion_command = prep_command; + intent.dribbler_speed = 255.0; + + return intent; + } + case DRIBBLING: { + rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); + auto curr_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); + SPDLOG_INFO("GOAL POSITIONS ARE {} AND {}", their_goal_pos.x(), their_goal_pos.y()); + // if (their_goal_pos.y() > 4.5) { + // rj_geometry::Point target_pt{0, 6.75}; + // } else { + // rj_geometry::Point target_pt{0, 2.25}; + // } + rj_geometry::Point target_pt{0, 6.75}; + rj_geometry::Point target_vel{0.0, 0.0}; + planning::LinearMotionInstant target{target_pt, target_vel}; + planning::MotionCommand prep_command{"path_target", target, planning::FaceBall{}, true}; + intent.motion_command = prep_command; + intent.dribbler_speed = 255.0; + return intent; + break; + } case SHOOTING_START: { target_ = calculate_best_shot(); rj_geometry::Point ball_position = last_world_state_->ball.position; diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp index d60af05adf2..b49fbc91a6a 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp @@ -40,7 +40,7 @@ class PenaltyPlayer : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - enum State { LINE_UP, SHOOTING_START, SHOOTING }; + enum State { LINE_UP, SHOOTING_START, SHOOTING, DRIBBLING, DRIBBLING_START }; static constexpr double kOwnBallRadius{kRobotRadius + 0.1}; @@ -56,6 +56,11 @@ class PenaltyPlayer : public Position { last_world_state_->get_robot(true, robot_id_).pose.position()); }; + double distance_to_shooting() const { + return last_world_state_->get_robot(true, robot_id_).pose.position().dist_to( + rj_geometry::Point(0,6.75)); + }; + /** * @return the target (within the goal) that would be the most clear shot */ From 27f938ffb313aeb8312ae86640a7a31b9990e810 Mon Sep 17 00:00:00 2001 From: Cameron Lyon <91966745+CameronLyon@users.noreply.github.com> Date: Sun, 2 Feb 2025 19:57:54 -0500 Subject: [PATCH 2/9] 2/2/2025, modified behaviour of penalty shooting so that the ball gets kicked once shortly, follows it, then kicks to goal. Only works when ball is at center field. --- .../agent/position/penalty_player.cpp | 85 +++++++++++-------- .../agent/position/penalty_player.hpp | 6 +- 2 files changed, 52 insertions(+), 39 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 3db3b070055..80c1a5147f7 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -18,31 +18,36 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { // if penalty playing and restart penalty in playstate we switch to shooting if (current_play_state_.is_ready() && (current_play_state_.is_penalty() || current_play_state_.is_kickoff())) { - return DRIBBLING_START; + return SMALL_KICK_START; } break; } - case DRIBBLING_START: { - if (distance_to_ball() < kOwnBallRadius) { - return DRIBBLING; + case SMALL_KICK_START: { + if (distance_to_ball() < kOwnBallRadius || check_is_done()) { + return SMALL_KICK; } break; } - case DRIBBLING: { - // if (distance_to_shooting() < kOwnBallRadius) { - // return SHOOTING_START; - // } + case SMALL_KICK: { + if (check_is_done() || distance_from_enemy_goal() < 3.5) { + return LINE_UP_2; + } break; } - case SHOOTING_START: { + case LINE_UP_2: { if (check_is_done()) { - return SHOOTING; + return SHOOTING_START; } if (distance_to_ball() < kOwnBallRadius) { - return SHOOTING; + return SHOOTING_START; } break; } + case SHOOTING_START: { + if (check_is_done()) { + return SHOOTING; + } + } case SHOOTING: { if (check_is_done()) { return LINE_UP; @@ -55,14 +60,9 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { switch (latest_state_) { - case LINE_UP: { + case LINE_UP: { // First, gets the robot to the ball to begin penalty dribbling-shooting double y_pos = last_world_state_->ball.position.y(); - // if ball is above goal, increase y_pos, else decrease - // if (y_pos - field_dimensions_.their_goal_loc().y() > 0) { - // y_pos += kRobotRadius - 0.15; - // } else { y_pos -= kRobotRadius + 0.3; - // } rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball @@ -76,38 +76,49 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; break; } - case DRIBBLING_START: { - target_ = calculate_best_shot(); + case SMALL_KICK_START: { rj_geometry::Point ball_position = last_world_state_->ball.position; auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); auto move_vector = (current_pos - ball_position).normalized(0.2); - planning::LinearMotionInstant target{ball_position}; + planning::LinearMotionInstant target{ball_position + move_vector}; planning::MotionCommand prep_command{"path_target", target, planning::FaceBall{}}; intent.motion_command = prep_command; - intent.dribbler_speed = 255.0; return intent; } - case DRIBBLING: { - rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - auto curr_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); - SPDLOG_INFO("GOAL POSITIONS ARE {} AND {}", their_goal_pos.x(), their_goal_pos.y()); - // if (their_goal_pos.y() > 4.5) { - // rj_geometry::Point target_pt{0, 6.75}; - // } else { - // rj_geometry::Point target_pt{0, 2.25}; - // } - rj_geometry::Point target_pt{0, 6.75}; - rj_geometry::Point target_vel{0.0, 0.0}; - planning::LinearMotionInstant target{target_pt, target_vel}; - planning::MotionCommand prep_command{"path_target", target, planning::FaceBall{}, true}; - intent.motion_command = prep_command; - intent.dribbler_speed = 255.0; + case SMALL_KICK: { + rj_geometry::Point center_goal{0,9}; + auto line_kick_cmd = + planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}}; + + intent.motion_command = line_kick_cmd; + intent.shoot_mode = RobotIntent::ShootMode::KICK; + intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; + intent.kick_speed = 0.05; + return intent; break; } + case LINE_UP_2: { + double y_pos = last_world_state_->ball.position.y(); + y_pos -= kRobotRadius + 0.3; + rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; + rj_geometry::Point target_vel{0.0, 0.0}; + // Face ball + planning::PathTargetFaceOption face_option{planning::FaceBall{}}; + // Avoid ball + bool ignore_ball{false}; + + // Create Motion Command + planning::LinearMotionInstant goal{target_pt, target_vel}; + intent.motion_command = + planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + break; + + return intent; + } case SHOOTING_START: { target_ = calculate_best_shot(); rj_geometry::Point ball_position = last_world_state_->ball.position; @@ -173,6 +184,8 @@ rj_geometry::Point PenaltyPlayer::calculate_best_shot() const { // Ball location rj_geometry::Point ball_position = this->last_world_state_->ball.position; + // Iterates across 19 possible shot locations along the goal width in 0.05-meter increments. + // For each location, it calculates the clearance distance from opponent robots and updates the best shot position if a better (less obstructed) option is found. rj_geometry::Point best_shot = their_goal_pos; double best_distance = -1.0; rj_geometry::Point increment(0.05, 0); diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp index b49fbc91a6a..9b355816cd6 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp @@ -40,7 +40,7 @@ class PenaltyPlayer : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - enum State { LINE_UP, SHOOTING_START, SHOOTING, DRIBBLING, DRIBBLING_START }; + enum State { LINE_UP, SHOOTING_START, SHOOTING, SMALL_KICK_START, SMALL_KICK, LINE_UP_2 }; static constexpr double kOwnBallRadius{kRobotRadius + 0.1}; @@ -56,9 +56,9 @@ class PenaltyPlayer : public Position { last_world_state_->get_robot(true, robot_id_).pose.position()); }; - double distance_to_shooting() const { + double distance_from_enemy_goal() const { return last_world_state_->get_robot(true, robot_id_).pose.position().dist_to( - rj_geometry::Point(0,6.75)); + field_dimensions_.their_goal_loc()); }; /** From d4585c644844f7c45abc9a6cd29592c74f3b6785 Mon Sep 17 00:00:00 2001 From: rociopv06 Date: Sun, 2 Feb 2025 21:07:04 -0500 Subject: [PATCH 3/9] Modified penalty behavior to work from the correct position (y=3). Follows ball by kicking with speed 0 and then kicks when closer to the goal. WARNING: Has only been tested on yellow side --- .../agent/position/penalty_player.cpp | 38 +++++-------------- 1 file changed, 10 insertions(+), 28 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 80c1a5147f7..3088256f282 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -18,18 +18,12 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { // if penalty playing and restart penalty in playstate we switch to shooting if (current_play_state_.is_ready() && (current_play_state_.is_penalty() || current_play_state_.is_kickoff())) { - return SMALL_KICK_START; - } - break; - } - case SMALL_KICK_START: { - if (distance_to_ball() < kOwnBallRadius || check_is_done()) { return SMALL_KICK; } break; } case SMALL_KICK: { - if (check_is_done() || distance_from_enemy_goal() < 3.5) { + if (distance_from_enemy_goal() < 3.5) { return LINE_UP_2; } break; @@ -62,33 +56,20 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { switch (latest_state_) { case LINE_UP: { // First, gets the robot to the ball to begin penalty dribbling-shooting double y_pos = last_world_state_->ball.position.y(); - y_pos -= kRobotRadius + 0.3; + y_pos -= kRobotRadius + 0.3; // added the 0.01 as a buffer space rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball planning::PathTargetFaceOption face_option{planning::FaceBall{}}; - // Avoid ball - bool ignore_ball{false}; // Create Motion Command planning::LinearMotionInstant goal{target_pt, target_vel}; intent.motion_command = - planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; + planning::MotionCommand{"path_target", goal, planning::FaceBall{}}; break; } - case SMALL_KICK_START: { - rj_geometry::Point ball_position = last_world_state_->ball.position; - auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); - auto move_vector = (current_pos - ball_position).normalized(0.2); - - planning::LinearMotionInstant target{ball_position + move_vector}; - planning::MotionCommand prep_command{"path_target", target, planning::FaceBall{}}; - - intent.motion_command = prep_command; - - return intent; - } - case SMALL_KICK: { + case SMALL_KICK: { //less of a kick, more of a "follow" ball closely + rj_geometry::Point center_goal{0,9}; auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}}; @@ -96,14 +77,15 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - intent.kick_speed = 0.05; + intent.kick_speed = 0.0; + // the point of making a 0 kick speed is to fake dribble since we cannot get the vaccum behavior to work return intent; break; } - case LINE_UP_2: { + case LINE_UP_2: { // gets the robot behind the ball with a certain distance double y_pos = last_world_state_->ball.position.y(); - y_pos -= kRobotRadius + 0.3; + y_pos -= kRobotRadius + 0.1; rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball @@ -119,7 +101,7 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING_START: { + case SHOOTING_START: { target_ = calculate_best_shot(); rj_geometry::Point ball_position = last_world_state_->ball.position; auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); From e8f50a3a87908a877e7b7827b4ea4f1e4dd3f2d6 Mon Sep 17 00:00:00 2001 From: rociopv06 Date: Sun, 2 Feb 2025 21:23:47 -0500 Subject: [PATCH 4/9] quick edit to the .hpp states --- soccer/src/soccer/strategy/agent/position/penalty_player.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp index 9b355816cd6..5e986de5d13 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp @@ -40,7 +40,7 @@ class PenaltyPlayer : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - enum State { LINE_UP, SHOOTING_START, SHOOTING, SMALL_KICK_START, SMALL_KICK, LINE_UP_2 }; + enum State { LINE_UP, SHOOTING_START, SHOOTING, SMALL_KICK, LINE_UP_2 }; static constexpr double kOwnBallRadius{kRobotRadius + 0.1}; From 310f2d728094b6fc3a771e20005f3b053306132b Mon Sep 17 00:00:00 2001 From: Cameron Lyon <91966745+CameronLyon@users.noreply.github.com> Date: Tue, 4 Feb 2025 20:10:13 -0500 Subject: [PATCH 5/9] 2/4/2025, added improvements following Jack's comments --- .../agent/position/penalty_player.cpp | 32 +++++++++++-------- .../agent/position/penalty_player.hpp | 6 ++-- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 3088256f282..98ca0ae663b 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -14,7 +14,7 @@ std::optional PenaltyPlayer::derived_get_task(RobotIntent intent) { PenaltyPlayer::State PenaltyPlayer::update_state() { switch (latest_state_) { - case LINE_UP: { + case START: { // if penalty playing and restart penalty in playstate we switch to shooting if (current_play_state_.is_ready() && (current_play_state_.is_penalty() || current_play_state_.is_kickoff())) { @@ -23,12 +23,12 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { break; } case SMALL_KICK: { - if (distance_from_enemy_goal() < 3.5) { - return LINE_UP_2; + if (distance_from_enemy_goal() < kDistanceToGoalThreshold) { + return LINE_UP; } break; } - case LINE_UP_2: { + case LINE_UP: { if (check_is_done()) { return SHOOTING_START; } @@ -41,10 +41,12 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { if (check_is_done()) { return SHOOTING; } + + break; } case SHOOTING: { if (check_is_done()) { - return LINE_UP; + return START; } break; } @@ -54,7 +56,8 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { switch (latest_state_) { - case LINE_UP: { // First, gets the robot to the ball to begin penalty dribbling-shooting + case START: { // First, gets the robot to the ball to begin penalty dribbling-shooting + SPDLOG_INFO("START"); double y_pos = last_world_state_->ball.position.y(); y_pos -= kRobotRadius + 0.3; // added the 0.01 as a buffer space rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; @@ -69,8 +72,8 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { break; } case SMALL_KICK: { //less of a kick, more of a "follow" ball closely - - rj_geometry::Point center_goal{0,9}; + //SPDLOG_INFO("SMALL_KICK"); + rj_geometry::Point center_goal = field_dimensions_.their_goal_loc(); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}}; @@ -81,9 +84,9 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { // the point of making a 0 kick speed is to fake dribble since we cannot get the vaccum behavior to work return intent; - break; } - case LINE_UP_2: { // gets the robot behind the ball with a certain distance + case LINE_UP: { // gets the robot behind the ball with a certain distance (usually immediatly skipped if the robot is already close) + //SPDLOG_INFO("LINE_UP"); double y_pos = last_world_state_->ball.position.y(); y_pos -= kRobotRadius + 0.1; rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; @@ -101,7 +104,8 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING_START: { + case SHOOTING_START: { // Positions the robot behind the ball at a certain angle so that it has a straight shot towards the goal + //SPDLOG_INFO("SHOOTING_START"); target_ = calculate_best_shot(); rj_geometry::Point ball_position = last_world_state_->ball.position; auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); @@ -114,7 +118,8 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING: { + case SHOOTING: { // Kicks the ball with a now much higher speed (basically SMALL_KICK but power set to 4) + //SPDLOG_INFO("SHOOTING"); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; @@ -124,7 +129,6 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { intent.kick_speed = 4.0; return intent; - break; } } @@ -161,7 +165,7 @@ double PenaltyPlayer::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point PenaltyPlayer::calculate_best_shot() const { // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - double goal_width = field_dimensions_.goal_width(); // 1.0 meters + double goal_width = field_dimensions_.goal_width(); // 1.0 meters // BALL SEEMS TO OCCASIONALLY MISS SHOT ON EDGE, CONSIDER CHANGING THIS? // Ball location rj_geometry::Point ball_position = this->last_world_state_->ball.position; diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp index 5e986de5d13..e9c214ccf87 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp @@ -40,10 +40,12 @@ class PenaltyPlayer : public Position { private: std::optional derived_get_task(RobotIntent intent) override; - enum State { LINE_UP, SHOOTING_START, SHOOTING, SMALL_KICK, LINE_UP_2 }; + enum State { START, SMALL_KICK, LINE_UP, SHOOTING_START, SHOOTING }; static constexpr double kOwnBallRadius{kRobotRadius + 0.1}; + static constexpr double kDistanceToGoalThreshold{3.5}; + State update_state(); /** @@ -82,7 +84,7 @@ class PenaltyPlayer : public Position { std::optional state_to_task(RobotIntent intent); // current state of Goalie (state machine) - State latest_state_ = LINE_UP; + State latest_state_ = START; }; } // namespace strategy From 7873e37cc673c6fb45b671a0c0a2728fcd720d41 Mon Sep 17 00:00:00 2001 From: Cameron Lyon <91966745+CameronLyon@users.noreply.github.com> Date: Tue, 4 Feb 2025 20:12:31 -0500 Subject: [PATCH 6/9] quick change to a single line --- soccer/src/soccer/strategy/agent/position/penalty_player.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 98ca0ae663b..3ddf350c8ea 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -100,7 +100,6 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { planning::LinearMotionInstant goal{target_pt, target_vel}; intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, ignore_ball}; - break; return intent; } From 7c2126f40cb17e7d59c9cdf2ddf3227bc7a0251d Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 11 Feb 2025 19:34:49 -0500 Subject: [PATCH 7/9] Fix Code Style On cameron-carson/penalty-position-dribbling (#2337) automated style fixes Co-authored-by: CameronLyon --- .../agent/position/penalty_player.cpp | 39 +++++++++++-------- .../agent/position/penalty_player.hpp | 5 ++- 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 3ddf350c8ea..1eccb8522a2 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -25,7 +25,7 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { case SMALL_KICK: { if (distance_from_enemy_goal() < kDistanceToGoalThreshold) { return LINE_UP; - } + } break; } case LINE_UP: { @@ -56,10 +56,10 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { switch (latest_state_) { - case START: { // First, gets the robot to the ball to begin penalty dribbling-shooting + case START: { // First, gets the robot to the ball to begin penalty dribbling-shooting SPDLOG_INFO("START"); double y_pos = last_world_state_->ball.position.y(); - y_pos -= kRobotRadius + 0.3; // added the 0.01 as a buffer space + y_pos -= kRobotRadius + 0.3; // added the 0.01 as a buffer space rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball @@ -71,8 +71,8 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { planning::MotionCommand{"path_target", goal, planning::FaceBall{}}; break; } - case SMALL_KICK: { //less of a kick, more of a "follow" ball closely - //SPDLOG_INFO("SMALL_KICK"); + case SMALL_KICK: { // less of a kick, more of a "follow" ball closely + // SPDLOG_INFO("SMALL_KICK"); rj_geometry::Point center_goal = field_dimensions_.their_goal_loc(); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}}; @@ -80,13 +80,15 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - intent.kick_speed = 0.0; - // the point of making a 0 kick speed is to fake dribble since we cannot get the vaccum behavior to work + intent.kick_speed = 0.0; + // the point of making a 0 kick speed is to fake dribble since we cannot get the vaccum + // behavior to work return intent; } - case LINE_UP: { // gets the robot behind the ball with a certain distance (usually immediatly skipped if the robot is already close) - //SPDLOG_INFO("LINE_UP"); + case LINE_UP: { // gets the robot behind the ball with a certain distance (usually + // immediatly skipped if the robot is already close) + // SPDLOG_INFO("LINE_UP"); double y_pos = last_world_state_->ball.position.y(); y_pos -= kRobotRadius + 0.1; rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; @@ -103,8 +105,9 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING_START: { // Positions the robot behind the ball at a certain angle so that it has a straight shot towards the goal - //SPDLOG_INFO("SHOOTING_START"); + case SHOOTING_START: { // Positions the robot behind the ball at a certain angle so that it + // has a straight shot towards the goal + // SPDLOG_INFO("SHOOTING_START"); target_ = calculate_best_shot(); rj_geometry::Point ball_position = last_world_state_->ball.position; auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); @@ -117,8 +120,9 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING: { // Kicks the ball with a now much higher speed (basically SMALL_KICK but power set to 4) - //SPDLOG_INFO("SHOOTING"); + case SHOOTING: { // Kicks the ball with a now much higher speed (basically SMALL_KICK but + // power set to 4) + // SPDLOG_INFO("SHOOTING"); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; @@ -164,13 +168,16 @@ double PenaltyPlayer::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point PenaltyPlayer::calculate_best_shot() const { // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - double goal_width = field_dimensions_.goal_width(); // 1.0 meters // BALL SEEMS TO OCCASIONALLY MISS SHOT ON EDGE, CONSIDER CHANGING THIS? + double goal_width = + field_dimensions_.goal_width(); // 1.0 meters // BALL SEEMS TO OCCASIONALLY MISS SHOT ON + // EDGE, CONSIDER CHANGING THIS? // Ball location rj_geometry::Point ball_position = this->last_world_state_->ball.position; - // Iterates across 19 possible shot locations along the goal width in 0.05-meter increments. - // For each location, it calculates the clearance distance from opponent robots and updates the best shot position if a better (less obstructed) option is found. + // Iterates across 19 possible shot locations along the goal width in 0.05-meter increments. + // For each location, it calculates the clearance distance from opponent robots and updates the + // best shot position if a better (less obstructed) option is found. rj_geometry::Point best_shot = their_goal_pos; double best_distance = -1.0; rj_geometry::Point increment(0.05, 0); diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp index e9c214ccf87..9b1febddc30 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp @@ -59,8 +59,9 @@ class PenaltyPlayer : public Position { }; double distance_from_enemy_goal() const { - return last_world_state_->get_robot(true, robot_id_).pose.position().dist_to( - field_dimensions_.their_goal_loc()); + return last_world_state_->get_robot(true, robot_id_) + .pose.position() + .dist_to(field_dimensions_.their_goal_loc()); }; /** From 260f45844e8ea33224981885fa46cf1ec64cef1b Mon Sep 17 00:00:00 2001 From: rociopv06 Date: Sun, 16 Feb 2025 20:18:58 -0500 Subject: [PATCH 8/9] Edits based on pull request comments. Mainly cleaned up comments and fixed inconsistencies in variable naming- improved code readability --- .../agent/position/penalty_player.cpp | 39 +++++++++---------- 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 1eccb8522a2..064443a3372 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -29,10 +29,7 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { break; } case LINE_UP: { - if (check_is_done()) { - return SHOOTING_START; - } - if (distance_to_ball() < kOwnBallRadius) { + if (check_is_done() || distance_to_ball() < kOwnBallRadius) { return SHOOTING_START; } break; @@ -57,9 +54,10 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { switch (latest_state_) { case START: { // First, gets the robot to the ball to begin penalty dribbling-shooting - SPDLOG_INFO("START"); double y_pos = last_world_state_->ball.position.y(); - y_pos -= kRobotRadius + 0.3; // added the 0.01 as a buffer space + // Add 0.3 buffer space to the y_pos of the ball to ensure the robot does not + // hit the ball before being properly lined up behind it + y_pos -= kRobotRadius + 0.3; rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball @@ -69,10 +67,9 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { planning::LinearMotionInstant goal{target_pt, target_vel}; intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}}; - break; + return intent; } case SMALL_KICK: { // less of a kick, more of a "follow" ball closely - // SPDLOG_INFO("SMALL_KICK"); rj_geometry::Point center_goal = field_dimensions_.their_goal_loc(); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}}; @@ -81,16 +78,15 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; intent.kick_speed = 0.0; - // the point of making a 0 kick speed is to fake dribble since we cannot get the vaccum + // The point of making a 0 kick speed is to fake dribble since we cannot get the vaccum // behavior to work return intent; } - case LINE_UP: { // gets the robot behind the ball with a certain distance (usually + case LINE_UP: { // Gets the robot behind the ball with a certain distance (usually // immediatly skipped if the robot is already close) - // SPDLOG_INFO("LINE_UP"); double y_pos = last_world_state_->ball.position.y(); - y_pos -= kRobotRadius + 0.1; + y_pos -= kOwnBallRadius; rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball @@ -107,7 +103,6 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { } case SHOOTING_START: { // Positions the robot behind the ball at a certain angle so that it // has a straight shot towards the goal - // SPDLOG_INFO("SHOOTING_START"); target_ = calculate_best_shot(); rj_geometry::Point ball_position = last_world_state_->ball.position; auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); @@ -122,7 +117,6 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { } case SHOOTING: { // Kicks the ball with a now much higher speed (basically SMALL_KICK but // power set to 4) - // SPDLOG_INFO("SHOOTING"); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; @@ -164,25 +158,28 @@ double PenaltyPlayer::distance_from_their_robots(rj_geometry::Point tail, } return min_angle; } - +/** + * @brief Iterates across 19 possible shot target locations along the goal width in 0.05-meter increments. + * For each location, it calculates the clearance distance from opponent robots and updates the + * best shot position if a better (less obstructed) option is found. + * @return The best target position (farthest from obstacles) found after considering all 19 possibilities. + */ rj_geometry::Point PenaltyPlayer::calculate_best_shot() const { // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); + // TODO: Consider reducing goal width variable to reduce the possibility of the shot missing at edges double goal_width = - field_dimensions_.goal_width(); // 1.0 meters // BALL SEEMS TO OCCASIONALLY MISS SHOT ON - // EDGE, CONSIDER CHANGING THIS? + field_dimensions_.goal_width(); // 1.0 meters // Ball location rj_geometry::Point ball_position = this->last_world_state_->ball.position; - - // Iterates across 19 possible shot locations along the goal width in 0.05-meter increments. - // For each location, it calculates the clearance distance from opponent robots and updates the - // best shot position if a better (less obstructed) option is found. + // Sets initial target shot at the middle of their goal rj_geometry::Point best_shot = their_goal_pos; double best_distance = -1.0; rj_geometry::Point increment(0.05, 0); rj_geometry::Point curr_point = their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; + // Compare shots to find best possibility for (int i = 0; i < 19; i++) { double distance = distance_from_their_robots(ball_position, curr_point); if (distance > best_distance) { From ffc9fc00e28a7f037f1747558a7799d64efe6762 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 16 Feb 2025 23:09:31 -0500 Subject: [PATCH 9/9] Fix Code Style On cameron-carson/penalty-position-dribbling (#2345) automated style fixes Co-authored-by: rociopv06 --- .../agent/position/penalty_player.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 064443a3372..6e9bc063451 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -57,7 +57,7 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { double y_pos = last_world_state_->ball.position.y(); // Add 0.3 buffer space to the y_pos of the ball to ensure the robot does not // hit the ball before being properly lined up behind it - y_pos -= kRobotRadius + 0.3; + y_pos -= kRobotRadius + 0.3; rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball @@ -159,21 +159,22 @@ double PenaltyPlayer::distance_from_their_robots(rj_geometry::Point tail, return min_angle; } /** - * @brief Iterates across 19 possible shot target locations along the goal width in 0.05-meter increments. - * For each location, it calculates the clearance distance from opponent robots and updates the - * best shot position if a better (less obstructed) option is found. - * @return The best target position (farthest from obstacles) found after considering all 19 possibilities. + * @brief Iterates across 19 possible shot target locations along the goal width in 0.05-meter + * increments. For each location, it calculates the clearance distance from opponent robots and + * updates the best shot position if a better (less obstructed) option is found. + * @return The best target position (farthest from obstacles) found after considering all 19 + * possibilities. */ rj_geometry::Point PenaltyPlayer::calculate_best_shot() const { // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - // TODO: Consider reducing goal width variable to reduce the possibility of the shot missing at edges - double goal_width = - field_dimensions_.goal_width(); // 1.0 meters + // TODO: Consider reducing goal width variable to reduce the possibility of the shot missing at + // edges + double goal_width = field_dimensions_.goal_width(); // 1.0 meters // Ball location rj_geometry::Point ball_position = this->last_world_state_->ball.position; - // Sets initial target shot at the middle of their goal + // Sets initial target shot at the middle of their goal rj_geometry::Point best_shot = their_goal_pos; double best_distance = -1.0; rj_geometry::Point increment(0.05, 0);