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 
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 
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
jsk_footstep_planner::PerceptionGridGraph
GridGraph< CostedGridState > PerceptionGridGraph
Definition: grid_perception.h:103
jsk_footstep_planner::PerceptionGridMap
Definition: grid_perception.h:33
jsk_footstep_planner::GridMap< CostedGridState >::inRange
virtual int inRange(int ix, int iy)
Definition: grid_graph.h:66
boost::shared_ptr
grid_graph.h
jsk_footstep_planner::PerceptionGridMap::getState
virtual StatePtr getState(int ix, int iy)
Definition: grid_perception.h:55
jsk_footstep_planner::GridMap< CostedGridState >::size_x_
int size_x_
Definition: grid_graph.h:78
jsk_footstep_planner::GridMap
Definition: grid_graph.h:10
jsk_footstep_planner::PerceptionGridMap::checkState
virtual bool checkState(int ix, int iy)
Definition: grid_perception.h:46
jsk_footstep_planner::GridMap< CostedGridState >::state_list_
std::vector< StatePtr > state_list_
Definition: grid_graph.h:80
jsk_footstep_planner
Definition: ann_grid.h:50
jsk_footstep_planner::PerceptionGridMap::updateCost
virtual bool updateCost(StatePtr st)
Definition: grid_perception.h:67
y
double y
jsk_footstep_planner::GridMap< CostedGridState >::size_y_
int size_y_
Definition: grid_graph.h:79
graph
FootstepGraph::Ptr graph
Definition: footstep_planning_2d_interactive_demo.cpp:51
jsk_footstep_planner::PerceptionGridMap::PerceptionGridMap
PerceptionGridMap(int _x, int _y)
Definition: grid_perception.h:39
jsk_footstep_planner::GridMap< CostedGridState >::MapState
CostedGridState MapState
Definition: grid_graph.h:16
jsk_footstep_planner::PerceptionGridMap::Ptr
boost::shared_ptr< PerceptionGridMap > Ptr
Definition: grid_perception.h:36
jsk_footstep_planner::PerceptionGridMap::cost_func_
UpdateCostFunction cost_func_
Definition: grid_perception.h:100
jsk_footstep_planner::GridMap< CostedGridState >::index
virtual int index(int ix, int iy)
Definition: grid_graph.h:65
jsk_footstep_planner::PerceptionGridMap::UpdateCostFunction
boost::function< bool(GridState::Ptr)> UpdateCostFunction
Definition: grid_perception.h:37
grid_astar_solver.h
jsk_footstep_planner::gridPerceptionHeuristicDistance
double gridPerceptionHeuristicDistance(SolverNode< PerceptionGridGraph::State, PerceptionGridGraph >::Ptr node, PerceptionGridGraph::Ptr graph)
Definition: grid_perception.h:106
jsk_footstep_planner::PerceptionGridMap::setCostFunction
virtual void setCostFunction(UpdateCostFunction func)
Definition: grid_perception.h:89
x
double x
jsk_footstep_planner::GridGraph
Definition: grid_graph.h:84
jsk_footstep_planner::PerceptionGridMap::resetGrid
virtual void resetGrid()
Definition: grid_perception.h:95


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