zone_planner.H

Go to the documentation of this file.

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

#ifndef CAJUN_ZONE_PLANNER_H
#define CAJUN_ZONE_PLANNER_H

#include "world_state.H"
#include "rndf.H"
#include "rndf_point.H"

#include "map_grid_publisher.H"
#include "zone_marker.H"
#include "goal_marker.H"
#include "obstacle_marker.H"

#include "search_grid_publisher.H"

#include "wave_gc_oracle.H"

#include "obstacle_free.H"
#include "path_extractor.H"
#include "basic_work_list.H"

#include "uc_planner_param.H"

namespace cajun
{
class zone_planner_t
{
public:
        zone_planner_t (world_state_t *ws_, rndf_t const *rndf_,
                        unsigned zone_id, rndf_point_t *start_rp_,
                        rndf_point_t *end_rp_);
        ~zone_planner_t ();
        void generate_path (double tstamp_, path_t &path_, double start_x_,
                            double start_y_, double orient_);

        void mark_obstacles (unsigned zone_id_);

        void publish_grids (double tstamp_);
private:

        rndf_point_t *m_start_rp;
        rndf_point_t *m_end_rp;

        double m_goal_x;
        double m_goal_y;
        double m_goal_orient;

        // Stored in global list for multiple publish grid call
        double m_start_x;
        double m_start_y;
        double m_orient;

        double m_cell_size;
        world_state_t *m_ws;

        rndf_t const *m_rndf;

        const uc_planner_param_t *m_uc_param;

        unsigned m_zone_id;

        map_grid_t m_map_grid;

        // Map grid related
        map_grid_publisher_t *m_map_publisher;

        zone_marker_t *m_zone_marker;
        goal_marker_t *m_goal_marker;
        obstacle_marker_t *m_obstacle_marker;

        // Search grid related
        search_grid_t m_search_grid;
        search_grid_publisher_t *m_search_publisher;

        // Work list
        work_list_interface_t *m_work_list;

        // Oracle
        gc_oracle_t *m_gc_oracle;

        // path extractor
        path_extractor_t *m_path_extractor;

        obstacle_free_t *m_obstacle_free;

        void compute_goal (rndf_point_t *end_rp_);

        void mark_zone_region (unsigned zone_id_);

        void mark_goal (rndf_point_t *end_rp_);

        void get_non_blocking_start (double &new_x, double &new_y,
                                     double x_, double y_);

        void mark_obstacle (obstacle_points_data_t const &obst_);

        bool obstacle_in_zone (obstacle_points_data_t const &obst_,
                               rndf_t::zone_t const &zone);

        void parking_properly (path_t &path_);


        class filler_t
        {
        public:
                filler_t (map_grid_t *map_grid_) :
                        m_map_grid (map_grid_), m_cost (0) {}
                void set_cost (float cost_) { m_cost = cost_; }

                void fill (int x_, int y_, unsigned w_)
                        {
                                map_grid_t::iterator_t iter =
                                        m_map_grid->find (x_, y_);

                                while (w_--)
                                {
                                        iter->boundary = m_cost;
                                        iter.neighbor (1, 0);
                                }
                        }

        protected:
                map_grid_t * const m_map_grid;

                float m_cost;
        };


};


};


#endif

---