83 acceptedDistToTarget ( 0.1 ),
85 maxComputationTime ( 0.0 ),
86 minComputationTime ( 0.0 )
112 ptg_cache_files_directory(
"."),
115 minDistanceBetweenNewNodes(0.10),
140 world_bbox_min(-10.,-10.0,-
M_PI),
141 world_bbox_max( 10., 10.0,
M_PI)
159 goal_distance( std::numeric_limits<double>::max() ),
160 path_cost( std::numeric_limits<double>::max() ),
218 draw_shape_decimation(1),
220 x_nearest_pose( NULL ),
221 local_obs_from_nearest_pose( NULL ),
224 highlight_last_added_edge(false),
225 ground_xy_grid_frequency(10.0),
226 color_vehicle(0xFF,0x00,0x00,0xFF),
227 color_obstacles(0x00,0x00,0xFF,0x40),
228 color_local_obstacles(0x00,0x00,0xFF),
229 color_start(0x00, 0x00, 0x00, 0x00),
230 color_goal(0x00, 0x00, 0x00, 0x00),
231 color_ground_xy_grid(0xFF,0xFF,0xFF),
232 color_normal_edge(0x22,0x22,0x22,0x40),
233 color_last_edge(0xff,0xff,0x00),
234 color_optimal_edge(0x00,0x00,0x00),
235 width_last_edge(3.f),
236 width_normal_edge(1.f),
237 width_optimal_edge(4.f),
238 point_size_obstacles(5),
239 point_size_local_obstacles(5),
240 vehicle_shape_z(0.01),
241 vehicle_line_width(2.0),
242 draw_obstacles(true),
243 log_msg_position(0,0,0),
274 const double MAX_DIST_XY
280 const double MAX_DIST,
281 std::vector<float> &out_TPObstacles
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
2D polygon, inheriting from std::vector<TPoint2D>.
This is the base class for any user-defined PTG.
TP Space-based RRT path planning for SE(2) (planar) robots.
PlannerRRT_SE2_TPS()
Constructor.
mrpt::utils::CTimeLogger & getProfiler()
void initialize()
Must be called after setting all params (see loadConfig()) and before calling solve()
mrpt::nav::TListPTGPtr m_PTGs
void loadConfig(const mrpt::utils::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all params from a config file source.
void setRenderTreeVisualization()
TAlgorithmParams params
Parameters specific to this path solver algorithm.
void spaceTransformer(const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, std::vector< float > &out_TPObstacles)
const mrpt::nav::TListPTGPtr & getPTGs() const
TEndCriteria end_criteria
void solve(const TPlannerInput &pi, TPlannerResult &result)
The main API entry point: tries to find a planned path from 'goal' to 'target'.
mrpt::utils::CTimeLogger m_timelogger
mrpt::maps::CSimplePointsMap m_local_obs
mrpt::math::TPose2D node_pose_t
The type of poses at nodes.
void renderMoveTree(mrpt::opengl::COpenGLScene &scene, const TPlannerInput &pi, const TPlannerResult &result, const TRenderPlannedPathOptions &options)
static void transformPointcloudWithSquareClipping(const mrpt::maps::CPointsMap &in_map, mrpt::maps::CPointsMap &out_map, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY)
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
A class used to store a 2D pose.
This class allows loading and storing values and vectors of different types from a configuration text...
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
uint64_t TNodeID
The type for node IDs in graphs of different types.
std::vector< mrpt::nav::CParameterizedTrajectoryGeneratorPtr > TListPTGPtr
A list of PTGs (smart pointers)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double minDistanceBetweenNewNodes
Minimum distance [meters] to nearest node to accept creating a new one (default=0....
mrpt::math::TPolygon2D robot_shape
The robot shape used when computing collisions; it's loaded from the config file/text as a single 2xN...
double maxLength
(Very sensitive parameter!) Max length of each edge path (in meters, default=1.0)
double minAngBetweenNewNodes
Minimum angle [rad] to nearest node to accept creating a new one (default=15 deg) (Any of minDistance...
double goalBias
Probabily of picking the goal as random target (in [0,1], default=0.05)
std::string ptg_cache_files_directory
(Default: ".")
size_t save_3d_log_freq
Frequency (in iters) of saving tree state to debug log files viewable in SceneViewer3D (default=0,...
bool ptg_verbose
Display PTG construction info (default=true)
double acceptedDistToTarget
Maximum distance from a pose to target to accept it as a valid solution (meters). (Both acceptedDistT...
double minComputationTime
In seconds. 0 means the first valid path will be returned. Otherwise, the algorithm will try to refin...
double acceptedAngToTarget
Maximum angle from a pose to target to accept it as a valid solution (rad). (Both acceptedDistToTarge...
double maxComputationTime
In seconds. 0 means no limit until a solution is found.
TMoveTreeSE2_TP move_tree
The generated motion tree that explores free space starting at "start".
double path_cost
Total cost of the best found path (cost ~~ Euclidean distance)
double computation_time
Time spent (in secs)
double goal_distance
Distance from best found path to goal.
mrpt::utils::TNodeID best_goal_node_id
The ID of the best target node in the tree.
std::set< mrpt::utils::TNodeID > acceptable_goal_node_ids
The set of target nodes within an acceptable distance to target (including best_goal_node_id and othe...
bool success
Whether the target was reached or not.
Options for renderMoveTree()
mrpt::utils::TColor color_local_obstacles
local obstacles color
const mrpt::poses::CPose2D * new_state
double vehicle_shape_z
(Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps making it in the "first plane"
TRenderPlannedPathOptions()
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
const mrpt::poses::CPose2D * x_nearest_pose
double xyzcorners_scale
A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape)
mrpt::utils::TColor color_vehicle
Robot color.
mrpt::utils::TColor color_start
START indication color.
bool highlight_last_added_edge
(Default=false)
double vehicle_line_width
Robot line width for visualization - default 2.0.
const mrpt::maps::CPointsMap * local_obs_from_nearest_pose
bool draw_obstacles
(Default=true)
~TRenderPlannedPathOptions()
mrpt::math::TPoint3D log_msg_position
mrpt::utils::TColor color_optimal_edge
int point_size_local_obstacles
mrpt::utils::TColor color_goal
END indication color.
mrpt::utils::TColor color_last_edge
mrpt::utils::TColor color_obstacles
obstacles color
size_t draw_shape_decimation
(Default=1) Draw one out of N vehicle shapes along the highlighted path
const mrpt::poses::CPose2D * x_rand_pose
mrpt::utils::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
mrpt::utils::TColor color_ground_xy_grid
mrpt::utils::TColor color_normal_edge