sample_astar.cpp
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 #include "jsk_footstep_planner/astar_solver.h"
00037 #include <iostream>
00038 #include <fstream>
00039 #include <boost/format.hpp>
00040 #include <jsk_topic_tools/time_accumulator.h>
00041 #include <set>
00042 
00043 using namespace jsk_footstep_planner;
00044 
00045 class Grid2DNode
00046 {
00047 public:
00048   typedef boost::shared_ptr<Grid2DNode> Ptr;
00049   Grid2DNode(int x, int y, bool occupied): x_(x), y_(y), occupied_(occupied) {}
00050   bool operator==(Grid2DNode& other)
00051   {
00052     return x_ == other.x_ && y_ == other.y_ ;
00053   }
00054   int getX() { return x_; }
00055   int getY() { return y_; }
00056   bool isOccupied() { return occupied_; }
00057 protected:
00058   int x_;
00059   int y_;
00060   bool occupied_;
00061 private:
00062   
00063 };
00064 
00065 class Grid2DGraph: public Graph<Grid2DNode>
00066 {
00067 public:
00068   typedef boost::shared_ptr<Grid2DGraph> Ptr;
00069   virtual std::vector<Grid2DGraph::StatePtr> successors(
00070     StatePtr target_state)
00071   {
00072     std::vector<Grid2DGraph::StatePtr> ret;
00073     if (target_state->getX() - 1 > 0) {
00074       if (state_map_[target_state->getX() - 1][target_state->getY()] &&
00075           !state_map_[target_state->getX() - 1][target_state->getY()]->isOccupied()) {
00076         ret.push_back(state_map_[target_state->getX() - 1][target_state->getY()]);
00077       }
00078     }
00079     if (target_state->getX() + 1 < 1024) {
00080       if (state_map_[target_state->getX() + 1][target_state->getY()] &&
00081           !state_map_[target_state->getX() + 1][target_state->getY()]->isOccupied()) {
00082         ret.push_back(state_map_[target_state->getX() + 1][target_state->getY()]);
00083       }
00084     }
00085     if (target_state->getY() - 1 > 0) {
00086       if (state_map_[target_state->getX()][target_state->getY() - 1] &&
00087           !state_map_[target_state->getX()][target_state->getY() - 1]->isOccupied()) {
00088         ret.push_back(state_map_[target_state->getX()][target_state->getY() - 1]);
00089       }
00090     }
00091     if (target_state->getY() + 1 < 1024) {
00092       if (state_map_[target_state->getX()][target_state->getY() + 1] &&
00093           !state_map_[target_state->getX()][target_state->getY() + 1]->isOccupied()) {
00094         ret.push_back(state_map_[target_state->getX()][target_state->getY() + 1]);
00095       }
00096     }
00097     return ret;
00098   }
00099   
00100   virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
00101   {
00102     return prev_cost + 1;
00103   }
00104 
00105   virtual bool isGoal(StatePtr state)
00106   {
00107     return goal_state_ == state;
00108   }
00109   
00110   virtual void addNode(StatePtr state)
00111   {
00112     Graph<Grid2DNode>::addNode(state);
00113     state_map_[state->getX()][state->getY()] = state;
00114   }
00115 
00116   virtual StatePtr findNode(int x, int y)
00117   {
00118     return state_map_[x][y];
00119   }
00120 protected:
00121   StatePtr state_map_[1024][1024];
00122 private:
00123   
00124 };
00125 
00126 double heuristicFunction(AStarSolver<Grid2DGraph>::SolverNodePtr node,
00127                          Grid2DGraph::Ptr graph)
00128 {
00129   return 0;
00130 }
00131 
00132 int main(int argc, char** argv)
00133 {
00134   ros::init(argc, argv, "sample_astar");
00135   Grid2DGraph::Ptr graph(new Grid2DGraph);
00136   // parse file
00137   std::string map_file = argv[1];
00138   std::ifstream map_stream(map_file.c_str());
00139   std::string input;
00140   size_t j = 0;
00141   while (std::getline(map_stream, input)) {
00142     for (size_t i = 0; i < input.length(); i++) {
00143       char c = input[i];
00144       Grid2DNode::Ptr node(new Grid2DNode(i, j, c == 'x'));
00145       graph->addNode(node);
00146     }
00147     ++j;
00148   }
00149   graph->setStartState(graph->findNode(1, 1));
00150   graph->setGoalState(graph->findNode(61, 28));
00151   {
00152     jsk_topic_tools::TimeAccumulator acc;
00153     const size_t trials = 1000;
00154     for (size_t i = 0; i < trials; i++) {
00155       jsk_topic_tools::ScopedTimer timer = acc.scopedTimer();
00156       AStarSolver<Grid2DGraph> solver(graph);
00157       solver.setHeuristic(boost::bind(&heuristicFunction, _1, _2));
00158       std::vector<SolverNode<Grid2DNode, Grid2DGraph>::Ptr> path
00159         = solver.solve();
00160     }
00161     std::cout << "A* time to solve:" << acc.mean() << std::endl;
00162   }
00163   AStarSolver<Grid2DGraph> solver(graph);
00164   solver.setHeuristic(boost::bind(&heuristicFunction, _1, _2));
00165   std::vector<SolverNode<Grid2DNode, Grid2DGraph>::Ptr> path
00166     = solver.solve();
00167 
00168   std::set<Grid2DNode::Ptr> path_states;
00169   for (size_t i = 0; i < path.size(); i++) {
00170     path_states.insert(path[i]->getState());
00171   }
00172   // print path
00173   for (size_t j = 0; j < 1024; j++) {
00174     bool is_valid_line = false;
00175     for (size_t i = 0; i < 1024; i++) {
00176       Grid2DNode::Ptr node = graph->findNode(i, j);
00177       if (node) {
00178         if (node->isOccupied()) {
00179           std::cout << "x";
00180         }
00181         else if (*(graph->getStartState()) == *node) {
00182           std::cout << "s";
00183         }
00184         else if (*(graph->getGoalState()) == *node) {
00185           std::cout << "g";
00186         }
00187         else if (path_states.find(node) != path_states.end()) {
00188           std::cout << "o";
00189         }
00190         else {
00191           std::cout << " ";
00192         }
00193         is_valid_line = true;
00194       }
00195     }
00196     if (is_valid_line) {
00197       std::cout << std::endl;
00198     }
00199   }
00200   return 0;
00201 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Sep 16 2015 04:37:57