footstep_astar_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_FOOTSTEP_SOLVER_H_
38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_SOLVER_H_
39 
42 
43 namespace jsk_footstep_planner
44 {
45  // Only available for FootstepState and FootstepGraph
46  // because close list behavior is specialized for the purpose
47  template <class GraphT>
48  class FootstepAStarSolver: public AStarSolver<GraphT>
49  {
50  public:
52  typedef typename GraphT::StateT State;
53  typedef typename GraphT::StateT::Ptr StatePtr;
54  typedef typename GraphT::Ptr GraphPtr;
56  typedef typename boost::function<void(FootstepAStarSolver&, GraphPtr)> ProfileFunction;
57  typedef typename std::priority_queue<SolverNodePtr,
58  std::vector<SolverNodePtr>,
59  std::greater<SolverNodePtr> > OpenList;
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):
65  footstep_close_list_(x_num, y_num, theta_num),
66  profile_period_(profile_period),
68  cost_weight_(cost_weight),
69  heuristic_weight_(heuristic_weight),
70  is_cancelled_(false),
71  AStarSolver<GraphT>(graph)
72  {
73 
74  }
75 
76  virtual double fn(SolverNodePtr n)
77  {
78  return cost_weight_ * gn(n) + heuristic_weight_ * hn(n);
79  }
80 
81 
82  virtual
83  std::vector<typename SolverNode<State, GraphT>::Ptr>
84  solve(const ros::WallDuration& timeout = ros::WallDuration(1000000000.0))
85  {
86  ros::WallTime start_time = ros::WallTime::now();
87  SolverNodePtr start_state(new SolverNode<State, GraphT>(
88  graph_->getStartState(),
89  0, graph_));
90  TransitionLimit::Ptr limit = graph_->getTransitionLimit();
91  bool lazy_projection = graph_->lazyProjection();
92  addToOpenList(start_state);
93  while (!is_cancelled_ && !isOpenListEmpty() && isOK(start_time, timeout)) {
94  SolverNodePtr target_node = popFromOpenList();
95  if (graph_->usePointCloudModel() && lazy_projection) {
96  unsigned int error_state;
97  StatePtr projected_state = graph_->projectFootstep(target_node->getState(),
98  error_state);
99  if (!projected_state) { // failed to project footstep
100  if (graph_->localMovement() && error_state == projection_state::close_to_success) {
101  // try local movement
102  std::vector<StatePtr> locally_moved_states;
103  {
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],
108  error_state);
109  if (!!tmp_state) {
110  locally_moved_states.push_back(tmp_state);
111  }
112  }
113  }
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++) {
119 
120  if (graph_->isSuccessable(locally_moved_nodes[i]->getState(),
121  target_node->getParent()->getState())) {
122  addToOpenList(locally_moved_nodes[i]);
123  }
124  }
125  }
126  }
127  continue; // back to the top of while loop
128  }
129  else {
130  if (target_node->getParent()) {
131  if (graph_->isSuccessable(projected_state, target_node->getParent()->getState())) {
132  target_node->setState(projected_state);
133  }
134  else {
135  continue;
136  }
137  }
138  else {
139  target_node->setState(projected_state);
140  }
141  }
142  } //if (graph_->usePointCloudModel() && lazy_projection) {
143  if (graph_->isGoal(target_node->getState())) {
145  profile_function_(*this, graph_);
146  }
147  std::vector<SolverNodePtr> result_path = target_node->getPathWithoutThis();
148  result_path.push_back(target_node);
149  return result_path;
150  }
151  else if (!findInCloseList(target_node->getState())) {
152  addToCloseList(target_node->getState());
153  std::vector<SolverNodePtr> next_nodes = target_node->expand(target_node, verbose_);
154  // Add to open list only if next_nodes is not in close list.
155  // We can do it thanks to FootstepStateDiscreteCloseList
156  for (size_t i = 0; i < next_nodes.size(); i++) {
157  SolverNodePtr next_node = next_nodes[i];
158  if (!findInCloseList(next_node->getState())) {
159  addToOpenList(next_node);
160  }
161  }
162  }
163  }
164  if (is_cancelled_) {
165  ROS_WARN("FootstepAStarSolver is cancelled");
166  }
167  // Failed to search
168  return std::vector<SolverNodePtr>();
169  }
170 
171  virtual void cancelSolve() {
172  is_cancelled_ = true;
173  ROS_FATAL("cancel planning");
174  }
175 
176  // Overtake closelist behavior from solver class
177  virtual bool findInCloseList(StatePtr s)
178  {
179  return footstep_close_list_.find(s);
180  }
181 
182  virtual void addToCloseList(StatePtr s)
183  {
185  }
186 
187  virtual OpenList getOpenList() { return open_list_; }
188 
190  virtual void setProfileFunction(ProfileFunction f)
191  {
192  profile_function_ = f;
194  }
195 
196  virtual bool isOK(const ros::WallTime& start_time, const ros::WallDuration& timeout)
197  {
199  profile_function_(*this, graph_);
200  loop_counter_ = 0;
201  }
202  return (ros::ok() && (ros::WallTime::now() - start_time) < timeout);
203  }
204 
205  template <class PointT>
206  void closeListToPointCloud(pcl::PointCloud<PointT>& output_cloud)
207  {
209  }
210 
211  template <class PointT>
212  void openListToPointCloud(pcl::PointCloud<PointT>& output_cloud)
213  {
214  output_cloud.points.reserve(open_list_.size());
215  OpenList copied_open_list = open_list_;
216 
217  while (copied_open_list.size() > 0)
218  {
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();
224  }
225  }
226 
227  void addToOpenList(SolverNodePtr node)
228  {
229  if (node->isRoot()) {
231  }
232  else {
233  if (node->getState()->crossCheck(
234  node->getParent()->getState())) {
236  }
237  }
238  }
239 
246 
247  protected:
248  unsigned int loop_counter_;
249  unsigned int profile_period_;
250  ProfileFunction profile_function_;
256  const double cost_weight_;
257  const double heuristic_weight_;
259  };
260 }
261 
262 #endif
boost::function< void(FootstepAStarSolver &, GraphPtr)> ProfileFunction
virtual void setProfileFunction(ProfileFunction f)
FootstepStateDiscreteCloseList footstep_close_list_
#define ROS_FATAL(...)
virtual FootstepStateDiscreteCloseList getCloseList()
void closeListToPointCloud(pcl::PointCloud< PointT > &output_cloud)
virtual double hn(SolverNodePtr n)
Definition: astar_solver.h:66
#define ROS_WARN(...)
void openListToPointCloud(pcl::PointCloud< PointT > &output_cloud)
boost::shared_ptr< FootstepAStarSolver > Ptr
virtual bool isOK(const ros::WallTime &start_time, const ros::WallDuration &timeout)
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
ROSCPP_DECL bool ok()
state
FootstepGraph::Ptr graph
pcl::PointXYZRGB PointT
static WallTime now()
std::priority_queue< SolverNodePtr, std::vector< SolverNodePtr >, std::greater< SolverNodePtr > > OpenList
FootstepStateDiscreteCloseList is a special clas to use for close list of FootstepState.
virtual double gn(SolverNodePtr n)
Definition: astar_solver.h:61
FootstepAStarSolver(GraphPtr graph, size_t x_num, size_t y_num, size_t theta_num, unsigned int profile_period=1024, double cost_weight=1.0, double heuristic_weight=1.0)
SolverNode< State, GraphT >::Ptr SolverNodePtr
virtual void addToOpenList(SolverNodePtr node)


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28