MRPT  1.9.9
List of all members | Classes | Public Types | Public Member Functions | Public Attributes | Protected Member Functions | Static Protected Member Functions | Protected Attributes
mrpt::nav::PlannerRRT_SE2_TPS Class Reference

Detailed Description

TP Space-based RRT path planning for SE(2) (planar) robots.

This planner algorithm is described in the paper:

Typical usage:

// Set or load planner parameters:
//planner.loadConfig( mrpt::config::CConfigFile("config_file.cfg") );
//planner.params.... // See RRTAlgorithmParams
// Set RRT end criteria (when to stop searching for a solution)
//planner.end_criteria.... // See RRTEndCriteria
planner.initialize(); // Initialize after setting the algorithm parameters
// Set up planning problem:
PlannerRRT_SE2_TPS::TPlannerResult planner_result;
PlannerRRT_SE2_TPS::TPlannerInput planner_input;
// Start & goal:
planner_input.start_pose = mrpt::math::TPose2D(XXX,XXX,XXX);
planner_input.goal_pose = mrpt::math::TPose2D(XXX,XXX,XXX);
// Set obtacles: (...)
// planner_input.obstacles_points ...
// Set workspace bounding box for picking random poses in the RRT algorithm:
planner_input.world_bbox_min = mrpt::math::TPoint2D(XX,YY);
planner_input.world_bbox_max = mrpt::math::TPoint2D(XX,YY);
// Do path planning:
planner.solve( planner_input, planner_result);
// Analyze contents of planner_result...
Todo:
Factorize into more generic path planner classes! //template <class POSE, class MOTIONS>...

Definition at line 66 of file PlannerRRT_SE2_TPS.h.

#include <mrpt/nav/planners/PlannerRRT_SE2_TPS.h>

Inheritance diagram for mrpt::nav::PlannerRRT_SE2_TPS:

Classes

struct  TPlannerInput
 
struct  TPlannerResult
 

Public Types

using node_pose_t = mrpt::math::TPose2D
 The type of poses at nodes. More...
 

Public Member Functions

 PlannerRRT_SE2_TPS ()
 Constructor. More...
 
void loadConfig (const mrpt::config::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
 Load all params from a config file source. More...
 
void initialize ()
 Must be called after setting all params (see loadConfig()) and before calling solve() More...
 
void solve (const TPlannerInput &pi, TPlannerResult &result)
 The main API entry point: tries to find a planned path from 'goal' to 'target'. More...
 
mrpt::system::CTimeLoggergetProfiler ()
 
const mrpt::nav::TListPTGPtrgetPTGs () const
 
template<typename node_pose_t , typename world_limits_t , typename tree_t >
void renderMoveTree (mrpt::opengl::COpenGLScene &scene, const TPlannerInputTempl< node_pose_t, world_limits_t > &pi, const TPlannerResultTempl< tree_t > &result, const TRenderPlannedPathOptions &options)
 

Public Attributes

RRTEndCriteria end_criteria
 
RRTAlgorithmParams params
 Parameters specific to this path solver algorithm. More...
 

Protected Member Functions

void internal_loadConfig_PTG (const mrpt::config::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
 Load all PTG params from a config file source. More...
 
void internal_initialize_PTG ()
 Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve() More...
 
void spaceTransformer (const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, std::vector< double > &out_TPObstacles)
 
void spaceTransformerOneDirectionOnly (const int tp_space_k_direction, const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, double &out_TPObstacle_k)
 

Static Protected Member Functions

static void transformPointcloudWithSquareClipping (const mrpt::maps::CPointsMap &in_map, mrpt::maps::CPointsMap &out_map, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY)
 

Protected Attributes

bool m_initialized {false}
 
mrpt::system::CTimeLogger m_timelogger
 
bool m_initialized_PTG {false}
 
mrpt::nav::TListPTGPtr m_PTGs
 
mrpt::maps::CSimplePointsMap m_local_obs
 

Member Typedef Documentation

◆ node_pose_t

The type of poses at nodes.

Definition at line 70 of file PlannerRRT_SE2_TPS.h.

Constructor & Destructor Documentation

◆ PlannerRRT_SE2_TPS()

PlannerRRT_SE2_TPS::PlannerRRT_SE2_TPS ( )
default

Constructor.

Member Function Documentation

◆ getProfiler()

mrpt::system::CTimeLogger& mrpt::nav::PlannerTPS_VirtualBase::getProfiler ( )
inlineinherited

◆ getPTGs()

const mrpt::nav::TListPTGPtr& mrpt::nav::PlannerTPS_VirtualBase::getPTGs ( ) const
inlineinherited

Definition at line 142 of file PlannerRRT_common.h.

References mrpt::nav::PlannerTPS_VirtualBase::m_PTGs.

◆ initialize()

void PlannerRRT_SE2_TPS::initialize ( )

Must be called after setting all params (see loadConfig()) and before calling solve()

Definition at line 36 of file PlannerRRT_SE2_TPS.cpp.

References mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG().

Here is the call graph for this function:

◆ internal_initialize_PTG()

void PlannerTPS_VirtualBase::internal_initialize_PTG ( )
protectedinherited

Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve()

Definition at line 34 of file PlannerRRT_common.cpp.

References ASSERTMSG_, mrpt::format(), mrpt::math::TPolygon2D::getPlotData(), mrpt::nav::PlannerTPS_VirtualBase::m_initialized_PTG, mrpt::nav::PlannerTPS_VirtualBase::m_PTGs, and mrpt::nav::PlannerTPS_VirtualBase::m_timelogger.

Referenced by initialize().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ internal_loadConfig_PTG()

void PlannerTPS_VirtualBase::internal_loadConfig_PTG ( const mrpt::config::CConfigFileBase cfgSource,
const std::string sSectionName = std::string("PTG_CONFIG") 
)
protectedinherited

Load all PTG params from a config file source.

Definition at line 92 of file PlannerRRT_common.cpp.

References ASSERT_, mrpt::math::CMatrixDynamic< T >::cols(), mrpt::nav::CParameterizedTrajectoryGenerator::CreatePTG(), mrpt::format(), mrpt::math::MatrixVectorBase< Scalar, Derived >::fromMatlabStringFormat(), mrpt::nav::PlannerTPS_VirtualBase::m_PTGs, mrpt::config::CConfigFileBase::read_double(), mrpt::config::CConfigFileBase::read_int(), mrpt::config::CConfigFileBase::read_string(), mrpt::math::CMatrixDynamic< T >::rows(), and THROW_EXCEPTION_FMT.

Referenced by loadConfig().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ loadConfig()

void PlannerRRT_SE2_TPS::loadConfig ( const mrpt::config::CConfigFileBase cfgSource,
const std::string sSectionName = std::string("PTG_CONFIG") 
)

Load all params from a config file source.

Definition at line 28 of file PlannerRRT_SE2_TPS.cpp.

References mrpt::nav::PlannerTPS_VirtualBase::internal_loadConfig_PTG().

Here is the call graph for this function:

◆ renderMoveTree()

template<typename node_pose_t , typename world_limits_t , typename tree_t >
void mrpt::nav::PlannerTPS_VirtualBase::renderMoveTree ( mrpt::opengl::COpenGLScene scene,
const TPlannerInputTempl< node_pose_t, world_limits_t > &  pi,
const TPlannerResultTempl< tree_t > &  result,
const TRenderPlannedPathOptions options 
)
inherited

Definition at line 23 of file impl_renderMoveTree.h.

References ASSERT_, ASSERT_ABOVE_, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_goal, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_ground_xy_grid, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_last_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_local_obstacles, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_normal_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_obstacles, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_optimal_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_start, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_vehicle, mrpt::opengl::stock_objects::CornerXYZ(), mrpt::opengl::stock_objects::CornerXYZSimple(), mrpt::opengl::CGridPlaneXY::Create(), mrpt::opengl::CSetOfLines::Create(), mrpt::opengl::CText3D::Create(), mrpt::opengl::CPointCloud::Create(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::draw_obstacles, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::draw_shape_decimation, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::goal_pose, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::ground_xy_grid_frequency, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_last_added_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_path_to_node_id, mrpt::opengl::COpenGLScene::insert(), INVALID_NODEID, mrpt::keep_max(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::local_obs_from_nearest_pose, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg_position, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg_scale, M_PI, mrpt::nav::PlannerTPS_VirtualBase::m_PTGs, mrpt::nav::TPlannerResultTempl< tree_t >::move_tree, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::new_state, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::obstacles_points, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::point_size_local_obstacles, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::point_size_obstacles, R, mrpt::nav::CParameterizedTrajectoryGenerator::renderPathAsSimpleLine(), mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::start_pose, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::vehicle_line_width, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::vehicle_shape_z, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::width_last_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::width_normal_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::width_optimal_edge, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::world_bbox_max, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::world_bbox_min, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::x_nearest_pose, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::x_rand_pose, and mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::xyzcorners_scale.

Here is the call graph for this function:

◆ solve()

void PlannerRRT_SE2_TPS::solve ( const TPlannerInput pi,
PlannerRRT_SE2_TPS::TPlannerResult result 
)

The main API entry point: tries to find a planned path from 'goal' to 'target'.

Definition at line 45 of file PlannerRRT_SE2_TPS.cpp.

References mrpt::nav::TPlannerResultTempl< tree_t >::acceptable_goal_node_ids, mrpt::math::angDistance(), ASSERT_ABOVE_, ASSERTMSG_, mrpt::poses::CPose2D::asTPose(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::backtrackPath(), mrpt::nav::TPlannerResultTempl< tree_t >::best_goal_node_id, mrpt::nav::TPlannerResultTempl< tree_t >::computation_time, mrpt::nav::TMoveEdgeSE2_TP::cost, mrpt::system::createDirectory(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::distance2DTo(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::nav::TMoveEdgeSE2_TP::end_state, mrpt::format(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::getAllNodes(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::getNearestNode(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::getNextFreeNodeID(), mrpt::random::getRandomGenerator(), mrpt::nav::TPlannerResultTempl< tree_t >::goal_distance, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::goal_pose, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::ground_xy_grid_frequency, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_last_added_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_path_to_node_id, mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::insertNode(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::insertNodeAndEdge(), INVALID_NODEID, mrpt::keep_max(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg_position, mrpt::nav::TPlannerResultTempl< tree_t >::move_tree, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::obstacles_points, mrpt::nav::TMoveEdgeSE2_TP::parent_id, mrpt::nav::TPlannerResultTempl< tree_t >::path_cost, mrpt::math::TPose2D::phi, mrpt::poses::CPose2D::phi(), mrpt::graphs::CDirectedTree< TYPE_EDGES >::root, mrpt::opengl::COpenGLScene::saveToFile(), mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::start_pose, mrpt::nav::TNodeSE2_TP::state, mrpt::nav::TPlannerResultTempl< tree_t >::success, mrpt::system::CTicTac::Tac(), mrpt::system::CTicTac::Tic(), mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::world_bbox_max, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::world_bbox_min, mrpt::math::wrapToPiInPlace(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::x_rand_pose, and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().

Here is the call graph for this function:

◆ spaceTransformer()

void PlannerTPS_VirtualBase::spaceTransformer ( const mrpt::maps::CSimplePointsMap in_obstacles,
const mrpt::nav::CParameterizedTrajectoryGenerator in_PTG,
const double  MAX_DIST,
std::vector< double > &  out_TPObstacles 
)
protectedinherited

Definition at line 171 of file PlannerRRT_common.cpp.

References mrpt::format(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacle().

Here is the call graph for this function:

◆ spaceTransformerOneDirectionOnly()

void PlannerTPS_VirtualBase::spaceTransformerOneDirectionOnly ( const int  tp_space_k_direction,
const mrpt::maps::CSimplePointsMap in_obstacles,
const mrpt::nav::CParameterizedTrajectoryGenerator in_PTG,
const double  MAX_DIST,
double &  out_TPObstacle_k 
)
protectedinherited

Definition at line 221 of file PlannerRRT_common.cpp.

References mrpt::format(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle(), and mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacleSingle().

Here is the call graph for this function:

◆ transformPointcloudWithSquareClipping()

void PlannerTPS_VirtualBase::transformPointcloudWithSquareClipping ( const mrpt::maps::CPointsMap in_map,
mrpt::maps::CPointsMap out_map,
const mrpt::poses::CPose2D asSeenFrom,
const double  MAX_DIST_XY 
)
staticprotectedinherited

Member Data Documentation

◆ end_criteria

RRTEndCriteria mrpt::nav::PlannerTPS_VirtualBase::end_criteria
inherited

Definition at line 134 of file PlannerRRT_common.h.

◆ m_initialized

bool mrpt::nav::PlannerRRT_SE2_TPS::m_initialized {false}
protected

Definition at line 104 of file PlannerRRT_SE2_TPS.h.

◆ m_initialized_PTG

bool mrpt::nav::PlannerTPS_VirtualBase::m_initialized_PTG {false}
protectedinherited

◆ m_local_obs

mrpt::maps::CSimplePointsMap mrpt::nav::PlannerTPS_VirtualBase::m_local_obs
protectedinherited

Definition at line 231 of file PlannerRRT_common.h.

◆ m_PTGs

mrpt::nav::TListPTGPtr mrpt::nav::PlannerTPS_VirtualBase::m_PTGs
protectedinherited

◆ m_timelogger

mrpt::system::CTimeLogger mrpt::nav::PlannerTPS_VirtualBase::m_timelogger
protectedinherited

◆ params

RRTAlgorithmParams mrpt::nav::PlannerTPS_VirtualBase::params
inherited

Parameters specific to this path solver algorithm.

Definition at line 136 of file PlannerRRT_common.h.




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019