grid_perception.h
Go to the documentation of this file.
1 #ifndef JSK_FOOTSTEP_PLANNER_GRID_PERCEPTION_H_
2 #define JSK_FOOTSTEP_PLANNER_GRID_PERCEPTION_H_
3 
4 #include <pcl/pcl_base.h>
5 #include <pcl/point_cloud.h>
6 #include <pcl/point_types.h>
7 #include <pcl/point_representation.h>
8 
11 
12 namespace jsk_footstep_planner {
13 #if 0
14  class PerceptionGridState : public CostedGridState
15  {
16  public:
18 
19  PerceptionGridState(int x, int y) : CostedGridState(x, y), updated_(false)
20  {
21  }
22  inline virtual bool isValid() {
23  // TODO update for using cost
24  if(occupancy_ == 0) {
25  return true;
26  }
27  return false;
28  }
29  protected:
30  bool updated_;
31  };
32 #endif
33  class PerceptionGridMap : public GridMap <CostedGridState>
34  {
35  public:
37  typedef boost::function<bool(GridState::Ptr)> UpdateCostFunction;
38 
39  PerceptionGridMap(int _x, int _y) : GridMap()
40  {
41  size_x_ = _x;
42  size_y_ = _y;
43  resetGrid();
44  }
45 
46  inline virtual bool checkState(int ix, int iy)
47  {
48  StatePtr pt = state_list_.at(index(ix, iy));
49  if(!pt) {
50  return false;
51  }
52  return true;
53  }
54 
55  inline virtual StatePtr getState(int ix, int iy)
56  {
57  StatePtr pt = state_list_.at(index(ix, iy));
58  if(!pt) {
59  StatePtr new_pt(new MapState(ix, iy));
60  state_list_[index(ix, iy)] = new_pt;
61  updateCost(new_pt);
62  return new_pt;
63  }
64  return pt;
65  }
66 
67  virtual bool updateCost(StatePtr st)
68  {
69 #if 0
70  // update around the state
71  int ix = st->indexX();
72  int iy = st->indexY();
73  for(int y = -1; y < 2; y++) {
74  for(int x = -1; x < 2; x++) {
75  if(inRange(ix + x, iy + y)) {
76  StatePtr pt = state_list_.at(index(ix+x, iy+y));
77  if(!pt) {
78  StatePtr new_pt(new MapState(ix+x, iy+y));
79  state_list_[index(ix, iy)] = new_pt;
80  cost_func_(new_pt);
81  }
82  }
83  }
84  }
85 #endif
86  return cost_func_(st);
87  }
88 
89  virtual void setCostFunction(UpdateCostFunction func)
90  {
91  cost_func_ = func;
92  }
93 
94  protected:
95  virtual void resetGrid() {
96  state_list_.clear();
97  state_list_.resize(size_x_ * size_y_);
98  }
99 
100  UpdateCostFunction cost_func_;
101  };
102 
104 
105  // default heuristic function
108 
109  int ix = node->getState()->indexX();
110  int iy = node->getState()->indexY();
111  double gx = graph->getGoalState()->indexX() - ix;
112  double gy = graph->getGoalState()->indexY() - iy;
113 
114  return std::sqrt(gx * gx + gy * gy);
115  }
116 }
117 
118 #endif
GridGraph< CostedGridState > PerceptionGridGraph
virtual void setCostFunction(UpdateCostFunction func)
virtual StatePtr getState(int ix, int iy)
virtual bool checkState(int ix, int iy)
double y
unsigned int index
FootstepGraph::Ptr graph
boost::function< bool(GridState::Ptr)> UpdateCostFunction
boost::shared_ptr< PerceptionGridMap > Ptr
virtual bool updateCost(StatePtr st)
double gridPerceptionHeuristicDistance(SolverNode< PerceptionGridGraph::State, PerceptionGridGraph >::Ptr node, PerceptionGridGraph::Ptr graph)
double x


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