penalty_kick_fsm.h

#pragma once
 
#include "software/ai/hl/stp/tactic/dribble/dribble_fsm.h"
#include "software/ai/hl/stp/tactic/get_behind_ball/get_behind_ball_fsm.h"
#include "software/ai/hl/stp/tactic/kick/kick_fsm.h"
#include "software/ai/hl/stp/tactic/move/move_fsm.h"
#include "software/ai/hl/stp/tactic/tactic.h"
#include "software/geom/algorithms/closest_point.h"
#include "software/geom/algorithms/intersection.h"
 
struct PenaltyKickFSM
{
    PenaltyKickFSM();
 
    struct ControlParams
    {
    };
 
    DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS
 
    static bool evaluatePenaltyShot(std::optional<Robot> enemy_goalie, Field field,
                                    Point ball_position, Robot robot);
 
    static const Point evaluateNextShotPosition(std::optional<Robot> enemy_goalie,
                                                Field field);
 
    void shoot(const Update &event,
               boost::sml::back::process<KickFSM::Update> processEvent);
 
    void updateApproachKeeper(const Update &event,
                              boost::sml::back::process<DribbleFSM::Update> processEvent);
 
    void adjustOrientationForShot(
        const Update &event, boost::sml::back::process<DribbleFSM::Update> processEvent);
 
    bool takePenaltyShot(const Update &event);
 
    bool timeOutApproach(const Update &event);
 
 
    auto operator()()
    {
        using namespace boost::sml;
 
        DEFINE_SML_STATE(DribbleFSM)
        DEFINE_SML_STATE(KickFSM)
 
        DEFINE_SML_EVENT(Update)
 
        DEFINE_SML_GUARD(takePenaltyShot)
        DEFINE_SML_GUARD(timeOutApproach)
 
        DEFINE_SML_SUB_FSM_UPDATE_ACTION(shoot, KickFSM)
        DEFINE_SML_SUB_FSM_UPDATE_ACTION(updateApproachKeeper, DribbleFSM)
        DEFINE_SML_SUB_FSM_UPDATE_ACTION(adjustOrientationForShot, DribbleFSM)
 
        return make_transition_table(
            // src_state + event [guard] / action = dest state
            *DribbleFSM_S + Update_E[!takePenaltyShot_G] / updateApproachKeeper_A,
            DribbleFSM_S + Update_E[timeOutApproach_G] / shoot_A = KickFSM_S,
            DribbleFSM_S + Update_E / adjustOrientationForShot_A,
            DribbleFSM_S = KickFSM_S, KickFSM_S + Update_E / shoot_A, KickFSM_S = X,
            X + Update_E / SET_STOP_PRIMITIVE_ACTION = X);
    };
 
   private:
    static constexpr double PENALTY_KICK_POST_OFFSET = 0.03;
    static constexpr double PENALTY_KICK_SHOT_SPEED  = 5.0;
 
    // expected maximum acceleration of the opposition goalie robot
    static constexpr double PENALTY_KICK_GOALIE_MAX_ACC = 1.5;
    static constexpr double SSL_VISION_DELAY            = 0.30;  // seconds
 
    // the fraction of the enemy side of the field that we define to be the lower limit of
    // where we choose to shoot in other words, it helps define the minimum distance at
    // which we decide to potentially shoot
    static constexpr double PENALTY_KICK_MIN_SHOT_X_DISTANCE_FACTOR = 1.0 / 3.0;
 
    // timeout that forces a shot after the robot approaches the ball and advances
    // towards the keeper
    // these two timeouts together must be <= 9 seconds
    static const inline Duration PENALTY_FORCE_SHOOT_TIMEOUT = Duration::fromSeconds(4);
    static const inline Duration PENALTY_FINISH_APPROACH_TIMEOUT =
        Duration::fromSeconds(4);
 
   private:
    std::optional<Timestamp> complete_approach;
    Angle shot_angle;
};