dribble_fsm.h

#pragma once
 
#include "proto/parameters.pb.h"
#include "shared/constants.h"
#include "software/ai/evaluation/time_to_travel.h"
#include "software/ai/hl/stp/tactic/move/move_fsm.h"
#include "software/ai/hl/stp/tactic/tactic.h"
#include "software/ai/hl/stp/tactic/transition_conditions.h"
#include "software/geom/algorithms/contains.h"
#include "software/geom/algorithms/convex_angle.h"
#include "software/geom/algorithms/distance.h"
 
struct DribbleFSM
{
   public:
    class GetPossession;
    class Dribble;
    class LoseBall;
 
    explicit DribbleFSM(TbotsProto::DribbleTacticConfig dribble_tactic_config)
        : dribble_tactic_config(dribble_tactic_config)
    {
    }
 
    struct ControlParams
    {
        // The destination for dribbling the ball
        std::optional<Point> dribble_destination;
        // The final orientation to face the ball when finishing dribbling
        std::optional<Angle> final_dribble_orientation;
        // whether to allow excessive dribbling, i.e. more than 1 metre at a time
        bool allow_excessive_dribbling;
    };
 
    DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS
 
    static Point robotPositionToFaceBall(const Point &ball_position,
                                         const Angle &face_ball_angle,
                                         double additional_offset = 0.0);
 
    // TODO (#1968): Merge this functionality with findBestInterceptForBall in the
    // evaluation folder
    Point findInterceptionPoint(const Robot &robot, const Ball &ball, const Field &field);
 
    static Point getDribbleBallDestination(const Point &ball_position,
                                           std::optional<Point> dribble_destination);
 
    static Angle getFinalDribbleOrientation(
        const Point &ball_position, const Point &robot_position,
        std::optional<Angle> final_dribble_orientation);
 
    static std::tuple<Point, Angle> calculateNextDribbleDestinationAndOrientation(
        const Ball &ball, const Robot &robot,
        std::optional<Point> dribble_destination_opt,
        std::optional<Angle> final_dribble_orientation_opt);
 
    void getPossession(const Update &event);
 
    void dribble(const Update &event);
 
    void loseBall(const Update &event);
 
    bool havePossession(const Update &event);
 
    bool lostPossession(const Update &event);
 
    bool dribblingDone(const Update &event);
 
    bool shouldLoseBall(const Update &event);
 
    auto operator()()
    {
        using namespace boost::sml;
 
        DEFINE_SML_STATE(GetPossession)
        DEFINE_SML_STATE(Dribble)
        DEFINE_SML_STATE(LoseBall)
        DEFINE_SML_EVENT(Update)
        DEFINE_SML_GUARD(havePossession)
        DEFINE_SML_GUARD(lostPossession)
        DEFINE_SML_GUARD(dribblingDone)
        DEFINE_SML_GUARD(shouldLoseBall)
        DEFINE_SML_ACTION(loseBall)
        DEFINE_SML_ACTION(getPossession)
        DEFINE_SML_ACTION(dribble)
 
        return make_transition_table(
            // src_state + event [guard] / action = dest_state
            *GetPossession_S + Update_E[havePossession_G] / dribble_A = Dribble_S,
            GetPossession_S + Update_E[!havePossession_G] / getPossession_A,
            Dribble_S + Update_E[shouldLoseBall_G] / loseBall_A      = LoseBall_S,
            Dribble_S + Update_E[lostPossession_G] / getPossession_A = GetPossession_S,
            Dribble_S + Update_E[!dribblingDone_G] / dribble_A,
            Dribble_S + Update_E[dribblingDone_G] / dribble_A = X,
            LoseBall_S + Update_E[shouldLoseBall_G] / loseBall_A,
            LoseBall_S + Update_E[!shouldLoseBall_G] / getPossession_A = GetPossession_S,
            X + Update_E[lostPossession_G] / getPossession_A           = GetPossession_S,
            X + Update_E[!dribblingDone_G] / dribble_A                 = Dribble_S,
            X + Update_E / dribble_A                                   = X);
    }
 
   private:
    // the dribble tactic config
    TbotsProto::DribbleTacticConfig dribble_tactic_config;
};