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_SOLVER_H_
00038 #define JSK_FOOTSTEP_PLANNER_SOLVER_H_
00039 
00040 #include "jsk_footstep_planner/graph.h"
00041 #include "jsk_footstep_planner/solver_node.h"
00042 #include <boost/unordered_map.hpp>
00043 
00044 namespace jsk_footstep_planner
00045 {
00046   template <class GraphT>
00047   class Solver
00048   {
00049   public:
00050     typedef boost::shared_ptr<Solver> Ptr;
00051     typedef typename GraphT::StateT State;
00052     typedef typename GraphT::StateT::Ptr StatePtr;
00053     typedef typename GraphT::Ptr GraphPtr;
00054     typedef typename SolverNode<State, GraphT>::Ptr SolverNodePtr;
00055     typedef boost::unordered_map< StatePtr, double > SolveList;
00056 
00057     Solver(): verbose_(false) {};
00058     Solver(GraphPtr graph): graph_(graph), verbose_(false) {}
00059 
00060     virtual void setVerbose(bool v) { verbose_ = v; }
00061     
00062     virtual
00063     std::vector<typename SolverNode<State, GraphT>::Ptr>
00064     solve(const ros::WallDuration& timeout = ros::WallDuration(1000000000.0))
00065     {
00066       ros::WallTime start_time = ros::WallTime::now();
00067       SolverNodePtr start_state(new SolverNode<State, GraphT>(
00068                                   graph_->getStartState(),
00069                                   0, graph_));
00070       addToOpenList(start_state);
00071       while (!isOpenListEmpty() && isOK(start_time, timeout)) {
00072         SolverNodePtr target_node = popFromOpenList();
00073         if (graph_->isGoal(target_node->getState())) {
00074           std::vector<SolverNodePtr> result_path = target_node->getPathWithoutThis();
00075           result_path.push_back(target_node);
00076           return result_path;
00077         }
00078         else if (!findInCloseList(target_node->getState())) {
00079           //close_list_.push_back(target_node->getStnate());
00080           addToCloseList(target_node->getState());
00081           addToOpenList(target_node->expand(target_node, verbose_));
00082         }
00083       }
00084       // Failed to search
00085       return std::vector<SolverNodePtr>();
00086     }
00087     
00088     virtual bool isOK(const ros::WallTime& start_time, const ros::WallDuration& timeout)
00089     {
00090       return (ros::ok() && (ros::WallTime::now() - start_time) < timeout);
00091     }
00092     virtual bool isOpenListEmpty() = 0;
00093     virtual void addToOpenList(SolverNodePtr node) = 0;
00094     virtual SolverNodePtr popFromOpenList() = 0;
00095     
00096     virtual void addToOpenList(std::vector<SolverNodePtr> nodes)
00097     {
00098       for (size_t i = 0; i < nodes.size(); i++) {
00099         addToOpenList(nodes[i]);
00100       }
00101     }
00102 
00103     virtual void addToCloseList(StatePtr state, double cost = 0)
00104     {
00105       //close_list_.insert(state);
00106       //close_list_[state] = cost;
00107       close_list_
00108         .insert(typename SolveList::value_type(state, cost));
00109     }
00110     virtual bool findInCloseList(StatePtr state)
00111     {
00112       return close_list_.find(state) != close_list_.end();
00113     }
00114     virtual bool findInCloseList(StatePtr state, double &cost)
00115     {
00116 
00117       typename SolveList::const_iterator it
00118         = close_list_.find(state);
00119       if (it != close_list_.end()) {
00120         cost = it->second;
00121         return true;
00122       }
00123       return false;
00124     }
00125     virtual bool removeFromCloseList(StatePtr state)
00126     {
00127       typename SolveList::const_iterator it
00128         = close_list_.find(state);
00129       if(it != close_list_.end()) {
00130         close_list_.erase(it);
00131         return true;
00132       }
00133       return false;
00134     }
00135   protected:
00136     SolveList close_list_;
00137     GraphPtr graph_;
00138     bool verbose_;
00139   private:
00140     
00141   };
00142 }
00143 
00144 #endif


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