30 template<
class NODE_TYPE>
70 typedef typename MAPS_IMPLEMENTATION::template map<mrpt::utils::TNodeID,NODE_TYPE>
node_map_t;
74 template <
class NODE_TYPE_FOR_METRIC>
76 const NODE_TYPE_FOR_METRIC &query_pt,
78 double *out_distance = NULL,
79 const std::set<mrpt::utils::TNodeID> *ignored_nodes = NULL
82 MRPT_TODO(
"Optimize this query with KD-tree!")
85 double min_d = std::numeric_limits<double>::max();
89 if (ignored_nodes && ignored_nodes->find(it->first)!=ignored_nodes->end())
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))
95 double d = distanceMetricEvaluator.distance(ptFrom,ptTo);
101 if (out_distance) *out_distance = min_d;
108 const NODE_TYPE_DATA &new_child_node_data,
109 const EDGE_TYPE &new_edge_data )
113 edges_of_parent.push_back(
typename base_t::TEdgeInfo(new_child_id,
false, new_edge_data ) );
115 m_nodes[new_child_id] = NODE_TYPE(new_child_id,parent_id, &edges_of_parent.back().data, new_child_node_data);
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;
142 out_path.push_front(*node);
151 throw std::runtime_error(
"backtrackPath: Node ID not found during tree traversal!");
152 node = &it_next->second;
174 parent_id (parent_id_),
175 end_state( end_pose_ ),
177 ptg_index ( 0 ), ptg_K ( 0 ), ptg_dist ( 0.0 )
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;
235 else return std::numeric_limits<double>::max();
mrpt::utils::TNodeID parent_id
The ID of the parent node in the tree.
double x() const
Common members of all points & poses classes.
TMapNode2ListEdges edges_to_children
The edges of each node.
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
< Make available this typedef in this namespace too
Traits for using a mrpt::utils::map_as_vector<> (dense, fastest representation)
const mrpt::nav::CParameterizedTrajectoryGenerator & m_ptg
NODE_TYPE(mrpt::utils::TNodeID node_id_, mrpt::utils::TNodeID parent_id_, EDGE_TYPE *edge_to_parent_, const NODE_TYPE_DATA &data)
TNodeSE2_TP(const mrpt::math::TPose2D &state_)
mrpt::utils::TNodeID parent_id
INVALID_NODEID for the root, a valid ID otherwise.
TNodeSE2(const mrpt::math::TPose2D &state_)
EDGE_TYPE * edge_to_parent
NULL for root, a valid edge otherwise.
std::list< TEdgeInfo > TListEdges
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.
const Scalar * const_iterator
const node_map_t & getAllNodes() const
int ptg_index
indicate the type of trajectory used for this motion
bool cannotBeNearerThan(const TNodeSE2 &a, const TNodeSE2 &b, const double d) const
Generic base for metrics.
int ptg_K
identify the trajectory number K of the type ptg_index
double cost
cost associated to each motion, this should be defined by the user according to a spefic cost functio...
mrpt::graphs::CDirectedTree< EDGE_TYPE > base_t
This is the base class for any user-defined PTG.
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_)
double distance(const TNodeSE2_TP &src, const TNodeSE2_TP &dst) const
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)
This class contains motions and motions tree structures for the hybrid navigation algorithm...
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.
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...
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)
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...
T square(const T x)
Inline function for the square of a number.
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)
PoseDistanceMetric(const mrpt::nav::CParameterizedTrajectoryGenerator &ptg)
double distance(const TNodeSE2 &a, const TNodeSE2 &b) const
A class used to store a 2D pose.
TMoveTree< TNodeSE2_TP, TMoveEdgeSE2_TP > TMoveTreeSE2_TP
tree data structure for planning in SE2 within TP-Space manifolds
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...
mrpt::utils::TNodeID getNextFreeNodeID() const
double ptg_dist
identify the lenght of the trajectory for this motion
An edge for the move tree used for planning in SE2 and TP-space.
std::list< NODE_TYPE > path_t
A topological path up-tree.
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
node_map_t m_nodes
Info per node.
bool cannotBeNearerThan(const TNodeSE2_TP &a, const TNodeSE2_TP &b, const double d) const
double phi
Orientation (rads)
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, NODE_TYPE > node_map_t
Map: TNode_ID => Node info.