Fawkes API  Fawkes Development Version
search_state.h
1 /***************************************************************************
2  * search_state.h - Graph-based global path planning - A-Star search state
3  *
4  * Created: Tue Sep 18 18:14:58 2012
5  * Copyright 2012 Tim Niemueller [www.niemueller.de]
6  * 2002 Stefan Jacobs
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef _LIBS_NAVGRAPH_SEARCH_STATE_H_
23 #define _LIBS_NAVGRAPH_SEARCH_STATE_H_
24 
25 #include <core/utils/lockptr.h>
26 #include <navgraph/constraints/constraint_repo.h>
27 #include <navgraph/navgraph.h>
28 #include <utils/search/astar_state.h>
29 
30 #include <cmath>
31 #include <functional>
32 
33 namespace fawkes {
34 
36 {
37 public:
39  const fawkes::NavGraphNode & goal,
40  fawkes::NavGraph * map_graph,
41  fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
42 
44  const fawkes::NavGraphNode &goal,
45  fawkes::NavGraph * map_graph,
46  navgraph::EstimateFunction estimate_func,
47  navgraph::CostFunction cost_func = NavGraphSearchState::euclidean_cost,
48  fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
49 
51 
53 
54  virtual size_t
55  key()
56  {
57  return key_;
58  }
59  virtual float estimate();
60  virtual bool is_goal();
61 
62  /** Determine euclidean cost between two nodes.
63  * Note that the given notes are assumed to be adjacent nodes.
64  * @param from originating node
65  * @param to destination node
66  * @return cost from @p from to @p to.
67  */
68  static float
70  {
71  return sqrtf(powf(to.x() - from.x(), 2) + powf(to.y() - from.y(), 2));
72  }
73 
74  /** Determine straight line estimate between two nodes.
75  * @param node node to query heuristic value for
76  * @param goal goal node to get estimate for
77  * @return estimate of cost from @p node to @p goal.
78  */
79  static float
81  {
82  return sqrtf(powf(goal.x() - node.x(), 2) + powf(goal.y() - node.y(), 2));
83  }
84 
85 private:
87  const fawkes::NavGraphNode & goal,
88  double cost_sofar,
89  NavGraphSearchState * parent_state,
90  fawkes::NavGraph * map_graph,
91  navgraph::EstimateFunction estimate_func,
92  navgraph::CostFunction cost_func,
93  fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
94 
95 private:
96  std::vector<AStarState *> children();
97 
98  // state information
100 
101  // goal information
102  fawkes::NavGraphNode goal_;
103 
104  fawkes::NavGraph *map_graph_;
105 
106  fawkes::NavGraphConstraintRepo *constraint_repo_;
107 
108  size_t key_;
109 
110  navgraph::EstimateFunction estimate_func_;
111  navgraph::CostFunction cost_func_;
112 };
113 
114 } // end of namespace fawkes
115 
116 #endif
This is the abstract(!) class for an A* State.
Definition: astar_state.h:38
Fawkes library namespace.
Graph-based path planner A* search state.
Definition: search_state.h:35
Topological map graph.
Definition: navgraph.h:49
static float straight_line_estimate(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal)
Determine straight line estimate between two nodes.
Definition: search_state.h:80
virtual bool is_goal()
Check, wether we reached a goal or not.
virtual float estimate()
Estimate the heuristic cost to the goal.
virtual size_t key()
Generates a unique key for this state.
Definition: search_state.h:55
NavGraphSearchState(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal, fawkes::NavGraph *map_graph, fawkes::NavGraphConstraintRepo *constraint_repo=NULL)
Constructor.
Constraint repository to maintain blocks on nodes.
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
Topological graph node.
Definition: navgraph_node.h:35
static float euclidean_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Determine euclidean cost between two nodes.
Definition: search_state.h:69
fawkes::NavGraphNode & node()
Get graph node corresponding to this search state.
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58