TP Space-based RRT path planning for SE(2) (planar) robots.
This planner algorithm is described in the paper:
Typical usage:
Definition at line 68 of file PlannerRRT_SE2_TPS.h.
#include <mrpt/nav/planners/PlannerRRT_SE2_TPS.h>
Classes | |
struct | TPlannerInput |
struct | TPlannerResult |
Public Types | |
typedef mrpt::math::TPose2D | node_pose_t |
The type of poses at nodes. More... | |
Public Member Functions | |
PlannerRRT_SE2_TPS () | |
Constructor. More... | |
void | loadConfig (const mrpt::utils::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::utils::CTimeLogger & | getProfiler () |
const mrpt::nav::TListPTGPtr & | getPTGs () 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::utils::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 |
mrpt::utils::CTimeLogger | m_timelogger |
bool | m_initialized_PTG |
mrpt::nav::TListPTGPtr | m_PTGs |
mrpt::maps::CSimplePointsMap | m_local_obs |
The type of poses at nodes.
Definition at line 72 of file PlannerRRT_SE2_TPS.h.
PlannerRRT_SE2_TPS::PlannerRRT_SE2_TPS | ( | ) |
Constructor.
Definition at line 26 of file PlannerRRT_SE2_TPS.cpp.
|
inlineinherited |
Definition at line 150 of file PlannerRRT_common.h.
References mrpt::nav::PlannerTPS_VirtualBase::m_timelogger.
|
inlineinherited |
Definition at line 151 of file PlannerRRT_common.h.
References mrpt::nav::PlannerTPS_VirtualBase::m_PTGs.
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().
|
protectedinherited |
Must be called after setting all params (see internal_loadConfig_PTG()
) and before calling solve()
Definition at line 39 of file PlannerRRT_common.cpp.
References ASSERTMSG_, mrpt::mrpt::format(), mrpt::math::TPolygon2D::getPlotData(), mrpt::nav::PlannerTPS_VirtualBase::m_initialized_PTG, mrpt::nav::PlannerTPS_VirtualBase::m_PTGs, mrpt::nav::PlannerTPS_VirtualBase::m_timelogger, mrpt::nav::CPTG_RobotShape_Polygonal::setRobotShape(), and mrpt::nav::CPTG_RobotShape_Circular::setRobotShapeRadius().
Referenced by initialize().
|
protectedinherited |
Load all PTG params from a config file source.
Definition at line 98 of file PlannerRRT_common.cpp.
References ASSERT_, mrpt::nav::CParameterizedTrajectoryGenerator::CreatePTG(), mrpt::format(), mrpt::nav::PlannerTPS_VirtualBase::m_PTGs, mrpt::utils::CConfigFileBase::read_double(), mrpt::utils::CConfigFileBase::read_int(), mrpt::utils::CConfigFileBase::read_string(), and THROW_EXCEPTION_FMT.
Referenced by loadConfig().
void PlannerRRT_SE2_TPS::loadConfig | ( | const mrpt::utils::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().
|
inherited |
Definition at line 25 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::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::mrpt::utils::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.
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::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 >::distance2DTo(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::nav::TMoveEdgeSE2_TP::end_state, mrpt::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::mrpt::utils::keep_max(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg_position, min, 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::poses::CPose2D::phi(), mrpt::math::TPose2D::phi, mrpt::nav::TMoveEdgeSE2_TP::ptg_dist, mrpt::nav::TMoveEdgeSE2_TP::ptg_index, mrpt::nav::TMoveEdgeSE2_TP::ptg_K, 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, static_size, mrpt::nav::TPlannerResultTempl< tree_t >::success, mrpt::utils::CTicTac::Tac(), mrpt::utils::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 >::x(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::x_rand_pose, and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
|
protectedinherited |
Definition at line 179 of file PlannerRRT_common.cpp.
References mrpt::format(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles(), MAX_DIST, and mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacle().
|
protectedinherited |
Definition at line 229 of file PlannerRRT_common.cpp.
References mrpt::format(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle(), MAX_DIST, and mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacleSingle().
|
staticprotectedinherited |
Definition at line 145 of file PlannerRRT_common.cpp.
References mrpt::maps::CMetricMap::clear(), mrpt::poses::CPose2D::composePoint(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::maps::CPointsMap::insertPointFast(), mrpt::maps::CPointsMap::reserve(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
|
inherited |
Definition at line 143 of file PlannerRRT_common.h.
|
protected |
Definition at line 106 of file PlannerRRT_SE2_TPS.h.
|
protectedinherited |
Definition at line 251 of file PlannerRRT_common.h.
Referenced by mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG().
|
protectedinherited |
Definition at line 253 of file PlannerRRT_common.h.
|
protectedinherited |
|
protectedinherited |
Definition at line 250 of file PlannerRRT_common.h.
Referenced by mrpt::nav::PlannerTPS_VirtualBase::getProfiler(), and mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG().
|
inherited |
Parameters specific to this path solver algorithm.
Definition at line 145 of file PlannerRRT_common.h.
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019 |