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
jsk_footstep_planner::GridMap::setCost
virtual bool setCost(int ix, int iy, float cost=0.0)
Definition: grid_graph.h:35
jsk_footstep_planner::OccupancyGridMap
GridMap< OccupancyGridState > OccupancyGridMap
Definition: grid_graph.h:146
jsk_footstep_planner::GridMap::MapStatePtr
boost::shared_ptr< GStateT > MapStatePtr
Definition: grid_graph.h:17
jsk_footstep_planner::GridMap::inRange
virtual int inRange(int ix, int iy)
Definition: grid_graph.h:66
jsk_footstep_planner::GridMap::GridMap
GridMap(int _x, int _y)
Definition: grid_graph.h:20
jsk_footstep_planner::GridMap::getState
virtual StatePtr getState(int ix, int iy)
Definition: grid_graph.h:24
boost::shared_ptr
i
int i
jsk_footstep_planner::Graph< GridState >::goal_state_
StatePtr goal_state_
Definition: graph.h:135
jsk_footstep_planner::GridMap::createGrid
virtual void createGrid()
Definition: grid_graph.h:69
jsk_footstep_planner::GridGraph::successors
virtual std::vector< StatePtr > successors(StatePtr target_state)
Definition: grid_graph.h:107
jsk_footstep_planner::GridMap::size_x_
int size_x_
Definition: grid_graph.h:78
jsk_footstep_planner::GridMap
Definition: grid_graph.h:10
jsk_footstep_planner::GridGraph::getState
StatePtr getState(int ix, int iy)
Definition: grid_graph.h:98
jsk_footstep_planner::Graph
Definition: graph.h:78
jsk_footstep_planner::GridMap::state_list_
std::vector< StatePtr > state_list_
Definition: grid_graph.h:80
grid_state.h
jsk_footstep_planner::GridMap::isValid
virtual bool isValid(int ix, int iy)
Definition: grid_graph.h:58
jsk_footstep_planner::GridMap::getOccupancy
virtual int getOccupancy(int ix, int iy)
Definition: grid_graph.h:50
jsk_footstep_planner::GridMap::setOccupancy
virtual bool setOccupancy(std::vector< int > in)
Definition: grid_graph.h:39
jsk_footstep_planner::CostedGridMap
GridMap< CostedGridState > CostedGridMap
Definition: grid_graph.h:147
jsk_footstep_planner::GridMap::GridMap
GridMap()
Definition: grid_graph.h:68
jsk_footstep_planner::GridGraph::GridGraph
GridGraph(typename GridType::Ptr gr)
Definition: grid_graph.h:93
jsk_footstep_planner
Definition: ann_grid.h:50
jsk_footstep_planner::GridGraph::MapState
GStateT MapState
Definition: grid_graph.h:89
jsk_footstep_planner::GridGraph::isGoal
virtual bool isGoal(StatePtr state)
Definition: grid_graph.h:127
jsk_footstep_planner::GridMap::sizeX
virtual int sizeX()
Definition: grid_graph.h:63
jsk_footstep_planner::GridGraph::Ptr
boost::shared_ptr< GridGraph > Ptr
Definition: grid_graph.h:87
y
double y
jsk_footstep_planner::GridGraph::pathCost
virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
Definition: grid_graph.h:132
jsk_footstep_planner::GridMap::size_y_
int size_y_
Definition: grid_graph.h:79
pose_array.p
p
Definition: pose_array.py:21
jsk_footstep_planner::GridGraph::State
Graph< GridState >::StateT State
Definition: grid_graph.h:91
jsk_footstep_planner::GridMap::getCost
virtual float getCost(int ix, int iy)
Definition: grid_graph.h:54
jsk_footstep_planner::GridMap::MapState
GStateT MapState
Definition: grid_graph.h:16
jsk_footstep_planner::GridMap::setCost
virtual bool setCost(std::vector< float > in)
Definition: grid_graph.h:28
jsk_footstep_planner::GridMap::index
virtual int index(int ix, int iy)
Definition: grid_graph.h:65
jsk_footstep_planner::OccupancyGridGraph
GridGraph< OccupancyGridState > OccupancyGridGraph
Definition: grid_graph.h:150
jsk_footstep_planner::GridGraph::GridType
GridMap< GStateT > GridType
Definition: grid_graph.h:88
jsk_footstep_planner::GridGraph::MapStatePtr
GStateT::Ptr MapStatePtr
Definition: grid_graph.h:90
jsk_footstep_planner::GridState
Definition: grid_state.h:5
state
state
jsk_footstep_planner::GridMap::State
GridState State
Definition: grid_graph.h:14
jsk_footstep_planner::CostedGridGraph
GridGraph< CostedGridState > CostedGridGraph
Definition: grid_graph.h:151
jsk_footstep_planner::SimpleGridGraph
GridGraph< GridState > SimpleGridGraph
Definition: grid_graph.h:149
graph.h
jsk_footstep_planner::GridMap::Ptr
boost::shared_ptr< GridMap > Ptr
Definition: grid_graph.h:13
jsk_footstep_planner::GridMap::sizeY
virtual int sizeY()
Definition: grid_graph.h:64
jsk_footstep_planner::GridMap::setOccupancy
virtual bool setOccupancy(int ix, int iy, int occupancy=0)
Definition: grid_graph.h:46
x
double x
jsk_footstep_planner::GridMap::StatePtr
GridState::Ptr StatePtr
Definition: grid_graph.h:15
jsk_footstep_planner::GridGraph
Definition: grid_graph.h:84
jsk_footstep_planner::GridGraph::gridmap_
GridType::Ptr gridmap_
Definition: grid_graph.h:141
jsk_footstep_planner::SimpleGridMap
GridMap< GridState > SimpleGridMap
Definition: grid_graph.h:145


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:30