passing_lane_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_PASSING_LANE_TASK_PLANNER_H
#define CAJUN_PASSING_LANE_TASK_PLANNER_H

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

namespace cajun
{

class passing_lane_task_planner_t : public task_planner_t
{
public:
        passing_lane_task_planner_t ();

        void set_plan (unsigned sid, unsigned lid_1_, unsigned lid_2_,
                       double x_, double y_, double b_dis_);

        void set_path_blocked (unsigned p_id_);

        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 get_path_map_to_lane_lids (
                std::vector<path_lane_map_t> &pl_ids_);

        bool get_start_lid (unsigned &sid_, unsigned &lid_);

        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;

        // Lane width multiplier to compute allowed xtrack distance to reach a
        // wp and to be next to a lane
        double m_width_multiplier;
        double m_look_ahead_ratio;
        double m_dis_from_blk;
        double m_dis_after_blk;

        double m_stop_time;
        bool m_stopped;

        bool m_switch_pass_lane;  //switch to passing lane
        bool m_drive_on_passing_lane; // driving on passing lane
        bool m_switch_orig_lane; // switching back nto original lane

        unsigned m_sid;
        unsigned m_lid;
        unsigned m_passing_lid;

        double m_blk_x;
        double m_blk_y;
        double m_blk_dis;

        void compute_end_passing_points (
                rndf_lane_data_t &lb_, double &end_pass_x, double &end_pass_y);


        void compute_end_passing_points (
                world_state_t *ws_, rndf_lane_data_t &lb_, double pres_x_,
                double pres_y_, double &end_pass_x_, double &end_pass_y_);

        bool blockage_just_after (
                world_state_t *ws_,
                blocked_lane_data_t &blk_, double x_, double y_,
                unsigned sid_,
                unsigned lid_, unsigned &wid_, double dis_);

        void blk_end_points (
                world_state_t *ws_,
                blocked_lane_data_t const &blk_, double &x_, double &y_,
                unsigned &wid_);

        bool blockage_just_before (
                world_state_t *ws_,
                blocked_lane_data_t &blk_, double x_, double y_,
                unsigned sid_,
                unsigned lid_, unsigned &wid_, double dis_);

        void get_blk_around (
                world_state_t *ws_, double x_, double y_,
                std::vector<blocked_lane_data_t> &blk_list_, double dis_);


        bool closest_blk_before_point (
                world_state_t *ws_,
                std::vector <blocked_lane_data_t> const &blk_list,
                blocked_lane_data_t &best_blk_,
                unsigned sid_1_, unsigned lid_1_, unsigned wid_1_,
                unsigned sid_2_, unsigned lid_2_, unsigned wid_2_,
                double x, double y, double &dis_);

//      void compute_end_passing_points (rndf_lane_data_t &lb_,
//                                       double &end_pass_x,
//                                       double &end_pass_y);

        void drive_on_passing_lane (world_state_t *ws_,
                                    rndf_lane_data_t &passing_lb_,
                                    path_t &path_, double &local_p_len_left_);

        bool stop_before_passing (world_state_t *ws_);

        bool closest_blk_after_point (
                world_state_t *ws_,
                std::vector <blocked_lane_data_t> const &blk_list,
                blocked_lane_data_t &best_blk_,
                unsigned sid_1_, unsigned lid_1_, unsigned wid_1_,
                unsigned sid_2_, unsigned lid_2_, unsigned wid_2_,
                double x, double y, double &dis_);

        double distance_to_end_of_lane (
                world_state_t *ws_,
                unsigned sid1_, unsigned lid1_, unsigned wid1_,
                unsigned sid2_, unsigned lid2_, unsigned wid2_,
                double x_, double y_);

        double distance_to_beginning_of_lane (
                world_state_t *ws_,
                unsigned sid1_, unsigned lid1_, unsigned wid1_,
                unsigned sid2_, unsigned lid2_, unsigned wid2_,
                double x_, double y_);


};

};


#endif

---