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 0671ef1eb8..2f2eb3953d 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 @@ -38,6 +38,8 @@ class FreeKicker : public Position { private: std::optional derived_get_task(RobotIntent intent) override; + double get_shot_dist_to_goalie(rj_geometry::Point const& goalie_pos, + rj_geometry::Point const& shot_target); }; } // namespace strategy diff --git a/src/rj_strategy/src/agent/position/free_kicker.cpp b/src/rj_strategy/src/agent/position/free_kicker.cpp index 29611d40d0..f71d6b7d8f 100644 --- a/src/rj_strategy/src/agent/position/free_kicker.cpp +++ b/src/rj_strategy/src/agent/position/free_kicker.cpp @@ -11,11 +11,49 @@ std::optional FreeKicker::derived_get_task(RobotIntent intent) { // SPDLOG_INFO("Free Kicker {} is running", this->robot_id_); - rj_geometry::Point goal_corner{ - this->field_dimensions_.their_goal_loc().x() + 0.5 * this->field_dimensions_.goal_width(), - this->field_dimensions_.their_goal_loc().y()}; + // Read positions of their team + std::vector const their_robots = this->last_world_state_->their_robots; + rj_geometry::Point enemy_goalie_location = this->field_dimensions_.their_goal_loc(); - planning::LinearMotionInstant target{goal_corner}; + for (const RobotState& enemy : their_robots) { + // Get position of their goalie + if (this->field_dimensions_.their_defense_area().hit(enemy.pose.position())) { + enemy_goalie_location = enemy.pose.position(); + break; + } + } + + constexpr double BALL_WIDTH_OFFSET = kBallRadius * 10; + rj_geometry::Point const right_goal_post = + this->field_dimensions_.their_goal_loc() + + rj_geometry::Point((this->field_dimensions_.goal_width() / 2.0) - BALL_WIDTH_OFFSET, 0); + + rj_geometry::Point const left_goal_post = + this->field_dimensions_.their_goal_loc() - + rj_geometry::Point((this->field_dimensions_.goal_width() / 2.0) - BALL_WIDTH_OFFSET, 0); + + double const left_dist = get_shot_dist_to_goalie(enemy_goalie_location, left_goal_post); + double const right_dist = get_shot_dist_to_goalie(enemy_goalie_location, right_goal_post); + rj_geometry::Point const best_shot = left_dist < right_dist ? right_goal_post : left_goal_post; + + /* + LOGIC FOR PASSING WHEN SHOT TOO EXTREME (i.e. CORNER KICK) + + double const shot_angle = abs((best_shot - this->last_world_state_->ball.position).angle()); + + if (shot_angle > 3 * M_PI / 4.0 || + shot_angle < M_PI / 4.0) { + SPDLOG_INFO("Free Kicker {}: Shot angle {} too extreme, not shooting", this->robot_id_, + shot_angle); + // PASS + return intent; + } + + SPDLOG_INFO("Free Kicker {} shooting at point {}, {} with angle {}", this->robot_id_, + best_shot.x(), best_shot.y(), shot_angle); + */ + + planning::LinearMotionInstant target{best_shot}; auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; intent.motion_command = line_kick_cmd; @@ -37,4 +75,10 @@ void FreeKicker::derived_pass_ball() {} void FreeKicker::derived_acknowledge_ball_in_transit() {} -} // namespace strategy +double FreeKicker::get_shot_dist_to_goalie(rj_geometry::Point const& goalie_pos, + rj_geometry::Point const& shot_target) { + rj_geometry::Point const ball_position = this->last_world_state_->ball.position; + return std::abs((goalie_pos - ball_position).cross(shot_target - ball_position)) / + (shot_target - ball_position).mag(); +} +} // namespace strategy \ No newline at end of file