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 <utils/search/astar_state.h>
26 #include <navgraph/constraints/constraint_repo.h>
27 #include <navgraph/navgraph.h>
28 #include <core/utils/lockptr.h>
29 
30 #include <functional>
31 #include <cmath>
32 
33 namespace fawkes {
34 #if 0 /* just to make Emacs auto-indent happy */
35 }
36 #endif
37 
39 {
40  public:
42  fawkes::NavGraph *map_graph,
43  fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
44 
46  fawkes::NavGraph *map_graph,
47  navgraph::EstimateFunction estimate_func,
48  navgraph::CostFunction cost_func = NavGraphSearchState::euclidean_cost,
49  fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
50 
52 
54 
55  virtual size_t key() { return key_; }
56  virtual float estimate();
57  virtual bool is_goal();
58 
59  /** Determine euclidean cost between two nodes.
60  * Note that the given notes are assumed to be adjacent nodes.
61  * @param from originating node
62  * @param to destination node
63  * @return cost from @p from to @p to.
64  */
65  static float
67  const fawkes::NavGraphNode &to)
68  {
69  return sqrtf(powf(to.x() - from.x(), 2) +
70  powf(to.y() - from.y(), 2) );
71  }
72 
73  /** Determine straight line estimate between two nodes.
74  * @param node node to query heuristic value for
75  * @param goal goal node to get estimate for
76  * @return estimate of cost from @p node to @p goal.
77  */
78  static float
80  const fawkes::NavGraphNode &goal)
81  {
82  return sqrtf(powf(goal.x() - node.x(), 2) +
83  powf(goal.y() - node.y(), 2) );
84  }
85 
86  private:
88  double cost_sofar, NavGraphSearchState *parent_state,
89  fawkes::NavGraph *map_graph,
90  navgraph::EstimateFunction estimate_func,
91  navgraph::CostFunction cost_func,
92  fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
93 
94  private:
95  std::vector<AStarState *> children();
96 
97  // state information
99 
100  // goal information
101  fawkes::NavGraphNode goal_;
102 
103  fawkes::NavGraph *map_graph_;
104 
105  fawkes::NavGraphConstraintRepo *constraint_repo_;
106 
107  size_t key_;
108 
109  navgraph::EstimateFunction estimate_func_;
110  navgraph::CostFunction cost_func_;
111 };
112 
113 
114 } // end of namespace fawkes
115 
116 #endif
This is the abstract(!) class for an A* State.
Definition: astar_state.h:38
~NavGraphSearchState()
Destructor.
Fawkes library namespace.
Graph-based path planner A* search state.
Definition: search_state.h:38
Topological map graph.
Definition: navgraph.h:57
static float straight_line_estimate(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal)
Determine straight line estimate between two nodes.
Definition: search_state.h:79
virtual bool is_goal()
Check, wether we reached a goal or not.
virtual float estimate()
Estimate the heuristic cost to the goal.
NavGraphSearchState(fawkes::NavGraphNode node, fawkes::NavGraphNode goal, fawkes::NavGraph *map_graph, fawkes::NavGraphConstraintRepo *constraint_repo=NULL)
Constructor.
virtual size_t key()
Generates a unique key for this state.
Definition: search_state.h:55
Constraint repository to maintain blocks on nodes.
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:59
Topological graph node.
Definition: navgraph_node.h:38
static float euclidean_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Determine euclidean cost between two nodes.
Definition: search_state.h:66
fawkes::NavGraphNode & node()
Get graph node corresponding to this search state.
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:54