37 #ifndef JSK_FOOTSTEP_PLANNER_SOLVER_NODE_H_ 38 #define JSK_FOOTSTEP_PLANNER_SOLVER_NODE_H_ 42 #include <boost/shared_ptr.hpp> 43 #include <boost/weak_ptr.hpp> 49 template <
class StateT,
class GraphT>
60 Ptr parent, GraphPtr
graph):
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];
81 solver_nodes.push_back(solver_node);
87 std::vector<Ptr>
expand(Ptr this_ptr,
bool verbose)
89 GraphPtr graph_ptr =
graph_.lock();
90 std::vector<Ptr> solver_nodes;
92 std::vector<StatePtr> successors = graph_ptr->successors(
state_);
94 std::cerr << successors.size() <<
" successors" << std::endl;
101 throw std::runtime_error(
"no graph is set in SolverNode");
113 std::vector<SolverNode::Ptr>
117 return std::vector<SolverNode::Ptr>();
120 std::vector<SolverNode::Ptr> parent_path =
parent_->getPathWithoutThis();
121 parent_path.push_back(
parent_);
128 friend bool operator<(const SolverNode<StateT, GraphT>::Ptr
a,
131 return a->getSortValue() <
b->getSortValue();
137 return a->getSortValue() > b->getSortValue();
152 #if 0 // Not using, but for testing 153 template <
class StateT,
class GraphT>
154 class SolverNodeSingleton :
public SolverNode< StateT, GraphT >
160 typedef typename GraphT::Ptr
GraphPtr;
165 SolverNodeSingleton(StatePtr
state,
const double cost,
166 Ptr parent, GraphPtr
graph) :
169 SolverNodeSingleton(StatePtr state,
const double cost, GraphPtr graph) :
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(
184 solver_nodes.push_back(solver_node);
193 return std::vector<Ptr>();
196 std::vector<Ptr> parent_path =
parent_->getPathWithoutThis();