grid_graph.h
Go to the documentation of this file.
00001 #ifndef JSK_FOOTSTEP_PLANNER_GRID_GRAPH_H_
00002 #define JSK_FOOTSTEP_PLANNER_GRID_GRAPH_H_
00003 
00004 #include "jsk_footstep_planner/graph.h"
00005 #include "jsk_footstep_planner/grid_state.h"
00006 
00007 namespace jsk_footstep_planner
00008 {
00009   template <class GStateT>
00010   class GridMap
00011   {
00012   public:
00013     typedef boost::shared_ptr< GridMap > Ptr;
00014     typedef GridState State;
00015     typedef GridState::Ptr StatePtr;
00016     typedef GStateT MapState;
00017     typedef boost::shared_ptr< GStateT > MapStatePtr;
00018 
00019 
00020     GridMap(int _x, int _y) : size_x_(_x), size_y_(_y)
00021     {
00022       createGrid();
00023     }
00024     inline virtual StatePtr getState(int ix, int iy)
00025     {
00026       return state_list_.at(index(ix, iy));
00027     }
00028     inline virtual bool setCost(std::vector<float> in)
00029     {
00030       size_t cnt = state_list_.size() <= in.size() ? state_list_.size() : in.size();
00031       for(int i = 0; i < cnt; i++) {
00032         state_list_[i]->setCost(in[i]);
00033       }
00034     }
00035     inline virtual bool setCost(int ix, int iy, float cost = 0.0)
00036     {
00037       return getState(ix, iy)->setCost(cost);
00038     }
00039     inline virtual bool setOccupancy(std::vector<int> in)
00040     {
00041       size_t cnt = state_list_.size() <= in.size() ? state_list_.size() : in.size();
00042       for(int i = 0; i < cnt; i++) {
00043         state_list_[i]->setOccupancy(in[i]);
00044       }
00045     }
00046     inline virtual bool setOccupancy(int ix, int iy, int occupancy = 0)
00047     {
00048       return getState(ix, iy)->setOccupancy(occupancy);
00049     }
00050     inline virtual int getOccupancy(int ix, int iy)
00051     {
00052       return getState(ix, iy)->getOccupancy();
00053     }
00054     inline virtual float getCost(int ix, int iy)
00055     {
00056       return getState(ix, iy)->getCost();
00057     }
00058     inline virtual bool isValid(int ix, int iy)
00059     {
00060       return getState(ix, iy)->isValid();
00061     }
00062 
00063     inline virtual int sizeX() { return size_x_; }
00064     inline virtual int sizeY() { return size_y_; }
00065     inline virtual int index(int ix, int iy) { return ((size_x_ * iy) + ix); }
00066     inline virtual int inRange(int ix, int iy) { return (ix >= 0 && ix < size_x_ && iy >= 0 && iy < size_y_ ); }
00067   protected:
00068     GridMap() {}
00069     virtual void createGrid() {
00070       state_list_.resize(size_x_ * size_y_);
00071       for (int y = 0; y < size_y_; y++) {
00072         for (int x = 0; x < size_x_; x++) {
00073           StatePtr st(new MapState(x, y));
00074           state_list_[index(x, y)] = st;
00075         }
00076       }
00077     }
00078     int size_x_;
00079     int size_y_;
00080     std::vector<StatePtr > state_list_;
00081   };
00082 
00083   template <class GStateT>
00084   class GridGraph: public Graph<GridState>
00085   {
00086   public:
00087     typedef boost::shared_ptr<GridGraph> Ptr;
00088     typedef GridMap<GStateT> GridType;
00089     typedef GStateT MapState;
00090     typedef typename GStateT::Ptr MapStatePtr;
00091     typedef Graph<GridState>::StateT State;
00092 
00093     GridGraph(typename GridType::Ptr gr)
00094     {
00095       gridmap_ = gr;
00096     }
00097 
00098     StatePtr getState(int ix, int iy) {
00099       if(gridmap_->inRange(ix, iy)) {
00100         return gridmap_->getState(ix, iy);
00101       }
00102       StatePtr p;
00103       p.reset();
00104       return p;
00105     }
00106 
00107     virtual std::vector<StatePtr> successors(StatePtr target_state)
00108     {
00109       std::vector<StatePtr> ret;
00110       int x_offset = target_state->indexX();
00111       int y_offset = target_state->indexY();
00112       for (int x = -1; x < 2; x++) {
00113         for (int y = -1; y < 2; y++) {
00114           if (x != 0 || y != 0) {
00115             StatePtr st =
00116               getState(x + x_offset, y + y_offset);
00117             if (!!st) {
00118               if(st->isValid()) {
00119                 ret.push_back(st);
00120               }
00121             }
00122           }
00123         }
00124       }
00125       return ret;
00126     }
00127     virtual bool isGoal(StatePtr state)
00128     {
00129       return (state == goal_state_);
00130     }
00131 
00132     virtual double pathCost(StatePtr from,
00133                             StatePtr to, double prev_cost)
00134     {
00135       double gx = from->indexX() - to->indexX();
00136       double gy = from->indexY() - to->indexY();
00137       return prev_cost + std::sqrt(gx * gx + gy * gy) + to->getCost();
00138     }
00139 
00140   protected:
00141     typename GridType::Ptr gridmap_;
00142   private:
00143   };
00144 
00145   typedef GridMap<GridState> SimpleGridMap;
00146   typedef GridMap<OccupancyGridState> OccupancyGridMap;
00147   typedef GridMap<CostedGridState> CostedGridMap;
00148 
00149   typedef GridGraph<GridState> SimpleGridGraph;
00150   typedef GridGraph<OccupancyGridState> OccupancyGridGraph;
00151   typedef GridGraph<CostedGridState> CostedGridGraph;
00152 }
00153 
00154 #endif


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:29