grid_astar_solver.h
Go to the documentation of this file.
1 #ifndef JSK_FOOTSTEP_PLANNER_GRID_ASTAR_SOLVER_H_
2 #define JSK_FOOTSTEP_PLANNER_GRID_ASTAR_SOLVER_H_
3 
5 #include <boost/unordered_map.hpp>
6 #include <cfloat> // DBL_MAX
7 
8 namespace jsk_footstep_planner
9 {
10  template <class GraphT>
11  class GridAStarSolver: public AStarSolver<GraphT>
12  {
13  public:
15  typedef typename GraphT::StateT State;
16  typedef typename GraphT::StateT::Ptr StatePtr;
17  typedef typename GraphT::Ptr GraphPtr;
19  typedef std::vector<typename SolverNode<State, GraphT>::Ptr> Path;
20  typedef boost::unordered_map< StatePtr, SolverNodePtr > SolveList;
21 
23 
24  virtual
25  std::vector<typename SolverNode<State, GraphT>::Ptr>
26  solve(const ros::WallDuration& timeout = ros::WallDuration(1000000000.0))
27  {
30  (graph_->getStartState(),
31  0, graph_));
32  addToOpenList(start_state);
33  if (verbose_) { std::cerr << "start" << std::endl; }
34  //while (!isOpenListEmpty() && isOK(start_time, timeout)) {
35  while (!isOpenListEmpty()) {
36  SolverNodePtr target_node = popFromOpenList();
37  StatePtr p = target_node->getState();
38  if (verbose_) { std::cerr << "t " << p->indexX() << " - " << p->indexY(); }
39  if (graph_->isGoal(target_node->getState())) {
40  if (verbose_) { std::cerr << " -> g" << std::endl; }
41  std::vector<SolverNodePtr> result_path = target_node->getPathWithoutThis();
42  result_path.push_back(target_node);
43  return result_path;
44  } else {
45  std::vector<SolverNodePtr> nnodes = target_node->expand(target_node, verbose_);
46  if (verbose_) { std::cerr << " -> c "; }
47  addToCloseList(target_node);
48  for(int i = 0; i < nnodes.size(); i++) {
49  SolverNodePtr anode = nnodes[i];
50  double prev_cost = 0;
51  if(verbose_) {
52  std::cerr << "a-" << i << " " << anode->getState()->indexX();
53  std::cerr << " - " << anode->getState()->indexY();
54  }
55  if(findInOpenList(anode, prev_cost)) {
56  if(verbose_) { std::cerr << " -o"; }
57  if(anode->getCost() < prev_cost) {
58  if(verbose_) { std::cerr << "-r"; }
59  removeFromOpenList(anode); // replace
60  addToOpenList(anode);
61  }
62  } else if(findInCloseList(anode, prev_cost)) {
63  if(verbose_) { std::cerr << " -c"; }
64  if(anode->getCost() < prev_cost) {
65  if(verbose_) { std::cerr << "-r"; }
66  removeFromCloseList(anode);
67  addToOpenList(anode);
68  }
69  } else {
70  if(verbose_) { std::cerr << " -n"; }
71  addToOpenList(anode);
72  }
73  if (verbose_) {
74  if (i != nnodes.size() -1) {
75  std::cerr << " / ";
76  }
77  }
78  }
79  }
80  if (verbose_) { std::cerr << std::endl; }
81  }
82  // Failed to search
83  return std::vector<SolverNodePtr>();
84  }
85 
87  virtual bool isOpenListEmpty()
88  {
89  return open_list_map_.empty();
90  }
92  {
93  // slow implimentation??
94  double min_cost = DBL_MAX;
95  SolverNodePtr ret;
96  for (typename SolveList::const_iterator it = open_list_map_.begin();
97  it != open_list_map_.end(); it++) {
98  if (it->second->getSortValue() < min_cost) {
99  ret = it->second;
100  min_cost = it->second->getSortValue();
101  }
102  }
103  if (!!ret) {
104  typename SolveList::const_iterator it
105  = open_list_map_.find(ret->getState());
106  open_list_map_.erase(it);
107  // remove from sorted_list(pop)
108  }
109  return ret;
110  }
111  virtual void addToOpenList(SolverNodePtr node)
112  {
113  typename SolveList::const_iterator it
114  = open_list_map_.find(node->getState());
115  if(it != open_list_map_.end()) {
116  std::cerr << ";;;; warn (duplicate adding to openlist) ;;;;" << std::endl;
117  }
118  node->setSortValue(AStarSolver<GraphT>::fn(node));
120  .insert(typename SolveList::value_type(node->getState(), node));
121  // add to sorted_list
122  }
123  virtual bool findInOpenList(SolverNodePtr node, double &cost)
124  {
125  typename SolveList::const_iterator it
126  = open_list_map_.find(node->getState());
127  if (it != open_list_map_.end()) {
128  cost = it->second->getCost();
129  return true;
130  }
131  return false;
132  }
134  {
135  typename SolveList::const_iterator it
136  = open_list_map_.find(node->getState());
137  if(it != open_list_map_.end()) {
138  open_list_map_.erase(it);
139  // remove from sorted_list(reconstruct)
140  return true;
141  } else {
142  std::cerr << ";;;; warn (fail remove) ;;;;" << std::endl;
143  }
144  return false;
145  }
146  virtual void addToCloseList(SolverNodePtr node)
147  {
148  // check
149  if(AStarSolver<GraphT>::findInCloseList(node->getState())) {
150  std::cerr << ";;;; warn (duplicate adding to closelist) ;;;;" << std::endl;
151  }
152  //
153  AStarSolver<GraphT>::addToCloseList(node->getState(), node->getCost());
154  }
155  virtual bool findInCloseList(SolverNodePtr node, double &cost)
156  {
157  return AStarSolver<GraphT>::findInCloseList(node->getState(), cost);
158  }
160  {
161  return AStarSolver<GraphT>::removeFromCloseList(node->getState());
162  }
164  virtual bool getOpenList(std::vector<StatePtr> &lst, std::vector<float> &cost)
165  {
166  return false;
167  }
168  virtual bool getCloseList(std::vector<StatePtr> &lst, std::vector<float> &cost)
169  {
170  return false;
171  }
172 
173  protected:
177  private:
178 
179  };
180 }
181 #endif
jsk_footstep_planner::Solver::removeFromCloseList
virtual bool removeFromCloseList(StatePtr state)
Definition: solver.h:189
jsk_footstep_planner::GridAStarSolver::removeFromOpenList
virtual bool removeFromOpenList(SolverNodePtr node)
Definition: grid_astar_solver.h:133
astar_solver.h
boost::shared_ptr
jsk_footstep_planner::Solver
Definition: solver.h:79
i
int i
jsk_footstep_planner::GridAStarSolver::getCloseList
virtual bool getCloseList(std::vector< StatePtr > &lst, std::vector< float > &cost)
Definition: grid_astar_solver.h:168
jsk_footstep_planner::GridAStarSolver::SolveList
boost::unordered_map< StatePtr, SolverNodePtr > SolveList
Definition: grid_astar_solver.h:20
jsk_footstep_planner::GridAStarSolver::solve
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
Definition: grid_astar_solver.h:26
jsk_footstep_planner::AStarSolver::GraphPtr
GraphT::Ptr GraphPtr
Definition: astar_solver.h:115
jsk_footstep_planner::GridAStarSolver::getOpenList
virtual bool getOpenList(std::vector< StatePtr > &lst, std::vector< float > &cost)
Definition: grid_astar_solver.h:164
jsk_footstep_planner::SolverNode
Definition: solver_node.h:82
jsk_footstep_planner::GridAStarSolver::GridAStarSolver
GridAStarSolver(GraphPtr graph)
Definition: grid_astar_solver.h:22
jsk_footstep_planner::GridAStarSolver::removeFromCloseList
virtual bool removeFromCloseList(SolverNodePtr node)
Definition: grid_astar_solver.h:159
jsk_footstep_planner::GridAStarSolver::findInCloseList
virtual bool findInCloseList(SolverNodePtr node, double &cost)
Definition: grid_astar_solver.h:155
jsk_footstep_planner::GridAStarSolver::findInOpenList
virtual bool findInOpenList(SolverNodePtr node, double &cost)
Definition: grid_astar_solver.h:123
jsk_footstep_planner
Definition: ann_grid.h:50
jsk_footstep_planner::AStarSolver
Definition: astar_solver.h:77
jsk_footstep_planner::Solver::verbose_
bool verbose_
Definition: solver.h:202
jsk_footstep_planner::GridAStarSolver::Ptr
boost::shared_ptr< GridAStarSolver > Ptr
Definition: grid_astar_solver.h:14
jsk_footstep_planner::AStarSolver::StatePtr
GraphT::StateT::Ptr StatePtr
Definition: astar_solver.h:113
ros::WallTime::now
static WallTime now()
jsk_footstep_planner::Solver::addToCloseList
virtual void addToCloseList(StatePtr state, double cost=0)
Definition: solver.h:167
pose_array.p
p
Definition: pose_array.py:21
graph
FootstepGraph::Ptr graph
Definition: footstep_planning_2d_interactive_demo.cpp:51
jsk_footstep_planner::GridAStarSolver::addToOpenList
virtual void addToOpenList(SolverNodePtr node)
Definition: grid_astar_solver.h:111
start_time
start_time
jsk_footstep_planner::GridAStarSolver::popFromOpenList
virtual SolverNodePtr popFromOpenList()
Definition: grid_astar_solver.h:91
ros::WallTime
jsk_footstep_planner::GridAStarSolver::StatePtr
GraphT::StateT::Ptr StatePtr
Definition: grid_astar_solver.h:16
jsk_footstep_planner::GridAStarSolver::isOpenListEmpty
virtual bool isOpenListEmpty()
Definition: grid_astar_solver.h:87
jsk_footstep_planner::GridAStarSolver::addToCloseList
virtual void addToCloseList(SolverNodePtr node)
Definition: grid_astar_solver.h:146
jsk_footstep_planner::Solver::findInCloseList
virtual bool findInCloseList(StatePtr state)
Definition: solver.h:174
jsk_footstep_planner::GridAStarSolver::GraphPtr
GraphT::Ptr GraphPtr
Definition: grid_astar_solver.h:17
ros::WallDuration
jsk_footstep_planner::GridAStarSolver::Path
std::vector< typename SolverNode< State, GraphT >::Ptr > Path
Definition: grid_astar_solver.h:19
jsk_footstep_planner::GridAStarSolver
Definition: grid_astar_solver.h:11
jsk_footstep_planner::Solver::graph_
GraphPtr graph_
Definition: solver.h:201
jsk_footstep_planner::GridAStarSolver::open_list_map_
SolveList open_list_map_
Definition: grid_astar_solver.h:174
jsk_footstep_planner::GridAStarSolver::State
GraphT::StateT State
Definition: grid_astar_solver.h:15
jsk_footstep_planner::GridAStarSolver::SolverNodePtr
SolverNode< State, GraphT >::Ptr SolverNodePtr
Definition: grid_astar_solver.h:18


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