sample_simple_graph.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 #include <ros/ros.h>
00036 
00037 #include "jsk_footstep_planner/simple_neighbored_graph.h"
00038 #include "jsk_footstep_planner/breadth_first_search_solver.h"
00039 #include "jsk_footstep_planner/depth_first_search_solver.h"
00040 #include "jsk_footstep_planner/best_first_search_solver.h"
00041 #include "jsk_footstep_planner/astar_solver.h"
00042 #include <iostream>
00043 #include <boost/format.hpp>
00044 #include <jsk_topic_tools/time_accumulator.h>
00045 
00046 using namespace jsk_footstep_planner;
00047 
00048 int main(int argc, char** argv)
00049 {
00050   ros::init(argc, argv, "sample_simple_graph");
00051   std::cout << "usage: rosrun jsk_footstep_planner sample_astar config/map.txt" << std::endl;
00052   SimpleNeighboredGraph::Ptr graph (new SimpleNeighboredGraph);
00053 
00054   // node name: depth_num
00055   const int width = 10;
00056   const int depth = 1000;
00057   for (size_t d = 0; d < depth; d++) {
00058     for (size_t w = 0; w < width; w++) {
00059       SimpleNeighboredNode::Ptr a(new SimpleNeighboredNode((boost::format("%lu_%lu") % d % w).str()));
00060       graph->addNode(a);
00061       if (w != 0) {
00062         graph->findNode((boost::format("%lu_%lu") % d % (w - 1)).str())->addNeighbor(graph->findNode((boost::format("%lu_%lu") % d % w).str()));
00063         graph->findNode((boost::format("%lu_%lu") % d % w).str())->addNeighbor(graph->findNode((boost::format("%lu_%lu") % d % (w - 1)).str()));
00064       }
00065       if (d != 0) {
00066         graph->findNode((boost::format("%lu_%lu") % (d - 1) % w).str())->addNeighbor(graph->findNode((boost::format("%lu_%lu") % d % w).str()));
00067       }
00068     }
00069   }
00070 
00071   std::cout << "done building graph" << std::endl;
00072   graph->setStartState(graph->findNode("0_0"));
00073   graph->setGoalState(graph->findNode((boost::format("%lu_%lu") % (depth - 1) % (width - 1)).str()));
00074   const int trials = 10;
00075   {
00076     jsk_topic_tools::TimeAccumulator acc;
00077     for (size_t i = 0; i < trials; i++) {
00078       jsk_topic_tools::ScopedTimer timer = acc.scopedTimer();
00079       BreadthFirstSearchSolver<SimpleNeighboredGraph>::Ptr solver(new BreadthFirstSearchSolver<SimpleNeighboredGraph>(graph));
00080       std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
00081       std::cout << "path: " << path.size() << std::endl;
00082       //std::cout << i << "/" << trials << std::endl;
00083     }
00084     std::cout << "braedth first time to solve:" << acc.mean() << std::endl;
00085   }
00086 
00087   {
00088     jsk_topic_tools::TimeAccumulator acc;
00089     for (size_t i = 0; i < trials; i++) {
00090       jsk_topic_tools::ScopedTimer timer = acc.scopedTimer();
00091       DepthFirstSearchSolver<SimpleNeighboredGraph>::Ptr solver(new DepthFirstSearchSolver<SimpleNeighboredGraph>(graph));
00092       std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
00093       std::cout << "path: " << path.size() << std::endl;
00094       //std::cout << i << "/" << trials << std::endl;
00095     }
00096     std::cout << "depth first time to solve:" << acc.mean() << std::endl;
00097   }
00098 
00099   {
00100     jsk_topic_tools::TimeAccumulator acc;
00101     for (size_t i = 0; i < trials; i++) {
00102       jsk_topic_tools::ScopedTimer timer = acc.scopedTimer();
00103       BestFirstSearchSolver<SimpleNeighboredGraph>::Ptr solver(new BestFirstSearchSolver<SimpleNeighboredGraph>(graph));
00104       std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
00105       std::cout << "path: " << path.size() << std::endl;
00106       //std::cout << i << "/" << trials << std::endl;
00107     }
00108     std::cout << "best first time to solve:" << acc.mean() << std::endl;
00109   }
00110   
00111   BreadthFirstSearchSolver<SimpleNeighboredGraph>::Ptr solver(new BreadthFirstSearchSolver<SimpleNeighboredGraph>(graph));
00112   std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
00113   std::cout << "number of nodes: " << graph->numNodes() << std::endl;
00114   std::cout << "start node: " << graph->getStartState()->getName() << std::endl;
00115   std::cout << "goal node: " << graph->getGoalState()->getName() << std::endl;
00116   std::cout << "path: " << path.size() << std::endl;
00117   for (size_t i = 0; i < path.size(); i++) {
00118     std::cout << " " << i << " - " << path[i]->getState()->getName() << std::endl;
00119   }
00120   return 0;
00121 }


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