solver.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef JSK_FOOTSTEP_PLANNER_SOLVER_H_
38 #define JSK_FOOTSTEP_PLANNER_SOLVER_H_
39 
42 #include <boost/unordered_map.hpp>
43 
44 namespace jsk_footstep_planner
45 {
46  template <class GraphT>
47  class Solver
48  {
49  public:
51  typedef typename GraphT::StateT State;
52  typedef typename GraphT::StateT::Ptr StatePtr;
53  typedef typename GraphT::Ptr GraphPtr;
55  typedef boost::unordered_map< StatePtr, double > SolveList;
56 
57  Solver(): verbose_(false) {};
58  Solver(GraphPtr graph): graph_(graph), verbose_(false) {}
59 
60  virtual void setVerbose(bool v) { verbose_ = v; }
61 
62  virtual
63  std::vector<typename SolverNode<State, GraphT>::Ptr>
64  solve(const ros::WallDuration& timeout = ros::WallDuration(1000000000.0))
65  {
67  SolverNodePtr start_state(new SolverNode<State, GraphT>(
68  graph_->getStartState(),
69  0, graph_));
70  addToOpenList(start_state);
71  while (!isOpenListEmpty() && isOK(start_time, timeout)) {
72  SolverNodePtr target_node = popFromOpenList();
73  if (graph_->isGoal(target_node->getState())) {
74  std::vector<SolverNodePtr> result_path = target_node->getPathWithoutThis();
75  result_path.push_back(target_node);
76  return result_path;
77  }
78  else if (!findInCloseList(target_node->getState())) {
79  //close_list_.push_back(target_node->getStnate());
80  addToCloseList(target_node->getState());
81  addToOpenList(target_node->expand(target_node, verbose_));
82  }
83  }
84  // Failed to search
85  return std::vector<SolverNodePtr>();
86  }
87 
88  virtual bool isOK(const ros::WallTime& start_time, const ros::WallDuration& timeout)
89  {
90  return (ros::ok() && (ros::WallTime::now() - start_time) < timeout);
91  }
92  virtual bool isOpenListEmpty() = 0;
93  virtual void addToOpenList(SolverNodePtr node) = 0;
94  virtual SolverNodePtr popFromOpenList() = 0;
95 
96  virtual void addToOpenList(std::vector<SolverNodePtr> nodes)
97  {
98  for (size_t i = 0; i < nodes.size(); i++) {
99  addToOpenList(nodes[i]);
100  }
101  }
102 
103  virtual void addToCloseList(StatePtr state, double cost = 0)
104  {
105  //close_list_.insert(state);
106  //close_list_[state] = cost;
108  .insert(typename SolveList::value_type(state, cost));
109  }
110  virtual bool findInCloseList(StatePtr state)
111  {
112  return close_list_.find(state) != close_list_.end();
113  }
114  virtual bool findInCloseList(StatePtr state, double &cost)
115  {
116 
117  typename SolveList::const_iterator it
118  = close_list_.find(state);
119  if (it != close_list_.end()) {
120  cost = it->second;
121  return true;
122  }
123  return false;
124  }
125  virtual bool removeFromCloseList(StatePtr state)
126  {
127  typename SolveList::const_iterator it
128  = close_list_.find(state);
129  if(it != close_list_.end()) {
130  close_list_.erase(it);
131  return true;
132  }
133  return false;
134  }
135  protected:
136  SolveList close_list_;
137  GraphPtr graph_;
138  bool verbose_;
139  private:
140 
141  };
142 }
143 
144 #endif
GraphT::StateT State
Definition: solver.h:51
virtual void setVerbose(bool v)
Definition: solver.h:60
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
Definition: solver.h:64
SolverNode< State, GraphT >::Ptr SolverNodePtr
Definition: solver.h:54
boost::unordered_map< StatePtr, double > SolveList
Definition: solver.h:55
virtual void addToOpenList(std::vector< SolverNodePtr > nodes)
Definition: solver.h:96
boost::shared_ptr< Solver > Ptr
Definition: solver.h:50
start_time
virtual bool isOK(const ros::WallTime &start_time, const ros::WallDuration &timeout)
Definition: solver.h:88
virtual bool removeFromCloseList(StatePtr state)
Definition: solver.h:125
ROSCPP_DECL bool ok()
FootstepGraph::Ptr graph
virtual void addToOpenList(SolverNodePtr node)=0
virtual void addToCloseList(StatePtr state, double cost=0)
Definition: solver.h:103
virtual SolverNodePtr popFromOpenList()=0
static WallTime now()
Solver(GraphPtr graph)
Definition: solver.h:58
virtual bool isOpenListEmpty()=0
GraphT::StateT::Ptr StatePtr
Definition: solver.h:52
virtual bool findInCloseList(StatePtr state, double &cost)
Definition: solver.h:114
int i
virtual bool findInCloseList(StatePtr state)
Definition: solver.h:110


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:19