sample_simple_graph.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 #include <ros/ros.h>
36 
42 #include <iostream>
43 #include <boost/format.hpp>
45 
46 using namespace jsk_footstep_planner;
47 
48 int main(int argc, char** argv)
49 {
50  ros::init(argc, argv, "sample_simple_graph");
51  std::cout << "usage: rosrun jsk_footstep_planner sample_astar config/map.txt" << std::endl;
53 
54  // node name: depth_num
55  const int width = 10;
56  const int depth = 1000;
57  for (size_t d = 0; d < depth; d++) {
58  for (size_t w = 0; w < width; w++) {
59  SimpleNeighboredNode::Ptr a(new SimpleNeighboredNode((boost::format("%lu_%lu") % d % w).str()));
60  graph->addNode(a);
61  if (w != 0) {
62  graph->findNode((boost::format("%lu_%lu") % d % (w - 1)).str())->addNeighbor(graph->findNode((boost::format("%lu_%lu") % d % w).str()));
63  graph->findNode((boost::format("%lu_%lu") % d % w).str())->addNeighbor(graph->findNode((boost::format("%lu_%lu") % d % (w - 1)).str()));
64  }
65  if (d != 0) {
66  graph->findNode((boost::format("%lu_%lu") % (d - 1) % w).str())->addNeighbor(graph->findNode((boost::format("%lu_%lu") % d % w).str()));
67  }
68  }
69  }
70 
71  std::cout << "done building graph" << std::endl;
72  graph->setStartState(graph->findNode("0_0"));
73  graph->setGoalState(graph->findNode((boost::format("%lu_%lu") % (depth - 1) % (width - 1)).str()));
74  const int trials = 10;
75  {
77  for (size_t i = 0; i < trials; i++) {
80  std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
81  std::cout << "path: " << path.size() << std::endl;
82  //std::cout << i << "/" << trials << std::endl;
83  }
84  std::cout << "braedth first time to solve:" << acc.mean() << std::endl;
85  }
86 
87  {
89  for (size_t i = 0; i < trials; i++) {
92  std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
93  std::cout << "path: " << path.size() << std::endl;
94  //std::cout << i << "/" << trials << std::endl;
95  }
96  std::cout << "depth first time to solve:" << acc.mean() << std::endl;
97  }
98 
99  {
101  for (size_t i = 0; i < trials; i++) {
104  std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
105  std::cout << "path: " << path.size() << std::endl;
106  //std::cout << i << "/" << trials << std::endl;
107  }
108  std::cout << "best first time to solve:" << acc.mean() << std::endl;
109  }
110 
112  std::vector<SolverNode<SimpleNeighboredNode, SimpleNeighboredGraph>::Ptr> path = solver->solve();
113  std::cout << "number of nodes: " << graph->numNodes() << std::endl;
114  std::cout << "start node: " << graph->getStartState()->getName() << std::endl;
115  std::cout << "goal node: " << graph->getGoalState()->getName() << std::endl;
116  std::cout << "path: " << path.size() << std::endl;
117  for (size_t i = 0; i < path.size(); i++) {
118  std::cout << " " << i << " - " << path[i]->getState()->getName() << std::endl;
119  }
120  return 0;
121 }
d
timer
path
w
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
FootstepGraph::Ptr graph
string str
int main(int argc, char **argv)
char a[26]
virtual ScopedTimer scopedTimer()
int i
width


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:19