solver_node.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_NODE_H_
38 #define JSK_FOOTSTEP_PLANNER_SOLVER_NODE_H_
39 
40 #include <algorithm>
41 #include <iterator>
42 #include <boost/shared_ptr.hpp>
43 #include <boost/weak_ptr.hpp>
45 #include <ros/ros.h>
46 
47 namespace jsk_footstep_planner
48 {
49  template <class StateT, class GraphT>
50  class SolverNode
51  {
52  public:
55  typedef typename GraphT::Ptr GraphPtr;
56  typedef typename boost::weak_ptr<GraphT> GraphWeakPtr;
57  //typedef boost::unordered_map< StatePtr, Ptr > SolverList;
58 
59  SolverNode(StatePtr state, const double cost,
60  Ptr parent, GraphPtr graph):
61  cost_(cost), state_(state), parent_(parent), graph_(graph) {}
62 
63  SolverNode(StatePtr state, const double cost, GraphPtr graph):
64  cost_(cost), state_(state), graph_(graph) {}
65 
66  virtual
67  StatePtr getState() const { return state_; }
68 
69  virtual
70  std::vector<Ptr> wrapWithSolverNodes(Ptr this_ptr, std::vector<StatePtr> successors)
71  {
72  GraphPtr graph_ptr = graph_.lock();
73  std::vector<Ptr> solver_nodes;
74  for (size_t i = 0; i < successors.size(); i++) {
75  StatePtr next_state = successors[i];
76  SolverNode::Ptr solver_node(new SolverNode(
77  next_state,
78  graph_ptr->pathCost(state_, next_state, cost_),
79  this_ptr,
80  graph_ptr));
81  solver_nodes.push_back(solver_node);
82  }
83  return solver_nodes;
84  }
85 
86  virtual
87  std::vector<Ptr> expand(Ptr this_ptr, bool verbose)
88  {
89  GraphPtr graph_ptr = graph_.lock();
90  std::vector<Ptr> solver_nodes;
91  if (graph_ptr) {
92  std::vector<StatePtr> successors = graph_ptr->successors(state_);
93  if (verbose) {
94  std::cerr << successors.size() << " successors" << std::endl;
95  }
96  return wrapWithSolverNodes(this_ptr, successors);
97  }
98  else {
99  // TODO: should raise exception
100  //ROS_FATAL("no graph is set");
101  throw std::runtime_error("no graph is set in SolverNode");
102  }
103  return solver_nodes;
104  }
105 
106  bool isRoot() const { return !parent_; }
107  double getCost() const { return cost_; }
108  double getSortValue() const { return sort_value_; }
109  void setGraph(GraphPtr graph) { graph_ = graph; }
110  void setSortValue(double v) { sort_value_ = v; }
111  void setCost(double c) { cost_ = c; }
112 
113  std::vector<SolverNode::Ptr>
115  {
116  if (isRoot()) {
117  return std::vector<SolverNode::Ptr>();
118  }
119  else {
120  std::vector<SolverNode::Ptr> parent_path = parent_->getPathWithoutThis();
121  parent_path.push_back(parent_); // recursive?
122  return parent_path;
123  }
124  }
125 
126  virtual void setState(StatePtr state) { state_ = state; }
127 
128  friend bool operator<(const SolverNode<StateT, GraphT>::Ptr a,
130  {
131  return a->getSortValue() < b->getSortValue();
132  }
133 
136  {
137  return a->getSortValue() > b->getSortValue();
138  }
139 
140  virtual Ptr getParent() const { return parent_; }
141 
142  protected:
143  double cost_;
144  double sort_value_; // for best first search
145  StatePtr state_;
146  Ptr parent_;
147  GraphWeakPtr graph_;
148  //std::vector<SolverNode::Ptr> memoized_path_;
149  private:
150  };
151 
152 #if 0 // Not using, but for testing
153  template <class StateT, class GraphT>
154  class SolverNodeSingleton : public SolverNode< StateT, GraphT >
155  {
156  public:
160  typedef typename GraphT::Ptr GraphPtr;
161  typedef typename boost::weak_ptr<GraphT> GraphWeakPtr;
162 
164 
165  SolverNodeSingleton(StatePtr state, const double cost,
166  Ptr parent, GraphPtr graph) :
167  SolverNode< StateT, GraphT > (state, cost, parent, graph) {}
168 
169  SolverNodeSingleton(StatePtr state, const double cost, GraphPtr graph) :
170  SolverNode< StateT, GraphT >(state, cost, graph) {}
171 
172  virtual
173  std::vector<Ptr> wrapWithSolverNodes(Ptr this_ptr, std::vector<StatePtr> successors)
174  {
175  GraphPtr graph_ptr = graph_.lock();
176  std::vector<Ptr> solver_nodes;
177  for (size_t i = 0; i < successors.size(); i++) {
178  StatePtr next_state = successors[i];
179  Ptr solver_node(new SolverNodeSingleton(
180  next_state,
181  graph_ptr->pathCost(state_, next_state, cost_),
182  this_ptr,
183  graph_ptr));
184  solver_nodes.push_back(solver_node);
185  }
186  return solver_nodes;
187  }
188 
189  std::vector<Ptr>
191  {
192  if (isRoot()) {
193  return std::vector<Ptr>();
194  }
195  else {
196  std::vector<Ptr> parent_path = parent_->getPathWithoutThis();
197  parent_path.push_back(graph_->getNode(parent_)); // recursive?
198  return parent_path;
199  }
200  }
201  protected:
203  using SolverNode< StateT, GraphT >::sort_value_; // for best first search
207  private:
208  };
209 #endif
210 }
211 
212 #endif
boost::weak_ptr< GraphT > GraphWeakPtr
Definition: solver_node.h:56
virtual std::vector< Ptr > wrapWithSolverNodes(Ptr this_ptr, std::vector< StatePtr > successors)
Definition: solver_node.h:70
virtual Ptr getParent() const
Definition: solver_node.h:140
std::vector< SolverNode::Ptr > getPathWithoutThis()
Definition: solver_node.h:114
friend bool operator>(const SolverNode< StateT, GraphT >::Ptr a, const SolverNode< StateT, GraphT >::Ptr b)
Definition: solver_node.h:134
virtual StatePtr getState() const
Definition: solver_node.h:67
void setGraph(GraphPtr graph)
Definition: solver_node.h:109
state
FootstepGraph::Ptr graph
boost::shared_ptr< StateT > StatePtr
Definition: solver_node.h:54
virtual void setState(StatePtr state)
Definition: solver_node.h:126
boost::shared_ptr< SolverNode > Ptr
Definition: solver_node.h:53
virtual std::vector< Ptr > expand(Ptr this_ptr, bool verbose)
Definition: solver_node.h:87
SolverNode(StatePtr state, const double cost, Ptr parent, GraphPtr graph)
Definition: solver_node.h:59
char a[26]
SolverNode(StatePtr state, const double cost, GraphPtr graph)
Definition: solver_node.h:63
int i


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