state_machine.H

Go to the documentation of this file.

// Copyright (C) 2006 University of Louisiana at Lafayette
// Authors: Suresh Golconda, Pablo Mejia

#ifndef CAJUN_STATE_MACHINE_H
#define CAJUN_STATE_MACHINE_H

#include "base_state_machine.H"
#include "simple_states.H"
#include "task_plan_handler.H"

#include "world_state.H"
#include "rndf.H"
#include "task_plan_handler.H"
#include "mission_handler.H"

#include "uc_planner_param.H"

namespace cajun
{
class base_state_machine_t;

class state_machine_t
{
        // Is bot stopped
        bool m_bot_stop;

        // Time at which bot stopped
        double m_bot_stop_time;

        base_state_machine_t *m_state;

        task_plan_handler_t *m_tp_handler;
        mission_handler_t *m_mission_handler;

        tp_plan_t *m_tp_plan;

        world_state_t *m_ws;
        rndf_t const *m_rndf;
        mdf_t const *m_mdf;

        //fixme: This is not the best way of doing, but doing this way for now
        //for connect all modules and system going. Will probably need
        //re-factoring (Suresh)

        // blockage info
        bool m_tp_blocked;

        blocked_lane_data_t m_blk_data;
        unsigned m_blk_tp_ind;


        unsigned m_path_blk_ind;

        unsigned m_LA_tp_ind;
        unsigned m_LA_path_ind;

        double m_LA_blk_x; // record the look ahead point before blk
        double m_LA_blk_y;

        unsigned m_LA_blk_sid;
        unsigned m_LA_blk_lid;
        unsigned m_LA_blk_wid;

        const uc_planner_param_t *m_uc_param;
public:
        state_machine_t (task_plan_handler_t *tp_handler_,
                         mission_handler_t *mission_handler_,
                         tp_plan_t *tp_plan_, world_state_t *ws_,
                         rndf_t const *rndf_, mdf_t const *mdf_);

        bool update_state ();

        // --------------------------------------------------------------
        // ---------- State Machine, condition check Functions ----------

//      bool did_bot_stopped_for (double for_tstamp) const;

        bool is_bot_stopped () const { return m_bot_stop;}


        bool lane_blocked () const; //fixme: which lane?

        void tp_blocked (blocked_lane_data_t const &blk_data_,
                         unsigned blk_tp_ind_, unsigned path_blk_ind_,
                         unsigned LA_tp_ind_, unsigned LA_path_ind_);

        bool active_TP_blocked () const;
        bool tp_blocked_is_LF () const;


        bool drove_to_blk_safety_distance () const;

        bool blk_before_stop_intersection () const;

        bool blk_after_stop_intersection () const;

        bool blk_at_intersection () const;

        // Related to safety region around stop intersection
        bool blk_in_safety_region () const;

        bool blk_just_before_stop_intersection () const;

        // blockage in general
        bool no_blockage () const;


        bool completed_CL () const;

        bool completed_PL () const;

        bool completed_UT () const;

        bool lane_for_CL (unsigned &change_lid_) const;

        bool passing_lane_exist (unsigned &passing_lid_) const;

        bool enough_passing_lane_exist (unsigned passing_lid_) const;

        bool passing_lane_blocked (unsigned passing_lid_) const;

        bool enough_uturn_region (unsigned passing_lid_) const;

        bool uturn_region_blocked (unsigned lane_id_) const;

        bool uturn_takes_to_next_checkpoint (unsigned passing_lid_) const;

        // ------------------------------------------------------------
        // ---------- State Machine, Action Functions -----------------
        void change_lane ();

        void pass_lane ();

        bool set_road_blocked ();

        void uturn_lane ();
        bool nudge_path ()
                { return m_uc_param->m_nudge_path;}
private:
        void update_bot_stopped ();


        bool changing_lane_on_one_side (
                unsigned sid_, unsigned start_lid_, unsigned end_lid_,
                int del_lid_, double x_, double y_, unsigned &best_lid_,
                rndf_t::lane_t::direction_t direction_)
                const ;


        bool switch_lane_possible (
                unsigned sid_, unsigned org_lid_, unsigned new_lid_,
                double x_, double y_, double &ch_x_, double &ch_y) const;


        void create_new_state (state_machine_id_t state_id_);

        bool rndf_point_after_uturn (
                rndf_point_t* &start_wp_id,
                unsigned sid_, unsigned new_lid, double x_, double y_)
                const;

        void point_after_blk (unsigned &aft_blk_sid_,
                              unsigned &aft_blk_lid_,
                              unsigned &aft_blk_wid_,
                              double &aft_blk_x_, double &aft_blk_y_);

        void start_new_mission_from (rndf_point_t *start_rp_);


};
};



#endif

---