footstep_astar_solver.h
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 
00037 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_SOLVER_H_
00038 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_SOLVER_H_
00039 
00040 #include "jsk_footstep_planner/astar_solver.h"
00041 #include "jsk_footstep_planner/footstep_state_discrete_close_list.h"
00042 
00043 namespace jsk_footstep_planner
00044 {
00045   // Only available for FootstepState and FootstepGraph
00046   // because close list behavior is specialized for the purpose
00047   template <class GraphT>
00048   class FootstepAStarSolver: public AStarSolver<GraphT>
00049   {
00050   public:
00051     typedef boost::shared_ptr<FootstepAStarSolver> Ptr;
00052     typedef typename GraphT::StateT State;
00053     typedef typename GraphT::StateT::Ptr StatePtr;
00054     typedef typename GraphT::Ptr GraphPtr;
00055     typedef typename SolverNode<State, GraphT>::Ptr SolverNodePtr;
00056     typedef typename boost::function<void(FootstepAStarSolver&, GraphPtr)> ProfileFunction;
00057     typedef typename std::priority_queue<SolverNodePtr,
00058                                          std::vector<SolverNodePtr>,
00059                                          std::greater<SolverNodePtr> > OpenList;    
00060     FootstepAStarSolver(
00061       GraphPtr graph, size_t x_num, size_t y_num, size_t theta_num,
00062       unsigned int profile_period = 1024,
00063       double cost_weight = 1.0,
00064       double heuristic_weight = 1.0):
00065       footstep_close_list_(x_num, y_num, theta_num),
00066       profile_period_(profile_period),
00067       is_set_profile_function_(false),
00068       cost_weight_(cost_weight),
00069       heuristic_weight_(heuristic_weight),
00070       is_cancelled_(false),
00071       AStarSolver<GraphT>(graph)
00072     {
00073       
00074     }
00075 
00076     virtual double fn(SolverNodePtr n)
00077     {
00078       return cost_weight_ * gn(n) + heuristic_weight_ * hn(n);
00079     }
00080 
00081     
00082     virtual
00083     std::vector<typename SolverNode<State, GraphT>::Ptr>
00084     solve(const ros::WallDuration& timeout = ros::WallDuration(1000000000.0))
00085     {
00086       ros::WallTime start_time = ros::WallTime::now();
00087       SolverNodePtr start_state(new SolverNode<State, GraphT>(
00088                                   graph_->getStartState(),
00089                                   0, graph_));
00090       TransitionLimit::Ptr limit = graph_->getTransitionLimit();
00091       bool lazy_projection = graph_->lazyProjection();
00092       addToOpenList(start_state);
00093       while (!is_cancelled_ && !isOpenListEmpty()  && isOK(start_time, timeout)) {
00094         SolverNodePtr target_node = popFromOpenList();
00095         if (graph_->usePointCloudModel() && lazy_projection) {
00096           unsigned int error_state;
00097           FootstepState::Ptr projected_state = graph_->projectFootstep(target_node->getState(),
00098                                                                        error_state);
00099           if (!projected_state) { // failed to project footstep
00100             if (graph_->localMovement() && error_state == projection_state::close_to_success) {
00101               // try local movement
00102               std::vector<FootstepState::Ptr> locally_moved_states;
00103               {
00104                 std::vector<FootstepState::Ptr> states_candidates
00105                   = graph_->localMoveFootstepState(target_node->getState());
00106                 for (int i = 0; i < states_candidates.size(); i ++) {
00107                   FootstepGraph::StatePtr tmp_state = graph_->projectFootstep(states_candidates[i],
00108                                                                               error_state);
00109                   if (!!tmp_state) {
00110                     locally_moved_states.push_back(tmp_state);
00111                   }
00112                 }
00113               }
00114               if (locally_moved_states.size() > 0) {
00115                 std::vector<SolverNodePtr> locally_moved_nodes
00116                   = target_node->wrapWithSolverNodes(target_node->getParent(),
00117                                                      locally_moved_states);
00118                 for (size_t i = 0; i < locally_moved_nodes.size(); i++) {
00119 
00120                   if (graph_->isSuccessable(locally_moved_nodes[i]->getState(),
00121                                             target_node->getParent()->getState())) {
00122                     addToOpenList(locally_moved_nodes[i]);
00123                   }
00124                 }
00125               }
00126             }
00127             continue;           // back to the top of while loop
00128           }
00129           else {
00130             if (target_node->getParent()) {
00131               if (graph_->isSuccessable(projected_state, target_node->getParent()->getState())) {
00132                 target_node->setState(projected_state);
00133               }
00134               else {
00135                 continue;
00136               }
00137             }
00138             else {
00139               target_node->setState(projected_state);
00140             }
00141           }
00142         } //if (graph_->usePointCloudModel() && lazy_projection) {
00143         if (graph_->isGoal(target_node->getState())) {
00144           if (is_set_profile_function_) {
00145             profile_function_(*this, graph_);
00146           }
00147           std::vector<SolverNodePtr> result_path = target_node->getPathWithoutThis();
00148           result_path.push_back(target_node);
00149           return result_path;
00150         }
00151         else if (!findInCloseList(target_node->getState())) {
00152           addToCloseList(target_node->getState());
00153           std::vector<SolverNodePtr> next_nodes = target_node->expand(target_node, verbose_);
00154           // Add to open list only if next_nodes is not in close list.
00155           // We can do it thanks to FootstepStateDiscreteCloseList
00156           for (size_t i = 0; i < next_nodes.size(); i++) {
00157             SolverNodePtr next_node = next_nodes[i];
00158             if (!findInCloseList(next_node->getState())) {
00159               addToOpenList(next_node);
00160             }
00161           }
00162         }
00163       }
00164       if (is_cancelled_) {
00165         ROS_WARN("FootstepAStarSolver is cancelled");
00166       }
00167       // Failed to search
00168       return std::vector<SolverNodePtr>();
00169     }
00170     
00171     virtual void cancelSolve() {
00172       is_cancelled_ = true;
00173       ROS_FATAL("cancel planning");
00174     }
00175 
00176     // Overtake closelist behavior from solver class
00177     virtual bool findInCloseList(StatePtr s)
00178     {
00179       return footstep_close_list_.find(s);
00180     }
00181 
00182     virtual void addToCloseList(StatePtr s)
00183     {
00184       footstep_close_list_.push_back(s);
00185     }
00186 
00187     virtual OpenList getOpenList() { return open_list_; }
00188     
00189     virtual FootstepStateDiscreteCloseList getCloseList() { return footstep_close_list_; }
00190     virtual void setProfileFunction(ProfileFunction f)
00191     {
00192       profile_function_ = f;
00193       is_set_profile_function_ = true;
00194     }
00195 
00196     virtual bool isOK(const ros::WallTime& start_time, const ros::WallDuration& timeout)
00197     {
00198       if (is_set_profile_function_ && ++loop_counter_ % profile_period_ == 0) {
00199         profile_function_(*this, graph_);
00200         loop_counter_ = 0;
00201       }
00202       return (ros::ok() && (ros::WallTime::now() - start_time) < timeout);
00203     }
00204 
00205     template <class PointT>
00206     void closeListToPointCloud(pcl::PointCloud<PointT>& output_cloud)
00207     {
00208       footstep_close_list_.toPointCloud<PointT>(output_cloud);
00209     }
00210     
00211     template <class PointT>
00212     void openListToPointCloud(pcl::PointCloud<PointT>& output_cloud)
00213     {
00214       output_cloud.points.reserve(open_list_.size());
00215       OpenList copied_open_list = open_list_;
00216       
00217       while (copied_open_list.size() > 0)
00218       {
00219         SolverNodePtr solver_node = copied_open_list.top();
00220         StatePtr state = solver_node->getState();
00221         PointT p = ((FootstepState::Ptr)state)->toPoint<PointT>(); // hacky way
00222         output_cloud.points.push_back(p);
00223         copied_open_list.pop();
00224       }
00225     }
00226 
00227     void addToOpenList(SolverNodePtr node)
00228     {
00229       if (node->isRoot()) {
00230         BestFirstSearchSolver<GraphT>::addToOpenList(node);
00231       }
00232       else {
00233         if (node->getState()->crossCheck(
00234               node->getParent()->getState())) {
00235           BestFirstSearchSolver<GraphT>::addToOpenList(node);
00236         }
00237       }
00238     }
00239     
00240     using Solver<GraphT>::isOpenListEmpty;
00241     using Solver<GraphT>::popFromOpenList;
00242     using Solver<GraphT>::addToOpenList;
00243     using BestFirstSearchSolver<GraphT>::addToOpenList;
00244     using AStarSolver<GraphT>::gn;
00245     using AStarSolver<GraphT>::hn;
00246     
00247   protected:
00248     unsigned int loop_counter_;
00249     unsigned int profile_period_;
00250     ProfileFunction profile_function_;
00251     bool is_set_profile_function_;
00252     FootstepStateDiscreteCloseList footstep_close_list_;
00253     using Solver<GraphT>::graph_;
00254     using Solver<GraphT>::verbose_;
00255     using BestFirstSearchSolver<GraphT>::open_list_;
00256     const double cost_weight_;
00257     const double heuristic_weight_;
00258     bool is_cancelled_;
00259   };
00260 }
00261 
00262 #endif


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:28