Check out the wall chucker

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

@@ -20,7 +20,7 @@
     return std::string{"Defense"} + std::to_string(static_cast<int>(current_state_));
 }
 
 Defense::State Defense::update_state() {
     State next_state = current_state_;
     // handle transitions between states
     WorldState* world_state = last_world_state_;
@@ -36,15 +36,19 @@
     }
 
     switch (current_state_) {
-        case IDLING:
+        case IDLING: {
             break;
-        case JOINING_WALL:
+        }
+
+        case JOINING_WALL: {
             send_join_wall_request();
             // SPDLOG_INFO("join wall {}", robot_id_);
             next_state = WALLING;
             walling_robots_ = {(u_int8_t)robot_id_};
             break;
-        case WALLING:
+        }
+
+        case WALLING: {
             // If a wall is already full,
             // Remove the robot with the highest ID from a wall
             // and make them a marker instead.
@@ -54,43 +58,76 @@
                 // SPDLOG_INFO("leave wall {}", robot_id_);
                 next_state = ENTERING_MARKING;
             }
+            // wall stealing
+            if (can_steal_ball()) {
+                send_leave_wall_request();
+                next_state = STEALING;
+            }
             break;
-        case SEARCHING:
+        }
+
+        case SEARCHING: {
             break;
-        case RECEIVING:
+        }
+
+        case RECEIVING: {
             // transition to idling if we are close enough to the ball
             if (distance_to_ball < ball_receive_distance_) {
                 next_state = IDLING;
             }
             break;
-        case PASSING:
+        }
+
+        case PASSING: {
             // transition to idling if we no longer have the ball (i.e. it was passed or it was
             // stolen)
-            if (check_is_done()) {
-                next_state = IDLING;
-            }
-
-            if (distance_to_ball > ball_lost_distance_) {
-                next_state = IDLING;
+            if (check_is_done() || distance_to_ball > ball_lost_distance_) {
+                next_state = JOINING_WALL;
             }
             break;
-        case FACING:
+        }
+
+        case FACING: {
             if (check_is_done()) {
                 next_state = IDLING;
             }
-        case MARKING:
+        }
+
+        case MARKING: {
             if (marker_.get_target() == -1 || marker_.target_out_of_bounds(world_state)) {
                 next_state = ENTERING_MARKING;
             }
             break;
-        case ENTERING_MARKING:
+        }
+
+        case ENTERING_MARKING: {
             marker_.choose_target(world_state);
             int target_id = marker_.get_target();
             if (target_id == -1) {
                 next_state = ENTERING_MARKING;
             } else {
                 next_state = MARKING;
             }
+            break;
+        }
+
+        case STEALING: {  // wall steal
+            // Go to passing if successful
+            if (check_is_done()) {
+                send_leave_wall_request();
+                next_state = CLEARING;
+            }
+
+            if (!can_steal_ball()) {
+                next_state = JOINING_WALL;
+            }
+        }
+
+        case CLEARING: {
+            if (check_is_done()) {
+                next_state = JOINING_WALL;
+            }
+        }
     }
 
     return next_state;
@@ -164,6 +201,22 @@
     } else if (current_state_ == MARKING) {
         // Marker marker = Marker((u_int8_t) robot_id_);
         return marker_.get_task(intent, last_world_state_, this->field_dimensions_);
+    } else if (current_state_ == STEALING) {  // wall stealer
+        auto collect_cmd = planning::MotionCommand{"collect"};
+        intent.motion_command = collect_cmd;
+        intent.dribbler_speed = 255.0;
+        return intent;
+    } else if (current_state_ == CLEARING) {
+        planning::LinearMotionInstant target{clear_point_};
+        auto line_kick_cmd = planning::MotionCommand{"line_kick", target};
+        intent.motion_command = line_kick_cmd;
+        intent.shoot_mode = RobotIntent::ShootMode::CHIP;
+        intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;
+        intent.kick_speed = 4.0;
+        intent.dribbler_speed = 255.0;
+        intent.is_active = true;
+
+        return intent;
     }
 
     return std::nullopt;

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

@@ -44,6 +44,8 @@
     void revive() override;
 
 private:
+    const rj_geometry::Point clear_point_{0.0, 4.5};
+
     // static constexpr int kMaxWallers{6};
     static constexpr int kMaxWallers{
         static_cast<int>(kNumShells)};  // This effectively turns off marking
@@ -69,6 +71,8 @@
         FACING,            // turning to face the passing robot
         MARKING,           // Following closely to an offense robot
         ENTERING_MARKING,  // Choosing/waiting for a robot to mark
+        STEALING,          // wall stealing
+        CLEARING,
     };
 
     State update_state();
@@ -85,6 +89,67 @@
      */
     void send_leave_wall_request();
 
+    /**
+     * @return if the current state has timed out
+     * wall steal
+     */
+    /*bool timed_out() const {
+        // Defined here so it can be inlined
+        using namespace std::chrono_literals;
+
+
+        return (max_time > 0s) && (last_time_ + max_time < RJ::now());
+    };*/
+
+    /**
+     * @return distance from this agent to ball
+     * wall steal
+     */
+    double distance_to_ball() const {
+        return last_world_state_->ball.position.dist_to(
+            last_world_state_->get_robot(true, robot_id_).pose.position());
+    };
+
+    /**
+     * @brief This FSM has timeouts for certain states.
+     * Ideally, these would not be necessary; as planners get more sophisticated
+     * they should not get "stuck". However, empirically, the offense FSM in particular
+     * has been observed to deadlock often.
+     *
+     * One common case is when waiting for a receiver to accept a pass; if no receiver responds,
+     * the timeout is necessary. In the future receivers may be able to respond in the negative
+     * instead of ignoring the request.
+     *
+     * The timeouts are a safety mechanism, and should not be the primary reason for a
+     * state transition. They are set relatively high for this reason.
+     *
+     * @return the maximum duration to stay in a given state, or -1 if there is no maximum.
+     *
+     */
+    static constexpr RJ::Seconds timeout(State s) {
+        switch (s) {
+            case PASSING:
+                return RJ::Seconds{5};
+            case STEALING:
+                return RJ::Seconds{10};
+        }
+    }
+
+    /**
+     * @return if the current state has timed out
+     */
+    bool timed_out() const {
+        // Defined here so it can be inlined
+        using namespace std::chrono_literals;
+
+        const auto max_time = timeout(current_state_);
+
+        return (max_time > 0s) && (last_time_ + max_time < RJ::now());
+    };
+
+    // The time at which the last state started.
+    RJ::Time last_time_;
+
     /**
      * @brief Adds the new waller to this robot's list of wallers and updates this robot's position
      * in the wall.

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

@@ -268,7 +268,6 @@ std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
             auto collect_cmd = planning::MotionCommand{"collect"};
             intent.motion_command = collect_cmd;
             intent.dribbler_speed = 255.0;
-            // }
 
             return intent;
         }
@@ -521,48 +520,6 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry:
     return min_angle;
 }
 
-bool Offense::can_steal_ball() const {
-    // Ball in red zone or not
-    if (ball_in_red()) {
-        return false;
-    }
-    // Ball location
-    rj_geometry::Point ball_position = this->last_world_state_->ball.position;
-
-    // Our robot is closest robot to ball
-    bool closest = true;
-
-    auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position();
-
-    auto our_dist = (current_pos - ball_position).mag();
-    for (auto enemy : this->last_world_state_->their_robots) {
-        auto dist = (enemy.pose.position() - ball_position).mag();
-        if (dist < our_dist) {
-            closest = false;
-            break;
-        }
-    }
-
-    if (!closest) {
-        return closest;
-    }
-
-    for (auto pal : this->last_world_state_->our_robots) {
-        // if (pal.robot_id_ == robot_id_) {
-        // continue;
-        // }
-        auto dist = (pal.pose.position() - ball_position).mag();
-        if (dist < our_dist) {
-            closest = false;
-            break;
-        }
-    }
-
-    return closest;
-
-    // return distance_to_ball() < kStealBallRadius;
-}
-
 rj_geometry::Point Offense::calculate_best_shot() const {
     // Goal location
     rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc();
@@ -587,13 +544,6 @@ rj_geometry::Point Offense::calculate_best_shot() const {
     return best_shot;
 }
 
-// Checks whether ball is out of range for stealing/receiving
-bool Offense::ball_in_red() const {
-    auto& ball_pos = last_world_state_->ball.position;
-    return (field_dimensions_.our_defense_area().contains_point(ball_pos) ||
-            field_dimensions_.their_defense_area().contains_point(ball_pos) ||
-            !field_dimensions_.field_rect().contains_point(ball_pos));
-}
 void Offense::broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding) {
     communication::SeekerRequest seeker_request{};
     communication::generate_uid(seeker_request);

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

@@ -210,10 +210,10 @@ class Offense : public Position {
      */
     double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const;
 
-    /**
-     * @brief Check if this agent could easily steal the ball
-     */
-    bool can_steal_ball() const;
+    // /**
+    //  * @brief Check if this agent could easily steal the ball
+    //  */
+    // bool can_steal_ball() const;
 
     /**
      * @return distance from this agent to ball
@@ -233,10 +233,10 @@ class Offense : public Position {
      */
     rj_geometry::Point calculate_best_shot() const;
 
-    /**
-     * @return whether the ball is in an area that non-goalies cannot reach.
-     */
-    bool ball_in_red() const;
+    // /**
+    //  * @return whether the ball is in an area that non-goalies cannot reach.
+    //  */
+    // bool ball_in_red() const;
 
     void broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding);
 

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

@@ -286,4 +286,54 @@ communication::Acknowledge Position::acknowledge_ball_in_transit(
     return acknowledge_response;
 }
 
+// Checks whether ball is out of range for stealing/receiving
+bool Position::ball_in_red() const {
+    auto& ball_pos = last_world_state_->ball.position;
+    return (field_dimensions_.our_defense_area().contains_point(ball_pos) ||
+            field_dimensions_.their_defense_area().contains_point(ball_pos) ||
+            !field_dimensions_.field_rect().contains_point(ball_pos));
+}
+
+bool Position::can_steal_ball() const {
+    // Ball in red zone or not
+    if (ball_in_red()) {
+        return false;
+    }
+    // Ball location
+    rj_geometry::Point ball_position = this->last_world_state_->ball.position;
+
+    // Our robot is closest robot to ball
+    bool closest = true;
+
+    auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position();
+
+    auto our_dist = (current_pos - ball_position).mag();
+    for (auto enemy : this->last_world_state_->their_robots) {
+        auto dist = (enemy.pose.position() - ball_position).mag();
+        if (dist < our_dist) {
+            closest = false;
+            break;
+        }
+    }
+
+    if (!closest) {
+        return closest;
+    }
+
+    for (auto pal : this->last_world_state_->our_robots) {
+        // if (pal.robot_id_ == robot_id_) {
+        // continue;
+        // }
+        auto dist = (pal.pose.position() - ball_position).mag();
+        if (dist < our_dist) {
+            closest = false;
+            break;
+        }
+    }
+
+    return closest;
+
+    // return distance_to_ball() < kStealBallRadius;
+}
+
 }  // namespace strategy