Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
19
20namespace 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_TYPE>
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_TYPE : 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_TYPE(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 }
67 };
68
70 typedef typename MAPS_IMPLEMENTATION::template map<mrpt::utils::TNodeID,NODE_TYPE> node_map_t; //!< Map: TNode_ID => Node info
71 typedef std::list<NODE_TYPE> path_t; //!< A topological path up-tree
72
73 /** Finds the nearest node to a given pose, using the given metric */
74 template <class NODE_TYPE_FOR_METRIC>
76 const NODE_TYPE_FOR_METRIC &query_pt,
77 const PoseDistanceMetric<NODE_TYPE_FOR_METRIC> &distanceMetricEvaluator,
78 double *out_distance = NULL,
79 const std::set<mrpt::utils::TNodeID> *ignored_nodes = NULL
80 ) const
81 {
82 MRPT_TODO("Optimize this query with KD-tree!")
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_TYPE(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_TYPE(node_id,INVALID_NODEID, NULL, node_data);
122 }
123
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_TYPE * 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 {}
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_) { }
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 float 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.refDistance; // 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}
< Make available this typedef in this namespace too
TMapNode2ListEdges edges_to_children
The edges of each node.
This is the base class for any user-defined PTG.
This class contains motions and motions tree structures for the hybrid navigation algorithm.
Definition TMoveTree.h:53
mrpt::utils::TNodeID getNextFreeNodeID() const
Definition TMoveTree.h:124
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:75
const node_map_t & getAllNodes() const
Definition TMoveTree.h:126
node_map_t m_nodes
Info per node.
Definition TMoveTree.h:159
std::list< NODE_TYPE > path_t
A topological path up-tree.
Definition TMoveTree.h:71
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, NODE_TYPE > node_map_t
Map: TNode_ID => Node info.
Definition TMoveTree.h:70
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
mrpt::graphs::CDirectedTree< EDGE_TYPE > base_t
Definition TMoveTree.h:69
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
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
A class used to store a 2D pose.
Definition CPose2D.h:37
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...
double x() const
Common members of all points & poses classes.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
Definition wrap2pi.h:91
uint64_t TNodeID
The type for node IDs in graphs of different types.
#define INVALID_NODEID
TMoveTree< TNodeSE2_TP, TMoveEdgeSE2_TP > TMoveTreeSE2_TP
tree data structure for planning in SE2 within TP-Space manifolds
Definition TMoveTree.h:244
#define MRPT_TODO(x)
Definition mrpt_macros.h:65
#define ASSERT_(f)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.
double phi
Orientation (rads)
double y
X,Y coordinates.
double distance(const TNodeSE2 &a, const TNodeSE2 &b) const
Definition TMoveTree.h:200
bool cannotBeNearerThan(const TNodeSE2 &a, const TNodeSE2 &b, const double d) const
Definition TMoveTree.h:193
PoseDistanceMetric(const mrpt::nav::CParameterizedTrajectoryGenerator &ptg)
Definition TMoveTree.h:237
bool cannotBeNearerThan(const TNodeSE2_TP &a, const TNodeSE2_TP &b, const double d) const
Definition TMoveTree.h:220
double distance(const TNodeSE2_TP &src, const TNodeSE2_TP &dst) const
Definition TMoveTree.h:226
const mrpt::nav::CParameterizedTrajectoryGenerator & m_ptg
Definition TMoveTree.h:239
Generic base for metrics.
Definition TMoveTree.h:31
An edge for the move tree used for planning in SE2 and TP-space.
Definition TMoveTree.h:165
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
Definition TMoveTree.h:167
double cost
cost associated to each motion, this should be defined by the user according to a spefic cost functio...
Definition TMoveTree.h:168
int ptg_index
indicate the type of trajectory used for this motion
Definition TMoveTree.h:169
mrpt::utils::TNodeID parent_id
The ID of the parent node in the tree.
Definition TMoveTree.h:166
double ptg_dist
identify the lenght of the trajectory for this motion
Definition TMoveTree.h:171
int ptg_K
identify the trajectory number K of the type ptg_index
Definition TMoveTree.h:170
TMoveEdgeSE2_TP(const mrpt::utils::TNodeID parent_id_, const mrpt::math::TPose2D end_pose_)
Definition TMoveTree.h:173
EDGE_TYPE * edge_to_parent
NULL for root, a valid edge otherwise.
Definition TMoveTree.h:59
NODE_TYPE(mrpt::utils::TNodeID node_id_, mrpt::utils::TNodeID parent_id_, EDGE_TYPE *edge_to_parent_, const NODE_TYPE_DATA &data)
Definition TMoveTree.h:60
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.
Definition TMoveTree.h:57
mrpt::utils::TNodeID parent_id
INVALID_NODEID for the root, a valid ID otherwise.
Definition TMoveTree.h:58
TNodeSE2_TP(const mrpt::math::TPose2D &state_)
Definition TMoveTree.h:212
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition TMoveTree.h:211
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition TMoveTree.h:184
TNodeSE2(const mrpt::math::TPose2D &state_)
Definition TMoveTree.h:185
Traits for using a mrpt::utils::map_as_vector<> (dense, fastest representation)
Definition traits_map.h:32



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 14:10:35 UTC 2023