ball_placement_play_fsm.h

#pragma once
 
#include "proto/parameters.pb.h"
#include "shared/constants.h"
#include "software/ai/hl/stp/play/play_fsm.h"
#include "software/ai/hl/stp/tactic/dribble/dribble_tactic.h"
#include "software/ai/hl/stp/tactic/move/move_tactic.h"
#include "software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h"
#include "software/ai/passing/eighteen_zone_pitch_division.h"
 
 
using Zones = std::unordered_set<EighteenZoneId>;
 
struct BallPlacementPlayFSM
{
    class StartState;
    class KickOffWallState;
    class AlignPlacementState;
    class PlaceBallState;
    class WaitState;
    class RetreatState;
 
    struct ControlParams
    {
    };
 
    DEFINE_PLAY_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS
 
    explicit BallPlacementPlayFSM(TbotsProto::AiConfig ai_config);
 
    void kickOffWall(const Update& event);
 
    void alignPlacement(const Update& event);
 
    void placeBall(const Update& event);
 
    void startWait(const Update& event);
 
    void retreat(const Update& event);
 
    bool shouldKickOffWall(const Update& event);
 
    bool alignDone(const Update& event);
 
    bool kickDone(const Update& event);
 
    bool ballPlaced(const Update& event);
 
    bool waitDone(const Update& event);
 
    bool retreatDone(const Update& event);
 
    Angle calculateWallKickoffAngle(const Point& ball_pos, const Rectangle& field_lines);
 
    void setupMoveTactics(const Update& event);
 
    auto operator()()
    {
        using namespace boost::sml;
 
        DEFINE_SML_STATE(StartState)
        DEFINE_SML_STATE(KickOffWallState)
        DEFINE_SML_STATE(AlignPlacementState)
        DEFINE_SML_STATE(PlaceBallState)
        DEFINE_SML_STATE(WaitState)
        DEFINE_SML_STATE(RetreatState)
        DEFINE_SML_EVENT(Update)
 
        DEFINE_SML_ACTION(alignPlacement)
        DEFINE_SML_ACTION(placeBall)
        DEFINE_SML_ACTION(kickOffWall)
        DEFINE_SML_ACTION(startWait)
        DEFINE_SML_ACTION(retreat)
 
        DEFINE_SML_GUARD(shouldKickOffWall)
        DEFINE_SML_GUARD(alignDone)
        DEFINE_SML_GUARD(kickDone)
        DEFINE_SML_GUARD(ballPlaced)
        DEFINE_SML_GUARD(waitDone)
        DEFINE_SML_GUARD(retreatDone)
 
        return make_transition_table(
            // src_state + event [guard] / action = dest_state
            *StartState_S + Update_E[!shouldKickOffWall_G] / alignPlacement_A =
                AlignPlacementState_S,
            StartState_S + Update_E[shouldKickOffWall_G] = KickOffWallState_S,
            KickOffWallState_S + Update_E[!kickDone_G && shouldKickOffWall_G] /
                                     kickOffWall_A                = KickOffWallState_S,
            KickOffWallState_S + Update_E[kickDone_G]             = KickOffWallState_S,
            KickOffWallState_S + Update_E[!kickDone_G]            = AlignPlacementState_S,
            AlignPlacementState_S + Update_E[shouldKickOffWall_G] = KickOffWallState_S,
            AlignPlacementState_S + Update_E[!alignDone_G] / alignPlacement_A =
                AlignPlacementState_S,
            AlignPlacementState_S + Update_E[alignDone_G]            = PlaceBallState_S,
            PlaceBallState_S + Update_E[!ballPlaced_G] / placeBall_A = PlaceBallState_S,
            PlaceBallState_S + Update_E[ballPlaced_G] / startWait_A  = WaitState_S,
            WaitState_S + Update_E[!waitDone_G]                      = WaitState_S,
            WaitState_S + Update_E[waitDone_G]                       = RetreatState_S,
            RetreatState_S + Update_E[retreatDone_G && ballPlaced_G] = X,
            RetreatState_S + Update_E[ballPlaced_G] / retreat_A      = RetreatState_S);
    }
 
   private:
    TbotsProto::AiConfig ai_config;
    std::shared_ptr<WallKickoffTactic> pivot_kick_tactic;
    std::shared_ptr<PlaceBallTactic> place_ball_tactic;
    std::shared_ptr<MoveTactic> align_placement_tactic;
    std::shared_ptr<MoveTactic> retreat_tactic;
    std::vector<std::shared_ptr<PlaceBallMoveTactic>> move_tactics;
    Point setup_point;
    Timestamp start_time;
    constexpr static double const WALL_KICKOFF_VELOCITY_M_PER_S   = 3.0;
    constexpr static double const RETREAT_DISTANCE_METERS         = 0.6;
    constexpr static double const PLACEMENT_DIST_THRESHOLD_METERS = 0.15;
    constexpr static double const BALL_IS_PLACED_WAIT_S           = 3.0;
};