selfdriving
Classes | Public Member Functions | Public Attributes | Private Types | Private Member Functions | Static Private Member Functions | Private Attributes | List of all members
selfdriving::TPS_RRTstar Class Reference

#include <selfdriving/algos/TPS_RRTstar.h>

Inheritance diagram for selfdriving::TPS_RRTstar:

Classes

struct  DrawFreePoseParams
 
struct  LocalObstaclesInfo
 

Public Member Functions

 TPS_RRTstar ()
 
 ~TPS_RRTstar ()=default
 
PlannerOutput plan (const PlannerInput &in)
 

Public Attributes

TPS_RRTstar_Parameters params_
 
std::vector< CostEvaluator::Ptr > costEvaluators_
 
mrpt::system::CTimeLogger profiler_ {true, "TPS_RRTstar"}
 

Private Types

using closest_lie_nodes_list_t = std::map< distance_t, std::reference_wrapper< const MotionPrimitivesTreeSE2::node_t > >
 
using already_existing_node_t = std::optional< TNodeID >
 
using draw_pose_return_t = std::tuple< mrpt::math::TPose2D, already_existing_node_t, closest_lie_nodes_list_t >
 
using path_to_nodes_list_t = std::map< distance_t, std::tuple< TNodeID, ptg_index_t, trajectory_index_t, distance_t > >
 

Private Member Functions

draw_pose_return_t draw_random_free_pose (const DrawFreePoseParams &p)
 
draw_pose_return_t draw_random_tps (const DrawFreePoseParams &p)
 
draw_pose_return_t draw_random_euclidean (const DrawFreePoseParams &p)
 
path_to_nodes_list_t find_source_nodes_towards (const MotionPrimitivesTreeSE2 &tree, const mrpt::math::TPose2D &query, const double maxDistance, const TrajectoriesAndRobotShape &trs, const TNodeID goalNodeToIgnore, const closest_lie_nodes_list_t &hintCloseNodes)
 
closest_lie_nodes_list_t find_nearby_nodes (const MotionPrimitivesTreeSE2 &tree, const mrpt::math::TPose2D &query, const double maxDistance)
 
std::tuple< distance_t, TNodeIDfind_closest_node (const MotionPrimitivesTreeSE2 &tree, const mrpt::math::TPose2D &query) const
 
path_to_nodes_list_t find_reachable_nodes_from (const MotionPrimitivesTreeSE2 &tree, const TNodeID queryNodeId, const double maxDistance, const TrajectoriesAndRobotShape &trs, const closest_lie_nodes_list_t &hintCloseNodes, const std::optional< TNodeID > &nodeToIgnoreHeading=std::nullopt)
 
distance_t tp_obstacles_single_path (const trajectory_index_t tp_space_k_direction, const mrpt::maps::CPointsMap &localObstacles, const ptg_t &ptg)
 
mrpt::maps::CPointsMap::Ptr cached_local_obstacles (const MotionPrimitivesTreeSE2 &tree, const TNodeID nodeID, const std::vector< mrpt::maps::CPointsMap::Ptr > &globalObstacles, double MAX_XY_DIST)
 
cost_t cost_path_segment (const MoveEdgeSE2_TPS &edge) const
 

Static Private Member Functions

static void transform_pc_square_clipping (const mrpt::maps::CPointsMap &inMap, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY, mrpt::maps::CPointsMap &outMap, bool appendToOutMap=true)
 

Private Attributes

std::map< TNodeID, LocalObstaclesInfolocal_obstacles_cache_
 

Member Typedef Documentation

◆ already_existing_node_t

◆ closest_lie_nodes_list_t

using selfdriving::TPS_RRTstar::closest_lie_nodes_list_t = std::map< distance_t, std::reference_wrapper<const MotionPrimitivesTreeSE2::node_t> >
private

◆ draw_pose_return_t

◆ path_to_nodes_list_t

Constructor & Destructor Documentation

◆ TPS_RRTstar()

selfdriving::TPS_RRTstar::TPS_RRTstar ( )

◆ ~TPS_RRTstar()

selfdriving::TPS_RRTstar::~TPS_RRTstar ( )
default

Member Function Documentation

◆ cached_local_obstacles()

mrpt::maps::CPointsMap::Ptr selfdriving::TPS_RRTstar::cached_local_obstacles ( const MotionPrimitivesTreeSE2 tree,
const TNodeID  nodeID,
const std::vector< mrpt::maps::CPointsMap::Ptr > &  globalObstacles,
double  MAX_XY_DIST 
)
private

◆ cost_path_segment()

cost_t selfdriving::TPS_RRTstar::cost_path_segment ( const MoveEdgeSE2_TPS edge) const
private

◆ draw_random_euclidean()

draw_pose_return_t selfdriving::TPS_RRTstar::draw_random_euclidean ( const DrawFreePoseParams p)
private

◆ draw_random_free_pose()

draw_pose_return_t selfdriving::TPS_RRTstar::draw_random_free_pose ( const DrawFreePoseParams p)
private

◆ draw_random_tps()

draw_pose_return_t selfdriving::TPS_RRTstar::draw_random_tps ( const DrawFreePoseParams p)
private

◆ find_closest_node()

std::tuple<distance_t, TNodeID> selfdriving::TPS_RRTstar::find_closest_node ( const MotionPrimitivesTreeSE2 tree,
const mrpt::math::TPose2D &  query 
) const
private

◆ find_nearby_nodes()

closest_lie_nodes_list_t selfdriving::TPS_RRTstar::find_nearby_nodes ( const MotionPrimitivesTreeSE2 tree,
const mrpt::math::TPose2D &  query,
const double  maxDistance 
)
private

Find all existing nodes "x" in the tree within a given ball, given by the metric on the Lie group, i.e. not following any particular PTG trajectory.

See also
find_reachable_nodes_from(), find_source_nodes_towards()

◆ find_reachable_nodes_from()

path_to_nodes_list_t selfdriving::TPS_RRTstar::find_reachable_nodes_from ( const MotionPrimitivesTreeSE2 tree,
const TNodeID  queryNodeId,
const double  maxDistance,
const TrajectoriesAndRobotShape trs,
const closest_lie_nodes_list_t hintCloseNodes,
const std::optional< TNodeID > &  nodeToIgnoreHeading = std::nullopt 
)
private

Find all existing nodes "x" in the tree that are reachable from query (i.e. query ==> other nodes), and the motion primitives for such motions. Here, query does includes a velocity state.

This method is used in the REWIRE stage of RRT*.

See also
find_source_nodes_towards()

◆ find_source_nodes_towards()

path_to_nodes_list_t selfdriving::TPS_RRTstar::find_source_nodes_towards ( const MotionPrimitivesTreeSE2 tree,
const mrpt::math::TPose2D &  query,
const double  maxDistance,
const TrajectoriesAndRobotShape trs,
const TNodeID  goalNodeToIgnore,
const closest_lie_nodes_list_t hintCloseNodes 
)
private

Find all existing nodes "x" in the tree from which we can reach query (i.e. other nodes ==> query), and the motion primitives for such motions. Note that, at this point, query does not have a velocity state: it will be determined by the best motion primitive.

This method is used in the EXTEND stage of RRT*, and uses the TP-Space motion primitives (PTGs).

See also
find_reachable_nodes_from(), find_nearby_nodes()

◆ plan()

PlannerOutput selfdriving::TPS_RRTstar::plan ( const PlannerInput in)

◆ tp_obstacles_single_path()

distance_t selfdriving::TPS_RRTstar::tp_obstacles_single_path ( const trajectory_index_t  tp_space_k_direction,
const mrpt::maps::CPointsMap &  localObstacles,
const ptg_t ptg 
)
private

Returns TPS-distance to obstacles. ptg dynamic state must be updated by the caller.

◆ transform_pc_square_clipping()

static void selfdriving::TPS_RRTstar::transform_pc_square_clipping ( const mrpt::maps::CPointsMap &  inMap,
const mrpt::poses::CPose2D &  asSeenFrom,
const double  MAX_DIST_XY,
mrpt::maps::CPointsMap &  outMap,
bool  appendToOutMap = true 
)
staticprivate

Returns local obstacles as seen from a given pose, clipped to a maximum distance.

Member Data Documentation

◆ costEvaluators_

std::vector<CostEvaluator::Ptr> selfdriving::TPS_RRTstar::costEvaluators_

◆ local_obstacles_cache_

std::map<TNodeID, LocalObstaclesInfo> selfdriving::TPS_RRTstar::local_obstacles_cache_
private

◆ params_

TPS_RRTstar_Parameters selfdriving::TPS_RRTstar::params_

◆ profiler_

mrpt::system::CTimeLogger selfdriving::TPS_RRTstar::profiler_ {true, "TPS_RRTstar"}

Time profiler (Default: enabled)


The documentation for this class was generated from the following file: