Has Open Shot (#2178)

Co-authored-by: sanatd33 <[email protected]>

soccer/src/soccer/strategy/agent/position/offense.cpp

@@ -94,7 +94,7 @@ Offense::State Offense::update_state() {
 
 std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
     float dist{0.0f};
-    SPDLOG_INFO(current_state_);
+    // SPDLOG_INFO(current_state_);
     if (current_state_ == IDLING) {
         // Do nothing
         auto empty_motion_cmd = planning::MotionCommand{};
@@ -215,6 +215,52 @@ std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
     return std::nullopt;
 }
 
+bool Offense::has_open_shot() {
+    // Goal location
+    rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc();
+    double goal_width = field_dimensions_.goal_width();  // 1.0 meters
+
+    // Ball location
+    rj_geometry::Point ball_position = this->last_world_state_->ball.position;
+
+    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;
+    for (int i = 0; i < 19; i++) {
+        double distance = distance_from_their_robots(ball_position, curr_point);
+        if (distance > best_distance) {
+            best_distance = distance;
+        }
+        curr_point = curr_point + increment;
+    }
+
+    return best_distance > max_receive_distance;
+}
+
+double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) {
+    rj_geometry::Point vec = head - tail;
+    auto their_robots = this->last_world_state_->their_robots;
+    double min_angle = -0.5;
+    for (auto enemy : their_robots) {
+        rj_geometry::Point enemy_vec = enemy.pose.position() - tail;
+        if (enemy_vec.dot(vec) < 0) {
+            continue;
+        }
+        auto projection = (enemy_vec.dot(vec) / vec.dot(vec));
+        enemy_vec = enemy_vec - (projection)*vec;
+        double distance = enemy_vec.mag();
+        if (distance < (kRobotRadius + kBallRadius)) {
+            return -1.0;
+        }
+        double angle = distance / projection;
+        if ((min_angle < 0) || (angle < min_angle)) {
+            min_angle = angle;
+        }
+    }
+    return min_angle;
+}
+
 void Offense::receive_communication_response(communication::AgentPosResponseWrapper response) {
     Position::receive_communication_response(response);
 

soccer/src/soccer/strategy/agent/position/offense.hpp

@@ -69,6 +69,10 @@ class Offense : public Position {
     bool scorer_ = false;
     bool last_scorer_ = false;
 
+    bool has_open_shot();
+
+    double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head);
+
     /**
      * @brief Send request to the other robots to see if this robot should be the scorer
      *