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 
22  GridAStarSolver(GraphPtr graph): AStarSolver<GraphT>(graph) {}
23 
24  virtual
25  std::vector<typename SolverNode<State, GraphT>::Ptr>
26  solve(const ros::WallDuration& timeout = ros::WallDuration(1000000000.0))
27  {
28  ros::WallTime start_time = ros::WallTime::now();
29  SolverNodePtr start_state(new SolverNode<State, GraphT>
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  }
91  virtual SolverNodePtr popFromOpenList()
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  }
133  virtual bool removeFromOpenList(SolverNodePtr node)
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  }
159  virtual bool removeFromCloseList(SolverNodePtr node)
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:
174  SolveList open_list_map_;
177  private:
178 
179  };
180 }
181 #endif
virtual bool removeFromCloseList(SolverNodePtr node)
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
virtual bool findInOpenList(SolverNodePtr node, double &cost)
virtual bool getOpenList(std::vector< StatePtr > &lst, std::vector< float > &cost)
boost::unordered_map< StatePtr, SolverNodePtr > SolveList
virtual bool removeFromCloseList(StatePtr state)
Definition: solver.h:125
boost::shared_ptr< GridAStarSolver > Ptr
FootstepGraph::Ptr graph
virtual void addToOpenList(SolverNodePtr node)
virtual void addToCloseList(SolverNodePtr node)
virtual SolverNodePtr popFromOpenList()
virtual void addToCloseList(StatePtr state, double cost=0)
Definition: solver.h:103
SolverNode< State, GraphT >::Ptr SolverNodePtr
virtual bool getCloseList(std::vector< StatePtr > &lst, std::vector< float > &cost)
static WallTime now()
std::vector< typename SolverNode< State, GraphT >::Ptr > Path
virtual bool findInCloseList(SolverNodePtr node, double &cost)
virtual bool removeFromOpenList(SolverNodePtr node)
virtual bool findInCloseList(StatePtr state)
Definition: solver.h:110


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri May 14 2021 02:51:52