Improved free kick routine

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
+unset _colcon_prefix_chain_bash_source_script

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
+unset _colcon_prefix_chain_zsh_source_script

src/rj_strategy/src/agent/position/free_kicker.cpp

@@ -11,11 +11,51 @@ std::optional<RobotIntent> 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<RobotState> 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 = 0.025;
+    constexpr double BALL_WIDTH_OFFSET = BALL_WIDTH * 3;
+    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.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.0);
+    rj_geometry::Point best_shot = right_goal_post;
+    double best_distance = -1.0;
+    rj_geometry::Point ball_position = this->last_world_state_->ball.position;
+
+    int num_samples = 20;
+
+    for (int i = 0; i < num_samples; ++i) {
+        double t = i / static_cast<double>(num_samples - 1);
+        rj_geometry::Point shot_target = left_goal_post + (right_goal_post - left_goal_post) * t;
+
+        double distance =
+            std::abs((enemy_goalie_location - ball_position).cross(shot_target - ball_position)) /
+            (shot_target - ball_position).mag();
+
+        if (distance > best_distance) {
+            best_distance = distance;
+            best_shot = shot_target;
+        }
+    }
+
+    SPDLOG_INFO("Free Kicker {} shooting at point {}, {}", this->robot_id_, best_shot.x(),
+                best_shot.y());
+
+    planning::LinearMotionInstant target{best_shot};
     auto line_kick_cmd = planning::MotionCommand{"line_kick", target};
     intent.motion_command = line_kick_cmd;
 

src/rj_strategy/src/agent/position/offense.cpp

@@ -15,7 +15,7 @@ std::optional<RobotIntent> 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);
         }