Main MRPT website > C++ reference for MRPT 1.9.9
TMoveTree.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/poses/CPose2D.h>
16 
18 
19 namespace mrpt
20 {
21 namespace nav
22 {
23 /** \addtogroup nav_planners Path planning
24  * \ingroup mrpt_nav_grp
25  * @{ */
26 
27 /** Generic base for metrics */
28 template <class node_t>
30 
31 /** This class contains motions and motions tree structures for the hybrid
32  * navigation algorithm
33  *
34  * <b>Usage:</b><br>
35  * \note: this class inheredit mrpt::graphs::CDirectedTree, please refer to
36  * inheritance for detail about generic tree methods
37  *
38  * - initialize a motions tree using .initializeMoveTree()
39  * - addEdge (from, to)
40  * - add here more instructions
41  *
42  *
43  * <b>Changes history</b>
44  * - 06/MAR/2014: Creation (MB)
45  * - 21/JAN/2015: Refactoring (JLBC)
46  */
47 template <
48  class NODE_TYPE_DATA, class EDGE_TYPE,
49  class MAPS_IMPLEMENTATION = mrpt::containers::map_traits_map_as_vector
50  /* Use std::map<> vs. std::vector<>*/
51  >
52 class TMoveTree : public mrpt::graphs::CDirectedTree<EDGE_TYPE>
53 {
54  public:
55  struct node_t : public NODE_TYPE_DATA
56  {
57  /** Duplicated ID (it's also in the map::iterator->first), but put here
58  * to make it available in path_t */
60  /** INVALID_NODEID for the root, a valid ID otherwise */
62  /** NULL for root, a valid edge otherwise */
63  EDGE_TYPE* edge_to_parent;
65  mrpt::graphs::TNodeID node_id_, mrpt::graphs::TNodeID parent_id_,
66  EDGE_TYPE* edge_to_parent_, const NODE_TYPE_DATA& data)
67  : NODE_TYPE_DATA(data),
68  node_id(node_id_),
69  parent_id(parent_id_),
70  edge_to_parent(edge_to_parent_)
71  {
72  }
73  node_t() {}
74  };
75 
77  using edge_t = EDGE_TYPE;
78  /** Map: TNode_ID => Node info */
79  using node_map_t = typename MAPS_IMPLEMENTATION::template map<
81  /** A topological path up-tree */
82  using path_t = std::list<node_t>;
83 
84  /** Finds the nearest node to a given pose, using the given metric */
85  template <class NODE_TYPE_FOR_METRIC>
87  const NODE_TYPE_FOR_METRIC& query_pt,
88  const PoseDistanceMetric<NODE_TYPE_FOR_METRIC>& distanceMetricEvaluator,
89  double* out_distance = NULL,
90  const std::set<mrpt::graphs::TNodeID>* ignored_nodes = NULL) const
91  {
92  ASSERT_(!m_nodes.empty());
93  double min_d = std::numeric_limits<double>::max();
95  for (typename node_map_t::const_iterator it = m_nodes.begin();
96  it != m_nodes.end(); ++it)
97  {
98  if (ignored_nodes &&
99  ignored_nodes->find(it->first) != ignored_nodes->end())
100  continue; // ignore it
101  const NODE_TYPE_FOR_METRIC ptTo(query_pt.state);
102  const NODE_TYPE_FOR_METRIC ptFrom(it->second.state);
103  if (distanceMetricEvaluator.cannotBeNearerThan(ptFrom, ptTo, min_d))
104  continue; // Skip the more expensive calculation of exact
105  // distance
106  double d = distanceMetricEvaluator.distance(ptFrom, ptTo);
107  if (d < min_d)
108  {
109  min_d = d;
110  min_id = it->first;
111  }
112  }
113  if (out_distance) *out_distance = min_d;
114  return min_id;
115  }
116 
118  const mrpt::graphs::TNodeID parent_id,
119  const mrpt::graphs::TNodeID new_child_id,
120  const NODE_TYPE_DATA& new_child_node_data,
121  const EDGE_TYPE& new_edge_data)
122  {
123  // edge:
124  typename base_t::TListEdges& edges_of_parent =
125  base_t::edges_to_children[parent_id];
126  edges_of_parent.push_back(typename base_t::TEdgeInfo(
127  new_child_id, false /*direction_child_to_parent*/, new_edge_data));
128  // node:
129  m_nodes[new_child_id] = node_t(
130  new_child_id, parent_id, &edges_of_parent.back().data,
131  new_child_node_data);
132  }
133 
134  /** Insert a node without edges (should be used only for a tree root node)
135  */
137  const mrpt::graphs::TNodeID node_id, const NODE_TYPE_DATA& node_data)
138  {
139  m_nodes[node_id] = node_t(node_id, INVALID_NODEID, NULL, node_data);
140  }
141 
143  const node_map_t& getAllNodes() const { return m_nodes; }
144  /** Builds the path (sequence of nodes, with info about next edge) up-tree
145  * from a `target_node` towards the root
146  * Nodes are ordered in the direction ROOT -> start_node
147  */
149  const mrpt::graphs::TNodeID target_node, path_t& out_path) const
150  {
151  out_path.clear();
152  typename node_map_t::const_iterator it_src = m_nodes.find(target_node);
153  if (it_src == m_nodes.end())
154  throw std::runtime_error(
155  "backtrackPath: target_node not found in tree!");
156  const node_t* node = &it_src->second;
157  for (;;)
158  {
159  out_path.push_front(*node);
160 
161  mrpt::graphs::TNodeID next_node_id = node->parent_id;
162  if (next_node_id == INVALID_NODEID)
163  {
164  break; // finished
165  }
166  else
167  {
168  typename node_map_t::const_iterator it_next =
169  m_nodes.find(next_node_id);
170  if (it_next == m_nodes.end())
171  throw std::runtime_error(
172  "backtrackPath: Node ID not found during tree "
173  "traversal!");
174  node = &it_next->second;
175  }
176  }
177  }
178 
179  private:
180  /** Info per node */
182 
183 }; // end TMoveTree
184 
185 /** An edge for the move tree used for planning in SE2 and TP-space */
187 {
188  /** The ID of the parent node in the tree */
190  /** state in SE2 as 2D pose (x, y, phi) - \note: it is not possible to
191  * initialize a motion without a state */
193  /** cost associated to each motion, this should be defined by the user
194  * according to a spefic cost function */
195  double cost;
196  /** indicate the type of trajectory used for this motion */
198  /** identify the trajectory number K of the type ptg_index */
199  int ptg_K;
200  /** identify the lenght of the trajectory for this motion */
201  double ptg_dist;
202 
204  const mrpt::graphs::TNodeID parent_id_,
205  const mrpt::math::TPose2D end_pose_)
206  : parent_id(parent_id_),
207  end_state(end_pose_),
208  cost(0.0),
209  ptg_index(0),
210  ptg_K(0),
211  ptg_dist(0.0) // these are all PTGs parameters
212  {
213  }
215 };
216 
217 struct TNodeSE2
218 {
219  /** state in SE2 as 2D pose (x, y, phi) */
221  TNodeSE2(const mrpt::math::TPose2D& state_) : state(state_) {}
222  TNodeSE2() {}
223 };
224 
225 /** Pose metric for SE(2) */
226 template <>
228 {
230  const TNodeSE2& a, const TNodeSE2& b, const double d) const
231  {
232  if (std::abs(a.state.x - b.state.x) > d) return true;
233  if (std::abs(a.state.y - b.state.y) > d) return true;
234  return false;
235  }
236 
237  double distance(const TNodeSE2& a, const TNodeSE2& b) const
238  {
239  return mrpt::square(a.state.x - b.state.x) +
240  mrpt::square(a.state.y - b.state.y) +
241  mrpt::square(mrpt::math::angDistance(a.state.phi, b.state.phi));
242  }
244 };
245 
247 {
248  /** state in SE2 as 2D pose (x, y, phi) */
250  TNodeSE2_TP(const mrpt::math::TPose2D& state_) : state(state_) {}
252 };
253 
254 /** Pose metric for SE(2) limited to a given PTG manifold. NOTE: This 'metric'
255  * is NOT symmetric for all PTGs: d(a,b)!=d(b,a) */
256 template <>
258 {
260  const TNodeSE2_TP& a, const TNodeSE2_TP& b, const double d) const
261  {
262  if (std::abs(a.state.x - b.state.x) > d) return true;
263  if (std::abs(a.state.y - b.state.y) > d) return true;
264  return false;
265  }
266  double distance(const TNodeSE2_TP& src, const TNodeSE2_TP& dst) const
267  {
268  double d;
269  int k;
271  relPose.inverseComposeFrom(
273  bool tp_point_is_exact =
274  m_ptg.inverseMap_WS2TP(relPose.x(), relPose.y(), k, d);
275  if (tp_point_is_exact)
276  return d * m_ptg.getRefDistance(); // de-normalize distance
277  else
278  return std::numeric_limits<double>::max(); // not in range: we
279  // can't evaluate this
280  // distance!
281  }
283  : m_ptg(ptg)
284  {
285  }
286 
287  private:
289 };
290 
291 // using TMoveTreeSE2_TP = TMoveTree<TNodeSE2 ,TMoveEdgeSE2>; //!< tree data
292 // structure for planning in SE2
293 /** tree data structure for planning in SE2 within TP-Space manifolds */
295 
296 /** @} */
297 } // namespace nav
298 } // namespace mrpt
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
TMapNode2ListEdges edges_to_children
The edges of each node.
Definition: CDirectedTree.h:84
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition: TMoveTree.h:249
A special kind of graph in the form of a tree with directed edges and optional edge annotations of te...
Definition: CDirectedTree.h:54
mrpt::graphs::TNodeID getNextFreeNodeID() const
Definition: TMoveTree.h:142
mrpt::graphs::TNodeID getNearestNode(const NODE_TYPE_FOR_METRIC &query_pt, const PoseDistanceMetric< NODE_TYPE_FOR_METRIC > &distanceMetricEvaluator, double *out_distance=NULL, const std::set< mrpt::graphs::TNodeID > *ignored_nodes=NULL) const
Finds the nearest node to a given pose, using the given metric.
Definition: TMoveTree.h:86
const mrpt::nav::CParameterizedTrajectoryGenerator & m_ptg
Definition: TMoveTree.h:288
TNodeSE2_TP(const mrpt::math::TPose2D &state_)
Definition: TMoveTree.h:250
mrpt::graphs::TNodeID parent_id
The ID of the parent node in the tree.
Definition: TMoveTree.h:189
TNodeSE2(const mrpt::math::TPose2D &state_)
Definition: TMoveTree.h:221
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:98
mrpt::graphs::TNodeID parent_id
INVALID_NODEID for the root, a valid ID otherwise.
Definition: TMoveTree.h:61
EDGE_TYPE edge_t
Definition: TMoveTree.h:77
void insertNodeAndEdge(const mrpt::graphs::TNodeID parent_id, const mrpt::graphs::TNodeID new_child_id, const NODE_TYPE_DATA &new_child_node_data, const EDGE_TYPE &new_edge_data)
Definition: TMoveTree.h:117
const node_map_t & getAllNodes() const
Definition: TMoveTree.h:143
GLuint src
Definition: glext.h:7278
int ptg_index
indicate the type of trajectory used for this motion
Definition: TMoveTree.h:197
bool cannotBeNearerThan(const TNodeSE2 &a, const TNodeSE2 &b, const double d) const
Definition: TMoveTree.h:229
std::list< node_t > path_t
A topological path up-tree.
Definition: TMoveTree.h:82
Generic base for metrics.
Definition: TMoveTree.h:29
int ptg_K
identify the trajectory number K of the type ptg_index
Definition: TMoveTree.h:199
double cost
cost associated to each motion, this should be defined by the user according to a spefic cost functio...
Definition: TMoveTree.h:195
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This is the base class for any user-defined PTG.
GLuint dst
Definition: glext.h:7135
double distance(const TNodeSE2_TP &src, const TNodeSE2_TP &dst) const
Definition: TMoveTree.h:266
GLubyte GLubyte b
Definition: glext.h:6279
This class contains motions and motions tree structures for the hybrid navigation algorithm...
Definition: TMoveTree.h:52
node_t(mrpt::graphs::TNodeID node_id_, mrpt::graphs::TNodeID parent_id_, EDGE_TYPE *edge_to_parent_, const NODE_TYPE_DATA &data)
Definition: TMoveTree.h:64
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Traits for using a mrpt::utils::map_as_vector<> (dense, fastest representation)
Definition: traits_map.h:39
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition: TMoveTree.h:220
PoseDistanceMetric(const mrpt::nav::CParameterizedTrajectoryGenerator &ptg)
Definition: TMoveTree.h:282
double distance(const TNodeSE2 &a, const TNodeSE2 &b) const
Definition: TMoveTree.h:237
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
typename MAPS_IMPLEMENTATION::template map< mrpt::graphs::TNodeID, node_t > node_map_t
Map: TNode_ID => Node info.
Definition: TMoveTree.h:80
void insertNode(const mrpt::graphs::TNodeID node_id, const NODE_TYPE_DATA &node_data)
Insert a node without edges (should be used only for a tree root node)
Definition: TMoveTree.h:136
Lightweight 2D pose.
EDGE_TYPE * edge_to_parent
NULL for root, a valid edge otherwise.
Definition: TMoveTree.h:63
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:17
mrpt::graphs::TNodeID node_id
Duplicated ID (it&#39;s also in the map::iterator->first), but put here to make it available in path_t...
Definition: TMoveTree.h:59
double ptg_dist
identify the lenght of the trajectory for this motion
Definition: TMoveTree.h:201
An edge for the move tree used for planning in SE2 and TP-space.
Definition: TMoveTree.h:186
TMoveEdgeSE2_TP(const mrpt::graphs::TNodeID parent_id_, const mrpt::math::TPose2D end_pose_)
Definition: TMoveTree.h:203
#define INVALID_NODEID
Definition: TNodeID.h:20
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
Definition: TMoveTree.h:192
void backtrackPath(const mrpt::graphs::TNodeID target_node, path_t &out_path) const
Builds the path (sequence of nodes, with info about next edge) up-tree from a target_node towards the...
Definition: TMoveTree.h:148
void inverseComposeFrom(const CPose2D &A, const CPose2D &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
Definition: CPose2D.cpp:234
node_map_t m_nodes
Info per node.
Definition: TMoveTree.h:181
bool cannotBeNearerThan(const TNodeSE2_TP &a, const TNodeSE2_TP &b, const double d) const
Definition: TMoveTree.h:259
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
const Scalar * const_iterator
Definition: eigen_plugins.h:27



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019