mission_planner.H

Go to the documentation of this file.

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

#ifndef CAJUN_MISSION_PLANNER_H
#define CAJUN_MISSION_PLANNER_H

#include "rndf.H"
#include "graph.H"
#include "graph_search.H"

#include "uc_planner_param.H"
#include "rndf_point.H"
#include"world_state.H"

#include <queue>
#include <vector>

namespace cajun
{
typedef std::deque<rndf_point_t*> mission_plan_t;
typedef graph_t<rp_containter_t>::vertex_t graph_node_t;

class mdf_t;

class mission_planner_t
{
        friend class mission_handler_t;
private:
        class wp_node_t
        { //Container for wp_ids, this container will be used to sort all
          //the wps of a segment by their distance along the lane
        public:
                wp_node_t (double dist_, rndf_point_t *wp_id_) :
                        m_dist (dist_), m_wp_id (wp_id_) { }
                bool operator < (wp_node_t const &n_) const
                        { return (m_dist > n_.m_dist); }

                double dist () { return m_dist;}
                rndf_point_t* wp_id () {return m_wp_id; }
        private:
                double m_dist; //distance along ROAD, not lane
                rndf_point_t *m_wp_id;
        };

        const uc_planner_param_t *m_uc_param;

        rndf_t const *m_rndf;
        mdf_t const *m_mdf;
        world_state_t *m_ws;
        graph_t <rp_containter_t> m_graph;
        graph_search_t<rp_containter_t> *m_g_search;

        std::deque<rndf_point_t*> m_intermediate_rps;

        double distance_between_rps (rndf_point_t const *rp_1_,
                                     rndf_point_t const *rp_2_);

        double compute_cost (rndf_point_t *id_1_, rndf_point_t *id_2_);


        void get_lanes_to_merge_with (
                rndf_t::segment_t const &seg_,
                unsigned li_, std::vector<unsigned> &lanes_to_merge_);

        bool can_pass_between_lanes (
                rndf_t::segment_t const &seg_, unsigned li1_,
                unsigned li2_);


        void merge_lanes (unsigned sid_,
                          std::vector<unsigned> const &lane_to_merge,
                          std::deque<wp_node_t> &seg_wps);


        bool should_be_graph_node (rndf_point_t const *wp_id_);

        bool is_mission_checkpoint (rndf_point_t const *rp_);

        bool create_graph ();

        void add_zone_to_graph (rndf_t::zone_t const &zone_);

        void create_road_edges (std::deque <wp_node_t> &seg_wps);

        void create_edges_from_wp (std::deque <wp_node_t> &seg_wps_,
                                   unsigned wp_i);

        void create_edge (rndf_t::waypoint_id_t wp_id_,
                          rndf_t::waypoint_id_t neighbor_id_);

        void create_edge (rndf_point_t *rp, rndf_point_t *neigh_rp);

        bool edge_exist (graph_node_t *v1, graph_node_t *v2);

        bool not_close (std::deque <wp_node_t> &seg_wps_,
                        unsigned ind1_, unsigned ind2_);

        void print_wp_node (std::deque<wp_node_t> &seg_wps_,
                            unsigned wp_i);


        bool mission_spot (rndf_t::spot_t const &spot_);

//      void get_lanes_in_same_direction (
//              unsigned seg_id_, rndf_t::lane_t::direction_t dir_,
//              std::vector <unsigned> &similar_lanes_);

//      /*!\brief Geneates a priority queue with all wps of the segment
//        arranged by their distance from along the segment. The distance
//        starts from 1st wp of 1st lane. Distance of wps on lane 1 is equal to
//        distance along the lane, while distance of a wp on other adjacent
//        lanes is equal to the sum of the distance of the closest wp on lane 1
//        + atrack distance of present wp to this closest wp on lane 1 along
//        the orientation of the lane 1 at that waypoint */
//      void sort_seg_wps_by_distance (std::deque<wp_node_t> &seg_wps_,
//                                     unsigned seg_id_,
//                                     rndf_t::lane_t::direction_t dir_);
        bool lanes_in_same_direction (rndf_t::lane_t const &lane_,
                                      rndf_t::lane_t const &n_lane_);

        bool is_next_checkpoint (rndf_point_t *rp_, unsigned next_ckpt_);

        bool keep_as_individual_rp_in_plan (rndf_point_t *rp_,
                                            unsigned next_ckpt_);

        bool is_entry_exit_pair (rndf_point_t *exit_rp_,
                                 rndf_point_t *entry_rp_);

        bool is_entry_exit_pair_to_keep (rndf_point_t *exit_rp_,
                                         rndf_point_t *entry_rp_);

        void filter_detail_plan (
                mission_plan_t const &input_plan_,
                mission_plan_t &ouput_plan_,
                unsigned next_checkpoint_);

        bool is_active_node (graph_node_t const *gn);
        void get_inter_rp_in_wp_seg (
                rndf_t::waypoint_id_t const &wp_id_,
                std::vector<rndf_point_t *> rp_list_);
public:
        mission_planner_t (rndf_t const *rndf_, mdf_t const *mdf_,
                           world_state_t *ws_);

        bool update_plan (unsigned next_checkpoint_,
                          mission_plan_t &detail_plan_,
                          mission_plan_t &filter_plan_);

        graph_node_t* graph_node_at_wp (
                rndf_t::waypoint_id_t const &wp_id_);

        graph_node_t* graph_node_at_wp (unsigned sid_, unsigned lid_,
                                        unsigned wid_);

        graph_node_t* active_graph_node_on_or_before (
                unsigned sid_, unsigned lid_, unsigned wid_);

        graph_node_t* active_graph_node_on_or_after (
                unsigned sid_, unsigned lid_, unsigned wid_);

        graph_node_t* active_graph_node_on_or_before (
                rndf_point_t *rp_);

        graph_node_t* active_graph_node_after (rndf_point_t *rp_);

        graph_node_t* active_graph_node_before (
                double x_, double y_,
                rndf_t::waypoint_id_t const &before_wp_id);

        graph_node_t* graph_node_at_rp (rndf_point_t *rp_);

        bool comes_later_on_wp_seg (rndf_point_t *rp_,
                                    double x_, double y_);

        // Return interms of rndf_point
        rndf_point_t* rndf_point_at_wp (
                rndf_t::waypoint_id_t const &wp_id_);

        rndf_point_t* rndf_point_at_wp (unsigned sid_, unsigned lid_,
                                        unsigned wid_);


        rndf_point_t* active_rndf_point_on_or_before (
                unsigned sid_, unsigned lid_, unsigned wid_);

        rndf_point_t* active_rndf_point_on_or_after (
                unsigned sid_, unsigned lid_, unsigned wid_);

        rndf_point_t* active_rndf_point_before (rndf_point_t *rp_);
        rndf_point_t* active_rndf_point_after (rndf_point_t *rp_);

        rndf_point_t* active_rndf_point_on_or_before (
                rndf_point_t *rp_);

        rndf_point_t* active_rndf_point_before (
                double x_, double y_,
                rndf_t::waypoint_id_t const &before_wp_id);

// ******************* Edge Blockage Related *****************
private:
        // To keep info of a blockage
        class blocked_edge_t
        {
                double m_tstamp_blocked;
                double m_orig_cost;
                graph_node_t *m_src_vertex;
                graph_node_t *m_des_vertex;

        public:
                blocked_edge_t (
                        double tstamp_,
                        double orig_cost_,
                        graph_node_t *src_vertex_,
                        graph_node_t *des_vertex_) :
                        m_tstamp_blocked (tstamp_),
                        m_orig_cost (orig_cost_),
                        m_src_vertex (src_vertex_),
                        m_des_vertex (des_vertex_)
                        {
                        }
                graph_node_t* const get_src_vertex ()
                        {return m_src_vertex;}
                graph_node_t* const get_des_vertex ()
                        {return m_des_vertex;}
        };
        // To keep track of blocked edges
        std::deque<blocked_edge_t> m_blocked_edges;
        unsigned block_index (
                graph_node_t const *src_vertex_,
                graph_node_t const *des_vertex_);
        bool unset_first_blockage ();

        rndf_point_t* create_rndf_point (rndf_t const *rndf_,
                                         rndf_t::waypoint_id_t const &wp_id);

        rndf_point_t* create_rndf_point (rndf_t const *rndf_,
                                         unsigned sid_, unsigned lid_,
                                         unsigned wid_);
        rndf_point_t* insert_graph_node (unsigned sid_, unsigned lid_,
                                         unsigned wid_,
                                         double x_, double y_);

        rndf_point_t* terminate_road_at (unsigned sid_, unsigned lid_,
                                         unsigned wid_, double x_,
                                         double y_);

        rndf_point_t* start_road_at (unsigned sid_, unsigned lid_,
                                     unsigned wid_, double x_, double y_);

        rndf_point_t* already_added_inter_rp_for_blk (
                unsigned sid_, unsigned lid_, unsigned wid_,
                double x_, double y_);

        void move_existing_inter_rp (rndf_point_t *blk_rp,
                                     unsigned wid_, double x_, double y_);

        bool get_other_end_inter_rp (rndf_point_t *rp_,
                                     unsigned other_lid_,
                                     rndf_point_t* &neigh_rp_,
                                     rndf_point_t* &follow_rp_);

        void print_mission (mission_plan_t const &plan_);


public:
        void unset_all_blockages ();

        void set_unblocked_till (rndf_point_t *wp_id_1_,
                                 rndf_point_t *wp_id_2_);

        bool set_blocked (rndf_point_t *wp_id_1_, rndf_point_t *wp_id_2_);

        bool get_plan (rndf_point_t *start_, rndf_point_t *end_,
                       mission_plan_t &path_);
        void print_graph ()
                { m_graph.print_graph (); }

};
};


#endif

---