planner.H

Go to the documentation of this file.

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

#ifndef CAJUN_PLANNER_H
#define CAJUN_PLANNER_H

#include "data_type.H"
#include "rndf.H"
#include "world_state.H"

#include "path_generator.H"
#include "path_verifier.H"
#include "traffic_verifier.H"

#include "base_path_provider.H"
#include "task_plan_handler.H"
#include "mission_handler.H"

#include "uc_planner_param.H"
#include "planner_defs.H"

#include "state_machine.H"
#include "rndf_point.H"

#include <deque>


namespace cajun
{

class mdf_t;
class task_planner_t;

class planner_t
{
public:
        planner_t (mdf_t const *mdf_, rndf_t const *rndf_,
                   world_state_t *ws_);

        ~planner_t ();

        void init ();

        void get_steering_path (path_data_t &d_, path_data_t &base_path_);

        path_turn_info_t get_path_turn_info ();

        bool is_finished () { return m_mission_handler->is_finished ();}

        void get_mission_status (mission_status_data_t &ms_);

        void get_pres_lid (unsigned &sid_, unsigned &lid_);

protected:
        void get_path (std::deque <task_planner_t *> &task_plan_,
                       path_t &path, const path_t &b_path);

        bool get_path_direction (std::deque<task_planner_t *> &task_plan_);

        void update_path_flags (path_data_t &path_data_,
                                path_data_t &b_path_data_);

        bool update_waypoints_reached (double x_, double y_);

        bool ready_to_handle_blocked_lane (const path_t &b_path,
                                           unsigned blk_tp_ind_);
        const uc_planner_param_t *m_uc_param;

        // Time for which to wait before blocked point before confirming its
        // blocked. Note: This is added to compenstate lack for determining
        // static/dynamic lane blockage.
        double m_passing_lane_confirm_tstamp;


        mdf_t const *m_mdf;
        rndf_t const *m_rndf;
        path_generator_t *m_path_generator;

        path_verifier_t *m_path_verifier;
        traffic_verifier_t *m_traffic_verifier;

        base_path_provider_t *m_base_path_provider;
        task_plan_handler_t *m_tp_handler;
        mission_handler_t *m_mission_handler;

        std::deque<task_planner_t *> m_plan;
        mission_plan_t m_mission_plan;

        world_state_t *m_world_state;

        // State machines related
        state_machine_t *m_sm;

        // Base path related variables
        bool m_first_iteration;

        void update_task_planner_status (double x_, double y_,
                                         double heading_);

        void update_planner_state ();

        void set_final_speed_limit (path_t &path_);

        void get_base_path (path_t &path_);

        void reset_path_speed (path_t &path_);

        void adjust_path_speeds (path_t &path, const path_t &b_path);

        void limit_path_speed (std::deque <task_planner_t *> &task_plan_,
                               unsigned tp_ind_, unsigned path_ind_,
                               double speed_);

        bool analyze_path (path_t &b_path_);

        void convoy_dynamic_object (blocked_lane_data_t &blk_data,
                                    unsigned blk_tp_ind, unsigned path_blk_index,
                                    double blk_speed);
        double dis_along_path (double x1_, double y1_,
                               unsigned tp_ind_, unsigned path_ind_);
        void print_plan (
                std::deque<rndf_t::waypoint_id_t> const &detail_plan_,
                std::deque<rndf_t::waypoint_id_t> const &filtered_plan_);

        void update_plan (bool could_be_at_start_shoot_ = false);

        double distance_from_static_blk (
                std::deque<task_planner_t *> const &plan_,
                blocked_lane_data_t const &blk_,
                unsigned tp_ind_, unsigned path_ind_);

        double compute_convoy_dis (double speed_);

        bool convoy_for_dyn_blk (unsigned blk_tp_ind,
                                 blocked_lane_data_t const &blk);

};

};


#endif

---