goalie_fsm.h

#pragma once
 
#include "proto/parameters.pb.h"
#include "shared/constants.h"
#include "software/ai/evaluation/calc_best_shot.h"
#include "software/ai/hl/stp/tactic/chip/chip_fsm.h"
#include "software/ai/hl/stp/tactic/pivot_kick/pivot_kick_fsm.h"
#include "software/ai/hl/stp/tactic/tactic.h"
#include "software/geom/algorithms/calculate_block_cone.h"
#include "software/geom/algorithms/closest_point.h"
#include "software/geom/algorithms/contains.h"
#include "software/geom/algorithms/convex_angle.h"
#include "software/geom/algorithms/intersection.h"
#include "software/geom/line.h"
 
 
struct GoalieFSM
{
   public:
    class Panic;
    class PositionToBlock;
    class MoveToGoalLine;
 
    struct ControlParams
    {
        bool should_move_to_goal_line;
    };
 
    DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS
 
    // Distance to chip the ball when trying to yeet it
    // TODO (#1878): Replace this with a more intelligent chip distance system
    static constexpr double YEET_CHIP_DISTANCE_METERS = 2.0;
 
    explicit GoalieFSM(
        TbotsProto::GoalieTacticConfig goalie_tactic_config,
        TbotsProto::RobotNavigationObstacleConfig robot_navigation_obstacle_config,
        TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode)
        : goalie_tactic_config(goalie_tactic_config),
          robot_navigation_obstacle_config(robot_navigation_obstacle_config),
          max_allowed_speed_mode(max_allowed_speed_mode),
          robot_radius_expansion_amount(
              ROBOT_MAX_RADIUS_METERS *
              robot_navigation_obstacle_config.robot_obstacle_inflation_factor())
    {
    }
 
    static Point getGoaliePositionToBlock(
        const Ball &ball, const Field &field,
        TbotsProto::GoalieTacticConfig goalie_tactic_config);
 
    static std::vector<Point> getIntersectionsBetweenBallVelocityAndFullGoalSegment(
        const Ball &ball, const Field &field);
 
    static Rectangle getNoChipRectangle(const Field &field);
 
    static Point findGoodChipTarget(
        const World &world, const TbotsProto::GoalieTacticConfig &goalie_tactic_config);
 
    bool shouldEvacuateCrease(const Update &event);
 
    bool shouldPanic(const Update &event);
 
    bool shouldMoveToGoalLine(const Update &event);
 
    bool shouldPivotChip(const Update &event);
 
    bool panicDone(const Update &event);
 
    void panic(const Update &event);
 
    bool retrieveDone(const Update &event);
 
    void retrieveFromDeadZone(const Update &event,
                              boost::sml::back::process<DribbleFSM::Update> processEvent);
 
    void moveToGoalLine(const Update &event);
 
    void updatePivotKick(const Update &event,
                         boost::sml::back::process<PivotKickFSM::Update> processEvent);
 
    void positionToBlock(const Update &event);
 
    bool ballInInflatedDefenseArea(const Update &event);
 
    auto operator()()
    {
        using namespace boost::sml;
 
        DEFINE_SML_STATE(Panic)
        DEFINE_SML_STATE(PivotKickFSM)
        DEFINE_SML_STATE(PositionToBlock)
        DEFINE_SML_STATE(MoveToGoalLine)
        DEFINE_SML_STATE(DribbleFSM)
 
        DEFINE_SML_EVENT(Update)
 
        DEFINE_SML_GUARD(ballInInflatedDefenseArea)
        DEFINE_SML_GUARD(panicDone)
        DEFINE_SML_GUARD(shouldEvacuateCrease)
        DEFINE_SML_GUARD(shouldPivotChip)
        DEFINE_SML_GUARD(shouldPanic)
        DEFINE_SML_GUARD(shouldMoveToGoalLine)
        DEFINE_SML_GUARD(retrieveDone)
 
        DEFINE_SML_ACTION(panic)
        DEFINE_SML_ACTION(positionToBlock)
        DEFINE_SML_ACTION(moveToGoalLine)
        DEFINE_SML_SUB_FSM_UPDATE_ACTION(updatePivotKick, PivotKickFSM)
        DEFINE_SML_SUB_FSM_UPDATE_ACTION(retrieveFromDeadZone, DribbleFSM)
 
        return make_transition_table(
            // src_state + event [guard] / action = dest_state
            *PositionToBlock_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A =
                MoveToGoalLine_S,
            PositionToBlock_S +
                Update_E[shouldEvacuateCrease_G] / retrieveFromDeadZone_A = DribbleFSM_S,
            PositionToBlock_S + Update_E[shouldPanic_G] / panic_A         = Panic_S,
            PositionToBlock_S + Update_E[shouldPivotChip_G] / updatePivotKick_A =
                PivotKickFSM_S,
            PositionToBlock_S + Update_E / positionToBlock_A,
            DribbleFSM_S + Update_E[retrieveDone_G] / updatePivotKick_A = PivotKickFSM_S,
            DribbleFSM_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A =
                MoveToGoalLine_S,
            DribbleFSM_S + Update_E[ballInInflatedDefenseArea_G] / retrieveFromDeadZone_A,
            DribbleFSM_S + Update_E[!ballInInflatedDefenseArea_G] / positionToBlock_A =
                PositionToBlock_S,
            Panic_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A =
                MoveToGoalLine_S,
            Panic_S + Update_E[shouldPivotChip_G] / updatePivotKick_A = PivotKickFSM_S,
            Panic_S + Update_E[panicDone_G] / positionToBlock_A       = PositionToBlock_S,
            Panic_S + Update_E / panic_A,
            PivotKickFSM_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A =
                MoveToGoalLine_S,
            PivotKickFSM_S + Update_E[ballInInflatedDefenseArea_G] / updatePivotKick_A,
            PivotKickFSM_S + Update_E[!ballInInflatedDefenseArea_G] / positionToBlock_A =
                PositionToBlock_S,
            MoveToGoalLine_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A =
                MoveToGoalLine_S,
            MoveToGoalLine_S + Update_E[!shouldMoveToGoalLine_G] / positionToBlock_A =
                PositionToBlock_S,
            X + Update_E = X);
    }
 
   private:
    static constexpr double BALL_RETRIEVED_THRESHOLD = 0.2;
    // The goalie tactic config
    TbotsProto::GoalieTacticConfig goalie_tactic_config;
    // Configuration values for inflated obstacles
    TbotsProto::RobotNavigationObstacleConfig robot_navigation_obstacle_config;
    // The maximum allowed speed mode
    TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode;
    // Expansion factor for inflated obstacles
    double robot_radius_expansion_amount;
};