intersection_task_planner.H

Go to the documentation of this file.

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

#ifndef CAJUN_INTERSECTION_TASK_PLANNER_H
#define CAJUN_INTERSECTION_TASK_PLANNER_H


#include "task_planner.H"
#include "uc_planner_param.H"
#include "rndf_point.H"

namespace cajun
{


class intersection_task_planner_t : public task_planner_t
{
public:
        enum internal_status_t
        {
                NOT_ACTIVE_YET, // Did not reach and stop at intersection
                STOP_AT_INTERSECTION, // Waiting at intersection
                READY_TO_GO, // Ready to drive through intersection
        };

        intersection_task_planner_t (world_state_t *ws_);

        void set_plan (rndf_point_t *exit_wp_, rndf_point_t *entry_wp_);

        void get_plan (rndf_point_t* &exit_wp_, rndf_point_t* &entry_wp_);

        void set_active ();

        path_status_t generate_path (
                world_state_t *ws_, path_t &path_, double &p_length_,
                const path_t &base_path_);

        bool get_path_lane_lids (unsigned &sid_,
                                std::vector<unsigned> &lids_);

        bool is_task_complete (world_state_t *ws_, double x_, double y_,
                               double heading_);

        void print_plan ();

        void set_path_blocked (unsigned p_id_);

        internal_status_t get_status ()
        {       return m_internal_status; }

        bool is_quick_acceleration_path ()
        { return true; }


protected:
        const uc_planner_param_t *m_uc_param;

        internal_status_t m_internal_status;

        double m_stopped; //true if bot came to stop at intersection
        double m_stop_time;

        double m_have_precedence_since;
        double m_inter_path_free_since;

        rndf_point_t* m_exit_point;
        rndf_point_t* m_entry_point;

        bool m_left_turn; //true - left turn, false - right turn
        // True if had to wait at the intersection, else false
        bool m_wait_at_intersection;

        // tstamp when reached intersection
        double m_inter_tstamp;
        // True if there are other stop signs at intersection
        bool m_is_stop_inter;
        unsigned m_inter_id;

        double m_lane_width;

        double m_min_inter_speed;
        double m_max_inter_speed;


        path_status_t add_intersection_path (world_state_t *ws_,
                                             path_t &path_,
                                             double &p_length_,
                                             const path_t &base_path_,
                                             double x_, double y_);

        bool stop_at_intersection (world_state_t *ws_);

        bool inter_precedence (world_state_t *ws_);

        bool stop_for_stop_sign (world_state_t *ws_);

        bool intersection_region_free (world_state_t *ws_, unsigned inter_id_);

//      bool path_free_between_wp (
//              world_state_t *ws_,
//              rndf_t::waypoint_t const &exit_wp,
//              rndf_t::waypoint_t const &etnry_wp);
};


};


#endif

---