Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
13 #include <mrpt/utils/traits_map.h>
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/poses/CPose2D.h>
16 
18 #include <mrpt/nav/link_pragmas.h>
19 
20 namespace mrpt
21 {
22  namespace nav
23  {
24  /** \addtogroup nav_planners Path planning
25  * \ingroup mrpt_nav_grp
26  * @{ */
27 
28 
29  /** Generic base for metrics */
30  template<class node_t>
32 
33  /** This class contains motions and motions tree structures for the hybrid navigation algorithm
34  *
35  * <b>Usage:</b><br>
36  * \note: this class inheredit mrpt::graphs::CDirectedTree, please refer to 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,
49  class EDGE_TYPE,
50  class MAPS_IMPLEMENTATION = mrpt::utils::map_traits_map_as_vector // 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  mrpt::utils::TNodeID node_id; //!< Duplicated ID (it's also in the map::iterator->first), but put here to make it available in path_t
58  mrpt::utils::TNodeID parent_id; //!< INVALID_NODEID for the root, a valid ID otherwise
59  EDGE_TYPE * edge_to_parent; //!< NULL for root, a valid edge otherwise
60  node_t(mrpt::utils::TNodeID node_id_, mrpt::utils::TNodeID parent_id_, EDGE_TYPE * edge_to_parent_, const NODE_TYPE_DATA &data) :
61  NODE_TYPE_DATA(data),
62  node_id(node_id_),parent_id(parent_id_),
63  edge_to_parent(edge_to_parent_)
64  {
65  }
66  node_t() {}
67  };
68 
70  typedef EDGE_TYPE edge_t;
71  typedef typename MAPS_IMPLEMENTATION::template map<mrpt::utils::TNodeID, node_t> node_map_t; //!< Map: TNode_ID => Node info
72  typedef std::list<node_t> path_t; //!< A topological path up-tree
73 
74  /** Finds the nearest node to a given pose, using the given metric */
75  template <class NODE_TYPE_FOR_METRIC>
77  const NODE_TYPE_FOR_METRIC &query_pt,
78  const PoseDistanceMetric<NODE_TYPE_FOR_METRIC> &distanceMetricEvaluator,
79  double *out_distance = NULL,
80  const std::set<mrpt::utils::TNodeID> *ignored_nodes = NULL
81  ) const
82  {
83  ASSERT_(!m_nodes.empty())
84 
85  double min_d = std::numeric_limits<double>::max();
87  for (typename node_map_t::const_iterator it=m_nodes.begin();it!=m_nodes.end();++it)
88  {
89  if (ignored_nodes && ignored_nodes->find(it->first)!=ignored_nodes->end())
90  continue; // ignore it
91  const NODE_TYPE_FOR_METRIC ptTo(query_pt.state);
92  const NODE_TYPE_FOR_METRIC ptFrom(it->second.state);
93  if (distanceMetricEvaluator.cannotBeNearerThan(ptFrom,ptTo,min_d))
94  continue; // Skip the more expensive calculation of exact distance
95  double d = distanceMetricEvaluator.distance(ptFrom,ptTo);
96  if (d<min_d) {
97  min_d = d;
98  min_id = it->first;
99  }
100  }
101  if (out_distance) *out_distance = min_d;
102  return min_id;
103  }
104 
106  const mrpt::utils::TNodeID parent_id,
107  const mrpt::utils::TNodeID new_child_id,
108  const NODE_TYPE_DATA &new_child_node_data,
109  const EDGE_TYPE &new_edge_data )
110  {
111  // edge:
112  typename base_t::TListEdges & edges_of_parent = base_t::edges_to_children[parent_id];
113  edges_of_parent.push_back( typename base_t::TEdgeInfo(new_child_id,false/*direction_child_to_parent*/, new_edge_data ) );
114  // node:
115  m_nodes[new_child_id] = node_t(new_child_id,parent_id, &edges_of_parent.back().data, new_child_node_data);
116  }
117 
118  /** Insert a node without edges (should be used only for a tree root node) */
119  void insertNode(const mrpt::utils::TNodeID node_id, const NODE_TYPE_DATA &node_data)
120  {
121  m_nodes[node_id] = node_t(node_id,INVALID_NODEID, NULL, node_data);
122  }
123 
124  mrpt::utils::TNodeID getNextFreeNodeID() const { return m_nodes.size(); }
125 
126  const node_map_t & getAllNodes() const { return m_nodes; }
127 
128  /** Builds the path (sequence of nodes, with info about next edge) up-tree from a `target_node` towards the root
129  * Nodes are ordered in the direction ROOT -> start_node
130  */
132  const mrpt::utils::TNodeID target_node,
133  path_t &out_path
134  ) const
135  {
136  out_path.clear();
137  typename node_map_t::const_iterator it_src = m_nodes.find(target_node);
138  if (it_src == m_nodes.end()) throw std::runtime_error("backtrackPath: target_node not found in tree!");
139  const node_t * node = &it_src->second;
140  for (;;)
141  {
142  out_path.push_front(*node);
143 
144  mrpt::utils::TNodeID next_node_id = node->parent_id;
145  if (next_node_id==INVALID_NODEID) {
146  break; // finished
147  }
148  else {
149  typename node_map_t::const_iterator it_next = m_nodes.find(next_node_id);
150  if (it_next == m_nodes.end())
151  throw std::runtime_error("backtrackPath: Node ID not found during tree traversal!");
152  node = &it_next->second;
153  }
154  }
155 
156  }
157 
158  private:
159  node_map_t m_nodes; //!< Info per node
160 
161  }; // end TMoveTree
162 
163  /** An edge for the move tree used for planning in SE2 and TP-space */
165  {
166  mrpt::utils::TNodeID parent_id; //!< The ID of the parent node in the tree
167  mrpt::math::TPose2D end_state; //!< state in SE2 as 2D pose (x, y, phi) - \note: it is not possible to initialize a motion without a state
168  double cost; //!< cost associated to each motion, this should be defined by the user according to a spefic cost function
169  int ptg_index; //!< indicate the type of trajectory used for this motion
170  int ptg_K; //!< identify the trajectory number K of the type ptg_index
171  double ptg_dist; //!< identify the lenght of the trajectory for this motion
172 
173  TMoveEdgeSE2_TP ( const mrpt::utils::TNodeID parent_id_, const mrpt::math::TPose2D end_pose_ ) :
174  parent_id (parent_id_),
175  end_state( end_pose_ ),
176  cost( 0.0 ),
177  ptg_index ( 0 ), ptg_K ( 0 ), ptg_dist ( 0.0 ) //these are all PTGs parameters
178  {}
179  TMoveEdgeSE2_TP() : parent_id(INVALID_NODEID) {}
180  };
181 
183  {
184  mrpt::math::TPose2D state; //!< state in SE2 as 2D pose (x, y, phi)
185  TNodeSE2( const mrpt::math::TPose2D &state_) : state(state_) { }
186  TNodeSE2() {}
187  };
188 
189  /** Pose metric for SE(2) */
190  template<>
192  {
193  bool cannotBeNearerThan(const TNodeSE2 &a, const TNodeSE2& b,const double d) const
194  {
195  if (std::abs(a.state.x-b.state.x)>d) return true;
196  if (std::abs(a.state.y-b.state.y)>d) return true;
197  return false;
198  }
199 
200  double distance(const TNodeSE2 &a, const TNodeSE2& b) const
201  {
202  return mrpt::math::square(a.state.x-b.state.x) +
203  mrpt::math::square(a.state.y-b.state.y) +
204  mrpt::math::square( mrpt::math::angDistance(a.state.phi,b.state.phi) );
205  }
207  };
208 
210  {
211  mrpt::math::TPose2D state; //!< state in SE2 as 2D pose (x, y, phi)
212  TNodeSE2_TP( const mrpt::math::TPose2D &state_) : state(state_) { }
214  };
215 
216  /** Pose metric for SE(2) limited to a given PTG manifold. NOTE: This 'metric' is NOT symmetric for all PTGs: d(a,b)!=d(b,a) */
217  template<>
219  {
220  bool cannotBeNearerThan(const TNodeSE2_TP &a, const TNodeSE2_TP& b,const double d) const
221  {
222  if (std::abs(a.state.x-b.state.x)>d) return true;
223  if (std::abs(a.state.y-b.state.y)>d) return true;
224  return false;
225  }
226  double distance(const TNodeSE2_TP &src, const TNodeSE2_TP& dst) const
227  {
228  double d;
229  int k;
232  bool tp_point_is_exact = m_ptg.inverseMap_WS2TP(relPose.x(),relPose.y(),k,d);
233  if (tp_point_is_exact)
234  return d * m_ptg.getRefDistance(); // de-normalize distance
235  else return std::numeric_limits<double>::max(); // not in range: we can't evaluate this distance!
236  }
238  private:
240  };
241 
242 
243  //typedef TMoveTree<TNodeSE2 ,TMoveEdgeSE2> TMoveTreeSE2_TP; //!< tree data structure for planning in SE2
244  typedef TMoveTree<TNodeSE2_TP,TMoveEdgeSE2_TP> TMoveTreeSE2_TP; //!< tree data structure for planning in SE2 within TP-Space manifolds
245 
246  /** @} */
247  }
248 }
mrpt::utils::TNodeID parent_id
The ID of the parent node in the tree.
Definition: TMoveTree.h:166
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
TMapNode2ListEdges edges_to_children
The edges of each node.
Definition: CDirectedTree.h:67
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition: TMoveTree.h:211
node_t(mrpt::utils::TNodeID node_id_, mrpt::utils::TNodeID parent_id_, EDGE_TYPE *edge_to_parent_, const NODE_TYPE_DATA &data)
Definition: TMoveTree.h:60
< Make available this typedef in this namespace too
Definition: CDirectedTree.h:48
Traits for using a mrpt::utils::map_as_vector<> (dense, fastest representation)
Definition: traits_map.h:32
const mrpt::nav::CParameterizedTrajectoryGenerator & m_ptg
Definition: TMoveTree.h:239
TNodeSE2_TP(const mrpt::math::TPose2D &state_)
Definition: TMoveTree.h:212
TNodeSE2(const mrpt::math::TPose2D &state_)
Definition: TMoveTree.h:185
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:91
#define INVALID_NODEID
std::list< node_t > path_t
A topological path up-tree.
Definition: TMoveTree.h:72
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, node_t > node_map_t
Map: TNode_ID => Node info.
Definition: TMoveTree.h:71
const Scalar * const_iterator
Definition: eigen_plugins.h:24
const node_map_t & getAllNodes() const
Definition: TMoveTree.h:126
GLuint src
Definition: glext.h:6303
int ptg_index
indicate the type of trajectory used for this motion
Definition: TMoveTree.h:169
bool cannotBeNearerThan(const TNodeSE2 &a, const TNodeSE2 &b, const double d) const
Definition: TMoveTree.h:193
Generic base for metrics.
Definition: TMoveTree.h:31
int ptg_K
identify the trajectory number K of the type ptg_index
Definition: TMoveTree.h:170
double cost
cost associated to each motion, this should be defined by the user according to a spefic cost functio...
Definition: TMoveTree.h:168
EDGE_TYPE edge_t
Definition: TMoveTree.h:70
mrpt::graphs::CDirectedTree< EDGE_TYPE > base_t
Definition: TMoveTree.h:69
mrpt::utils::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:57
This is the base class for any user-defined PTG.
GLuint dst
Definition: glext.h:6198
uint64_t TNodeID
The type for node IDs in graphs of different types.
TMoveEdgeSE2_TP(const mrpt::utils::TNodeID parent_id_, const mrpt::math::TPose2D end_pose_)
Definition: TMoveTree.h:173
double distance(const TNodeSE2_TP &src, const TNodeSE2_TP &dst) const
Definition: TMoveTree.h:226
void insertNode(const mrpt::utils::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:119
GLubyte GLubyte b
Definition: glext.h:5575
This class contains motions and motions tree structures for the hybrid navigation algorithm...
Definition: TMoveTree.h:52
mrpt::utils::TNodeID getNearestNode(const NODE_TYPE_FOR_METRIC &query_pt, const PoseDistanceMetric< NODE_TYPE_FOR_METRIC > &distanceMetricEvaluator, double *out_distance=NULL, const std::set< mrpt::utils::TNodeID > *ignored_nodes=NULL) const
Finds the nearest node to a given pose, using the given metric.
Definition: TMoveTree.h:76
void insertNodeAndEdge(const mrpt::utils::TNodeID parent_id, const mrpt::utils::TNodeID new_child_id, const NODE_TYPE_DATA &new_child_node_data, const EDGE_TYPE &new_edge_data)
Definition: TMoveTree.h:105
void backtrackPath(const mrpt::utils::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:131
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition: TMoveTree.h:184
PoseDistanceMetric(const mrpt::nav::CParameterizedTrajectoryGenerator &ptg)
Definition: TMoveTree.h:237
double distance(const TNodeSE2 &a, const TNodeSE2 &b) const
Definition: TMoveTree.h:200
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
TMoveTree< TNodeSE2_TP, TMoveEdgeSE2_TP > TMoveTreeSE2_TP
tree data structure for planning in SE2 within TP-Space manifolds
Definition: TMoveTree.h:244
T square(const T x)
Inline function for the square of a number.
mrpt::utils::TNodeID parent_id
INVALID_NODEID for the root, a valid ID otherwise.
Definition: TMoveTree.h:58
Lightweight 2D pose.
mrpt::utils::TNodeID getNextFreeNodeID() const
Definition: TMoveTree.h:124
EDGE_TYPE * edge_to_parent
NULL for root, a valid edge otherwise.
Definition: TMoveTree.h:59
#define ASSERT_(f)
double ptg_dist
identify the lenght of the trajectory for this motion
Definition: TMoveTree.h:171
An edge for the move tree used for planning in SE2 and TP-space.
Definition: TMoveTree.h:164
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
Definition: TMoveTree.h:167
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:233
node_map_t m_nodes
Info per node.
Definition: TMoveTree.h:159
bool cannotBeNearerThan(const TNodeSE2_TP &a, const TNodeSE2_TP &b, const double d) const
Definition: TMoveTree.h:220
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
GLubyte GLubyte GLubyte a
Definition: glext.h:5575



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019