sample_astar.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 
37 #include <iostream>
38 #include <fstream>
39 #include <boost/format.hpp>
41 #include <set>
42 
43 using namespace jsk_footstep_planner;
44 
46 {
47 public:
49  Grid2DNode(int x, int y, bool occupied): x_(x), y_(y), occupied_(occupied) {}
50  bool operator==(Grid2DNode& other)
51  {
52  return x_ == other.x_ && y_ == other.y_ ;
53  }
54  int getX() { return x_; }
55  int getY() { return y_; }
56  bool isOccupied() { return occupied_; }
57 protected:
58  int x_;
59  int y_;
60  bool occupied_;
61 private:
62 
63 };
64 
65 class Grid2DGraph: public Graph<Grid2DNode>
66 {
67 public:
69  virtual std::vector<Grid2DGraph::StatePtr> successors(
70  StatePtr target_state)
71  {
72  std::vector<Grid2DGraph::StatePtr> ret;
73  if (target_state->getX() - 1 > 0) {
74  if (state_map_[target_state->getX() - 1][target_state->getY()] &&
75  !state_map_[target_state->getX() - 1][target_state->getY()]->isOccupied()) {
76  ret.push_back(state_map_[target_state->getX() - 1][target_state->getY()]);
77  }
78  }
79  if (target_state->getX() + 1 < 1024) {
80  if (state_map_[target_state->getX() + 1][target_state->getY()] &&
81  !state_map_[target_state->getX() + 1][target_state->getY()]->isOccupied()) {
82  ret.push_back(state_map_[target_state->getX() + 1][target_state->getY()]);
83  }
84  }
85  if (target_state->getY() - 1 > 0) {
86  if (state_map_[target_state->getX()][target_state->getY() - 1] &&
87  !state_map_[target_state->getX()][target_state->getY() - 1]->isOccupied()) {
88  ret.push_back(state_map_[target_state->getX()][target_state->getY() - 1]);
89  }
90  }
91  if (target_state->getY() + 1 < 1024) {
92  if (state_map_[target_state->getX()][target_state->getY() + 1] &&
93  !state_map_[target_state->getX()][target_state->getY() + 1]->isOccupied()) {
94  ret.push_back(state_map_[target_state->getX()][target_state->getY() + 1]);
95  }
96  }
97  return ret;
98  }
99 
100  virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
101  {
102  return prev_cost + 1;
103  }
104 
105  virtual bool isGoal(StatePtr state)
106  {
107  return goal_state_ == state;
108  }
109 
110  virtual void addNode(StatePtr state)
111  {
113  state_map_[state->getX()][state->getY()] = state;
114  }
115 
116  virtual StatePtr findNode(int x, int y)
117  {
118  return state_map_[x][y];
119  }
120 protected:
121  StatePtr state_map_[1024][1024];
122 private:
123 
124 };
125 
128 {
129  return 0;
130 }
131 
132 int main(int argc, char** argv)
133 {
134  ros::init(argc, argv, "sample_astar");
136  // parse file
137  std::string map_file = argv[1];
138  std::ifstream map_stream(map_file.c_str());
139  std::string input;
140  size_t j = 0;
141  while (std::getline(map_stream, input)) {
142  for (size_t i = 0; i < input.length(); i++) {
143  char c = input[i];
144  Grid2DNode::Ptr node(new Grid2DNode(i, j, c == 'x'));
145  graph->addNode(node);
146  }
147  ++j;
148  }
149  graph->setStartState(graph->findNode(1, 1));
150  graph->setGoalState(graph->findNode(61, 28));
151  {
153  const size_t trials = 1000;
154  for (size_t i = 0; i < trials; i++) {
156  AStarSolver<Grid2DGraph> solver(graph);
157  solver.setHeuristic(boost::bind(&heuristicFunction, _1, _2));
158  std::vector<SolverNode<Grid2DNode, Grid2DGraph>::Ptr> path
159  = solver.solve();
160  }
161  std::cout << "A* time to solve:" << acc.mean() << std::endl;
162  }
163  AStarSolver<Grid2DGraph> solver(graph);
164  solver.setHeuristic(boost::bind(&heuristicFunction, _1, _2));
165  std::vector<SolverNode<Grid2DNode, Grid2DGraph>::Ptr> path
166  = solver.solve();
167 
168  std::set<Grid2DNode::Ptr> path_states;
169  for (size_t i = 0; i < path.size(); i++) {
170  path_states.insert(path[i]->getState());
171  }
172  // print path
173  for (size_t j = 0; j < 1024; j++) {
174  bool is_valid_line = false;
175  for (size_t i = 0; i < 1024; i++) {
176  Grid2DNode::Ptr node = graph->findNode(i, j);
177  if (node) {
178  if (node->isOccupied()) {
179  std::cout << "x";
180  }
181  else if (*(graph->getStartState()) == *node) {
182  std::cout << "s";
183  }
184  else if (*(graph->getGoalState()) == *node) {
185  std::cout << "g";
186  }
187  else if (path_states.find(node) != path_states.end()) {
188  std::cout << "o";
189  }
190  else {
191  std::cout << " ";
192  }
193  is_valid_line = true;
194  }
195  }
196  if (is_valid_line) {
197  std::cout << std::endl;
198  }
199  }
200  return 0;
201 }
timer
path
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
Definition: solver.h:64
boost::shared_ptr< Grid2DGraph > Ptr
virtual bool isGoal(StatePtr state)
double heuristicFunction(AStarSolver< Grid2DGraph >::SolverNodePtr node, Grid2DGraph::Ptr graph)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool operator==(Grid2DNode &other)
c
virtual void addNode(StatePtr state)
Definition: graph.h:59
boost::shared_ptr< Grid2DNode > Ptr
virtual StatePtr findNode(int x, int y)
bool isOccupied()
virtual void setHeuristic(HeuristicFunction h)
Definition: astar_solver.h:71
int main(int argc, char **argv)
FootstepGraph::Ptr graph
virtual std::vector< Grid2DGraph::StatePtr > successors(StatePtr target_state)
virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
Grid2DNode(int x, int y, bool occupied)
virtual void addNode(StatePtr state)
virtual ScopedTimer scopedTimer()


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28