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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020