grid_astar_solver.h
Go to the documentation of this file.
00001 #ifndef JSK_FOOTSTEP_PLANNER_GRID_ASTAR_SOLVER_H_
00002 #define JSK_FOOTSTEP_PLANNER_GRID_ASTAR_SOLVER_H_
00003 
00004 #include "jsk_footstep_planner/astar_solver.h"
00005 #include <boost/unordered_map.hpp>
00006 #include <cfloat> // DBL_MAX
00007 
00008 namespace jsk_footstep_planner
00009 {
00010   template <class GraphT>
00011   class GridAStarSolver: public AStarSolver<GraphT>
00012   {
00013   public:
00014     typedef boost::shared_ptr<GridAStarSolver> Ptr;
00015     typedef typename GraphT::StateT State;
00016     typedef typename GraphT::StateT::Ptr StatePtr;
00017     typedef typename GraphT::Ptr GraphPtr;
00018     typedef typename SolverNode<State, GraphT>::Ptr SolverNodePtr;
00019     typedef std::vector<typename SolverNode<State, GraphT>::Ptr> Path;
00020     typedef boost::unordered_map< StatePtr, SolverNodePtr > SolveList;
00021 
00022     GridAStarSolver(GraphPtr graph): AStarSolver<GraphT>(graph) {}
00023 
00024     virtual
00025     std::vector<typename SolverNode<State, GraphT>::Ptr>
00026     solve(const ros::WallDuration& timeout = ros::WallDuration(1000000000.0))
00027     {
00028       ros::WallTime start_time = ros::WallTime::now();
00029       SolverNodePtr start_state(new SolverNode<State, GraphT>
00030                                 (graph_->getStartState(),
00031                                  0, graph_));
00032       addToOpenList(start_state);
00033       if (verbose_) { std::cerr << "start" << std::endl; }
00034       //while (!isOpenListEmpty() && isOK(start_time, timeout)) {
00035       while (!isOpenListEmpty()) {
00036         SolverNodePtr target_node = popFromOpenList();
00037         StatePtr p = target_node->getState();
00038         if (verbose_) { std::cerr << "t " << p->indexX() << " - " << p->indexY(); }
00039         if (graph_->isGoal(target_node->getState())) {
00040           if (verbose_) { std::cerr << " -> g" << std::endl; }
00041           std::vector<SolverNodePtr> result_path = target_node->getPathWithoutThis();
00042           result_path.push_back(target_node);
00043           return result_path;
00044         } else {
00045           std::vector<SolverNodePtr> nnodes = target_node->expand(target_node, verbose_);
00046           if (verbose_) { std::cerr << " -> c "; }
00047           addToCloseList(target_node);
00048           for(int i = 0; i < nnodes.size(); i++) {
00049             SolverNodePtr anode = nnodes[i];
00050             double prev_cost = 0;
00051             if(verbose_) {
00052               std::cerr << "a-" << i << " " << anode->getState()->indexX();
00053               std::cerr << " - " << anode->getState()->indexY();
00054             }
00055             if(findInOpenList(anode, prev_cost)) {
00056               if(verbose_) { std::cerr << " -o"; }
00057               if(anode->getCost() < prev_cost) {
00058                 if(verbose_) { std::cerr << "-r"; }
00059                 removeFromOpenList(anode); // replace
00060                 addToOpenList(anode);
00061               }
00062             } else if(findInCloseList(anode, prev_cost)) {
00063               if(verbose_) { std::cerr << " -c"; }
00064               if(anode->getCost() < prev_cost) {
00065                 if(verbose_) { std::cerr << "-r"; }
00066                 removeFromCloseList(anode);
00067                 addToOpenList(anode);
00068               }
00069             } else {
00070               if(verbose_) { std::cerr << " -n"; }
00071               addToOpenList(anode);
00072             }
00073             if (verbose_) {
00074               if (i != nnodes.size() -1) {
00075                 std::cerr << " / ";
00076               }
00077             }
00078           }
00079         }
00080         if (verbose_) { std::cerr << std::endl; }
00081       }
00082       // Failed to search
00083       return std::vector<SolverNodePtr>();
00084     }
00085 
00087     virtual bool isOpenListEmpty()
00088     {
00089       return open_list_map_.empty();
00090     }
00091     virtual SolverNodePtr popFromOpenList()
00092     {
00093       // slow implimentation??
00094       double min_cost = DBL_MAX;
00095       SolverNodePtr ret;
00096       for (typename SolveList::const_iterator it = open_list_map_.begin();
00097            it != open_list_map_.end(); it++) {
00098         if (it->second->getSortValue() < min_cost) {
00099           ret = it->second;
00100           min_cost = it->second->getSortValue();
00101         }
00102       }
00103       if (!!ret) {
00104         typename SolveList::const_iterator it
00105           = open_list_map_.find(ret->getState());
00106         open_list_map_.erase(it);
00107         // remove from sorted_list(pop)
00108       }
00109       return ret;
00110     }
00111     virtual void addToOpenList(SolverNodePtr node)
00112     {
00113       typename SolveList::const_iterator it
00114         = open_list_map_.find(node->getState());
00115       if(it != open_list_map_.end()) {
00116         std::cerr << ";;;; warn (duplicate adding to openlist) ;;;;" << std::endl;
00117       }
00118       node->setSortValue(AStarSolver<GraphT>::fn(node));
00119       open_list_map_
00120         .insert(typename SolveList::value_type(node->getState(), node));
00121       // add to sorted_list
00122     }
00123     virtual bool findInOpenList(SolverNodePtr node, double &cost)
00124     {
00125       typename SolveList::const_iterator it
00126         = open_list_map_.find(node->getState());
00127       if (it != open_list_map_.end()) {
00128         cost = it->second->getCost();
00129         return true;
00130       }
00131       return false;
00132     }
00133     virtual bool removeFromOpenList(SolverNodePtr node)
00134     {
00135       typename SolveList::const_iterator it
00136         = open_list_map_.find(node->getState());
00137       if(it != open_list_map_.end()) {
00138         open_list_map_.erase(it);
00139         // remove from sorted_list(reconstruct)
00140         return true;
00141       } else {
00142         std::cerr << ";;;; warn (fail remove) ;;;;" << std::endl;
00143       }
00144       return false;
00145     }
00146     virtual void addToCloseList(SolverNodePtr node)
00147     {
00148       // check
00149       if(AStarSolver<GraphT>::findInCloseList(node->getState())) {
00150         std::cerr << ";;;; warn (duplicate adding to closelist) ;;;;" << std::endl;
00151       }
00152       //
00153       AStarSolver<GraphT>::addToCloseList(node->getState(), node->getCost());
00154     }
00155     virtual bool findInCloseList(SolverNodePtr node, double &cost)
00156     {
00157       return AStarSolver<GraphT>::findInCloseList(node->getState(), cost);
00158     }
00159     virtual bool removeFromCloseList(SolverNodePtr node)
00160     {
00161       return AStarSolver<GraphT>::removeFromCloseList(node->getState());
00162     }
00164     virtual bool getOpenList(std::vector<StatePtr> &lst, std::vector<float> &cost)
00165     {
00166       return false;
00167     }
00168     virtual bool getCloseList(std::vector<StatePtr> &lst, std::vector<float> &cost)
00169     {
00170       return false;
00171     }
00172 
00173   protected:
00174     SolveList open_list_map_;
00175     using Solver<GraphT>::graph_;
00176     using Solver<GraphT>::verbose_;
00177   private:
00178 
00179   };
00180 }
00181 #endif


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