zone_task_planner.H

Go to the documentation of this file.

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

#ifndef CAJUN_ZONE_TASK_PLANNER_H
#define CAJUN_ZONE_TASK_PLANNER_H


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

#include "zone_planner.H"


namespace cajun
{


class zone_task_planner_t : public task_planner_t
{
public:
        zone_task_planner_t (world_state_t *ws_, rndf_t const *rndf_);
        ~zone_task_planner_t ();

        void set_plan (rndf_point_t *start_rp_, rndf_point_t *end_rp_);
        void get_plan (rndf_point_t* &start_rp_, rndf_point_t* &end_rp_);
        void modify_plan (rndf_point_t *start_rp_, rndf_point_t *end_rp_);

        void set_active ();

        path_status_t generate_path (
                world_state_t *ws_, path_t &path_, double &p_len_left_,
                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 ();

protected:
        const uc_planner_param_t *m_uc_param;

        world_state_t *m_ws;
        rndf_t const *m_rndf;

        //fixme: change it to const
        rndf_point_t *m_start_rp;
        rndf_point_t *m_end_rp;
        unsigned m_zone_id;

        bool m_first_iteration;
        double m_start_x;
        double m_start_y;

         // straight path to parking spot, no OA (obstacle avoidance)
        bool m_give_straigth_path;

        // True if have to generate path
        bool m_generate_path;

        // ---------------      Zones related ---------
        zone_planner_t *m_zone_planner;

        base_interpolator_t *m_interp;

        path_status_t straight_path (world_state_t *ws_,
                                     path_t &path_,
                                     double &p_len_left_,
                                     const path_t &base_path_,
                                     double min_speed_,
                                     double max_speed_);

//      void compute_end_point (base_interpolator_t::waypoint_t &end_p_);

        unsigned find_point_on_path (base_interpolator_t::uniform_path_t &path_,
                                     double x_, double y_);

        bool is_path_good (path_t const &path_, unsigned zone_id_,
                           unsigned &path_index_);

        void update_start_point (path_t const &path_);

        void get_from_local_path (path_t const &base_path_, path_t &path_);

        bool is_path_good ();

        void generate_new_path (world_state_t *ws_,
                                path_t &path_,
                                double &p_len_left_,
                                const path_t &base_path_);

        void cut_down_path (path_t const &base_path_, path_t &path_);
        double start_orient (path_t const &path_);
};


};


#endif

---