path_extractor.H

Go to the documentation of this file.

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


#ifndef CAJUN_PATH_EXTRACTOR_H
#define CAJUN_PATH_EXTRACTOR_H


#include "map_grid.H"
#include "search_grid.H"

#include "planner_defs.H"


#include <cmath>
#include <vector>


namespace cajun
{


// struct path_node_t
// {
//      path_node_t () : speed_limit (INT_MAX) {}
//      path_node_t (double x_, double y_) :
//              x (x_), y (y_), speed_limit (INT_MAX) {}

//      double x;
//      double y;
//      double speed_limit;

//      bool operator!= (path_node_t const &p_) const
//              { return x != p_.x || y != p_.y; }
// };


class gc_oracle_t;


class path_extractor_t
{
public:
        path_extractor_t (double cell_size_) :
                m_cell_size (cell_size_), m_gc_oracle (0),
                m_map_grid (0), m_search_grid (0) {}
        virtual ~path_extractor_t () {}

        void set_gc_oracle (gc_oracle_t *oracle_) { m_gc_oracle = oracle_; }

        void set_map_grid (map_grid_t *grid_) { m_map_grid = grid_; }
        void set_search_grid (search_grid_t *grid_) { m_search_grid = grid_; }

//      void set_bot_orient (double orient_) { m_bot_orient = orient_; }
//      void set_bot_speed (double speed_) {m_bot_speed = speed_;}
        void set_search_orient (double orient_) { m_search_orient = orient_; }

        bool extract (double tstamp_, double sx_, double sy_,
                      double gx_, double gy_, double goal_orient,
                      path_t &path_, path_t &detail_path_, double orient_);

protected:
        double const m_cell_size;

        void split_coord (double c_, int &i_, float &f_) const
        {
                double v = c_ / m_cell_size;

                i_ = (int) floor (v);
                f_ = v - i_;
        }

        gc_oracle_t *m_gc_oracle;

        map_grid_t *m_map_grid;
        search_grid_t *m_search_grid;

        double m_sx;
        double m_sy;
        double m_dcos;
        double m_dsin;

        double m_goal_x;
        double m_goal_y;
        double m_goal_orient;

//      double m_bot_orient;
//      double m_bot_speed;
        double m_search_orient;

        int m_map_px;
        int m_map_py;
        map_grid_t::iterator_t m_map_beg;

        int m_search_px;
        int m_search_py;
        search_grid_t::iterator_t m_search_beg;

        virtual bool extract (double tstamp_, path_t &path_,
                              path_t &detail_path_, double orient_) = 0;

        double search_to_global_x (double x, double y)
                { return m_sx + x * m_dcos - y * m_dsin; }

        double search_to_global_y (double x, double y)
                { return m_sy + x * m_dsin + y * m_dcos; }
};


};


#endif

---