37 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_SOLVER_H_ 38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_SOLVER_H_ 47 template <
class GraphT>
52 typedef typename GraphT::StateT
State;
56 typedef typename boost::function<void(FootstepAStarSolver&, GraphPtr)>
ProfileFunction;
58 std::vector<SolverNodePtr>,
61 GraphPtr
graph,
size_t x_num,
size_t y_num,
size_t theta_num,
62 unsigned int profile_period = 1024,
63 double cost_weight = 1.0,
64 double heuristic_weight = 1.0):
76 virtual double fn(SolverNodePtr n)
83 std::vector<typename SolverNode<State, GraphT>::Ptr>
91 bool lazy_projection =
graph_->lazyProjection();
95 if (
graph_->usePointCloudModel() && lazy_projection) {
96 unsigned int error_state;
97 StatePtr projected_state =
graph_->projectFootstep(target_node->getState(),
99 if (!projected_state) {
102 std::vector<StatePtr> locally_moved_states;
104 std::vector<StatePtr> states_candidates
105 =
graph_->localMoveFootstepState(target_node->getState());
106 for (
int i = 0; i < states_candidates.size(); i ++) {
107 StatePtr tmp_state =
graph_->projectFootstep(states_candidates[i],
110 locally_moved_states.push_back(tmp_state);
114 if (locally_moved_states.size() > 0) {
115 std::vector<SolverNodePtr> locally_moved_nodes
116 = target_node->wrapWithSolverNodes(target_node->getParent(),
117 locally_moved_states);
118 for (
size_t i = 0; i < locally_moved_nodes.size(); i++) {
120 if (
graph_->isSuccessable(locally_moved_nodes[i]->getState(),
121 target_node->getParent()->getState())) {
130 if (target_node->getParent()) {
131 if (
graph_->isSuccessable(projected_state, target_node->getParent()->getState())) {
132 target_node->setState(projected_state);
139 target_node->setState(projected_state);
143 if (
graph_->isGoal(target_node->getState())) {
147 std::vector<SolverNodePtr> result_path = target_node->getPathWithoutThis();
148 result_path.push_back(target_node);
153 std::vector<SolverNodePtr> next_nodes = target_node->expand(target_node,
verbose_);
156 for (
size_t i = 0; i < next_nodes.size(); i++) {
157 SolverNodePtr next_node = next_nodes[i];
165 ROS_WARN(
"FootstepAStarSolver is cancelled");
168 return std::vector<SolverNodePtr>();
205 template <
class Po
intT>
211 template <
class Po
intT>
214 output_cloud.points.reserve(
open_list_.size());
217 while (copied_open_list.size() > 0)
219 SolverNodePtr solver_node = copied_open_list.top();
220 StatePtr
state = solver_node->getState();
221 PointT p = state->template toPoint<PointT>();
222 output_cloud.points.push_back(p);
223 copied_open_list.pop();
229 if (node->isRoot()) {
233 if (node->getState()->crossCheck(
234 node->getParent()->getState())) {