grid_graph.h
Go to the documentation of this file.
1 #ifndef JSK_FOOTSTEP_PLANNER_GRID_GRAPH_H_
2 #define JSK_FOOTSTEP_PLANNER_GRID_GRAPH_H_
3 
6 
7 namespace jsk_footstep_planner
8 {
9  template <class GStateT>
10  class GridMap
11  {
12  public:
14  typedef GridState State;
16  typedef GStateT MapState;
18 
19 
20  GridMap(int _x, int _y) : size_x_(_x), size_y_(_y)
21  {
22  createGrid();
23  }
24  inline virtual StatePtr getState(int ix, int iy)
25  {
26  return state_list_.at(index(ix, iy));
27  }
28  inline virtual bool setCost(std::vector<float> in)
29  {
30  size_t cnt = state_list_.size() <= in.size() ? state_list_.size() : in.size();
31  for(int i = 0; i < cnt; i++) {
32  state_list_[i]->setCost(in[i]);
33  }
34  }
35  inline virtual bool setCost(int ix, int iy, float cost = 0.0)
36  {
37  return getState(ix, iy)->setCost(cost);
38  }
39  inline virtual bool setOccupancy(std::vector<int> in)
40  {
41  size_t cnt = state_list_.size() <= in.size() ? state_list_.size() : in.size();
42  for(int i = 0; i < cnt; i++) {
43  state_list_[i]->setOccupancy(in[i]);
44  }
45  }
46  inline virtual bool setOccupancy(int ix, int iy, int occupancy = 0)
47  {
48  return getState(ix, iy)->setOccupancy(occupancy);
49  }
50  inline virtual int getOccupancy(int ix, int iy)
51  {
52  return getState(ix, iy)->getOccupancy();
53  }
54  inline virtual float getCost(int ix, int iy)
55  {
56  return getState(ix, iy)->getCost();
57  }
58  inline virtual bool isValid(int ix, int iy)
59  {
60  return getState(ix, iy)->isValid();
61  }
62 
63  inline virtual int sizeX() { return size_x_; }
64  inline virtual int sizeY() { return size_y_; }
65  inline virtual int index(int ix, int iy) { return ((size_x_ * iy) + ix); }
66  inline virtual int inRange(int ix, int iy) { return (ix >= 0 && ix < size_x_ && iy >= 0 && iy < size_y_ ); }
67  protected:
68  GridMap() {}
69  virtual void createGrid() {
70  state_list_.resize(size_x_ * size_y_);
71  for (int y = 0; y < size_y_; y++) {
72  for (int x = 0; x < size_x_; x++) {
73  StatePtr st(new MapState(x, y));
74  state_list_[index(x, y)] = st;
75  }
76  }
77  }
78  int size_x_;
79  int size_y_;
80  std::vector<StatePtr > state_list_;
81  };
82 
83  template <class GStateT>
84  class GridGraph: public Graph<GridState>
85  {
86  public:
89  typedef GStateT MapState;
90  typedef typename GStateT::Ptr MapStatePtr;
92 
93  GridGraph(typename GridType::Ptr gr)
94  {
95  gridmap_ = gr;
96  }
97 
98  StatePtr getState(int ix, int iy) {
99  if(gridmap_->inRange(ix, iy)) {
100  return gridmap_->getState(ix, iy);
101  }
102  StatePtr p;
103  p.reset();
104  return p;
105  }
106 
107  virtual std::vector<StatePtr> successors(StatePtr target_state)
108  {
109  std::vector<StatePtr> ret;
110  int x_offset = target_state->indexX();
111  int y_offset = target_state->indexY();
112  for (int x = -1; x < 2; x++) {
113  for (int y = -1; y < 2; y++) {
114  if (x != 0 || y != 0) {
115  StatePtr st =
116  getState(x + x_offset, y + y_offset);
117  if (!!st) {
118  if(st->isValid()) {
119  ret.push_back(st);
120  }
121  }
122  }
123  }
124  }
125  return ret;
126  }
127  virtual bool isGoal(StatePtr state)
128  {
129  return (state == goal_state_);
130  }
131 
132  virtual double pathCost(StatePtr from,
133  StatePtr to, double prev_cost)
134  {
135  double gx = from->indexX() - to->indexX();
136  double gy = from->indexY() - to->indexY();
137  return prev_cost + std::sqrt(gx * gx + gy * gy) + to->getCost();
138  }
139 
140  protected:
142  private:
143  };
144 
148 
152 }
153 
154 #endif
virtual bool setCost(int ix, int iy, float cost=0.0)
Definition: grid_graph.h:35
virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
Definition: grid_graph.h:132
virtual int inRange(int ix, int iy)
Definition: grid_graph.h:66
GridGraph(typename GridType::Ptr gr)
Definition: grid_graph.h:93
virtual StatePtr getState(int ix, int iy)
Definition: grid_graph.h:24
virtual bool setOccupancy(std::vector< int > in)
Definition: grid_graph.h:39
GridMap< OccupancyGridState > OccupancyGridMap
Definition: grid_graph.h:146
GridGraph< OccupancyGridState > OccupancyGridGraph
Definition: grid_graph.h:150
double y
virtual std::vector< StatePtr > successors(StatePtr target_state)
Definition: grid_graph.h:107
boost::shared_ptr< GridMap > Ptr
Definition: grid_graph.h:13
virtual float getCost(int ix, int iy)
Definition: grid_graph.h:54
std::vector< StatePtr > state_list_
Definition: grid_graph.h:80
GridMap< GStateT > GridType
Definition: grid_graph.h:88
GridMap< GridState > SimpleGridMap
Definition: grid_graph.h:145
virtual bool isValid(int ix, int iy)
Definition: grid_graph.h:58
virtual int index(int ix, int iy)
Definition: grid_graph.h:65
boost::shared_ptr< GridGraph > Ptr
Definition: grid_graph.h:87
Graph< GridState >::StateT State
Definition: grid_graph.h:91
GridGraph< GridState > SimpleGridGraph
Definition: grid_graph.h:149
virtual bool setCost(std::vector< float > in)
Definition: grid_graph.h:28
boost::shared_ptr< GStateT > MapStatePtr
Definition: grid_graph.h:17
GridGraph< CostedGridState > CostedGridGraph
Definition: grid_graph.h:151
virtual bool setOccupancy(int ix, int iy, int occupancy=0)
Definition: grid_graph.h:46
virtual int getOccupancy(int ix, int iy)
Definition: grid_graph.h:50
GridMap< CostedGridState > CostedGridMap
Definition: grid_graph.h:147
double x
virtual bool isGoal(StatePtr state)
Definition: grid_graph.h:127
StatePtr getState(int ix, int iy)
Definition: grid_graph.h:98


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28