Main MRPT website > C++ reference for MRPT 1.4.0
PlannerRRT_SE2_TPS.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9
10#pragma once
11
17#include <numeric>
18
20
21namespace mrpt
22{
23 namespace nav
24 {
25 /** \addtogroup nav_planners Path planning
26 * \ingroup mrpt_nav_grp
27 * @{ */
28
29 /** TP Space-based RRT path planning for SE(2) (planar) robots.
30 *
31 * This planner algorithm is described in the paper:
32 * - M. Bellone, J.L. Blanco, A. Gimenez, "TP-Space RRT: Kinematic path planning of non-holonomic any-shape vehicles", International Journal of Advanced Robotic Systems, 2015.
33 *
34 * Typical usage:
35 * \code
36 * mrpt::nav::PlannerRRT_SE2_TPS planner;
37 *
38 * // Set or load planner parameters:
39 * //planner.loadConfig( mrpt::utils::CConfigFile("config_file.cfg") );
40 * //planner.params.... // See TAlgorithmParams
41 *
42 * // Set RRT end criteria (when to stop searching for a solution)
43 * //planner.end_criteria.... // See TEndCriteria
44 *
45 * planner.initialize(); // Initialize after setting the algorithm parameters
46 *
47 * // Set up planning problem:
48 * PlannerRRT_SE2_TPS::TPlannerResult planner_result;
49 * PlannerRRT_SE2_TPS::TPlannerInput planner_input;
50 * // Start & goal:
51 * planner_input.start_pose = mrpt::math::TPose2D(XXX,XXX,XXX);
52 * planner_input.goal_pose = mrpt::math::TPose2D(XXX,XXX,XXX);
53 * // Set obtacles: (...)
54 * // planner_input.obstacles_points ...
55 * // Set workspace bounding box for picking random poses in the RRT algorithm:
56 * planner_input.world_bbox_min = mrpt::math::TPoint2D(XX,YY);
57 * planner_input.world_bbox_max = mrpt::math::TPoint2D(XX,YY);
58 * // Do path planning:
59 * planner.solve( planner_input, planner_result);
60 * // Analyze contents of planner_result...
61 * \endcode
62 *
63 * - Changes history:
64 * - 06/MAR/2014: Creation (MB)
65 * - 06/JAN/2015: Refactoring (JLBC)
66 *
67 * \todo Factorize into more generic path planner classes! //template <class POSE, class MOTIONS>...
68 */
70 {
71 public:
72 typedef mrpt::math::TPose2D node_pose_t; //!< The type of poses at nodes
73
75 {
76 double acceptedDistToTarget; //!< Maximum distance from a pose to target to accept it as a valid solution (meters). (Both acceptedDistToTarget & acceptedAngToTarget must be satisfied)
77 double acceptedAngToTarget; //!< Maximum angle from a pose to target to accept it as a valid solution (rad). (Both acceptedDistToTarget & acceptedAngToTarget must be satisfied)
78
79 double maxComputationTime; //!< In seconds. 0 means no limit until a solution is found.
80 double minComputationTime; //!< In seconds. 0 means the first valid path will be returned. Otherwise, the algorithm will try to refine and find a better one.
81
83 acceptedDistToTarget ( 0.1 ),
84 acceptedAngToTarget ( mrpt::utils::DEG2RAD(180) ),
85 maxComputationTime ( 0.0 ),
86 minComputationTime ( 0.0 )
87 {
88 }
89 };
91
93 {
94 /** The robot shape used when computing collisions; it's loaded from the
95 * config file/text as a single 2xN matrix in MATLAB format, first row are Xs, second are Ys, e.g:
96 * \code
97 * robot_shape = [-0.2 0.2 0.2 -0.2; -0.1 -0.1 0.1 0.1]
98 * \endcode
99 */
101 std::string ptg_cache_files_directory; //!< (Default: ".")
102
103 double goalBias; //!< Probabily of picking the goal as random target (in [0,1], default=0.05)
104 double maxLength; //!< (Very sensitive parameter!) Max length of each edge path (in meters, default=1.0)
105 double minDistanceBetweenNewNodes; //!< Minimum distance [meters] to nearest node to accept creating a new one (default=0.10). (Any of minDistanceBetweenNewNodes and minAngBetweenNewNodes must be satisfied)
106 double minAngBetweenNewNodes; //!< Minimum angle [rad] to nearest node to accept creating a new one (default=15 deg) (Any of minDistanceBetweenNewNodes and minAngBetweenNewNodes must be satisfied)
107 bool ptg_verbose; //!< Display PTG construction info (default=true)
108
109 size_t save_3d_log_freq; //!< Frequency (in iters) of saving tree state to debug log files viewable in SceneViewer3D (default=0, disabled)
110
112 ptg_cache_files_directory("."),
113 goalBias(0.05),
114 maxLength(1.0),
115 minDistanceBetweenNewNodes(0.10),
116 minAngBetweenNewNodes(mrpt::utils::DEG2RAD(15)),
117 ptg_verbose(true),
118 save_3d_log_freq(0)
119 {
120 robot_shape.push_back( mrpt::math::TPoint2D(-0.5,-0.5) );
121 robot_shape.push_back( mrpt::math::TPoint2D( 0.8,-0.4) );
122 robot_shape.push_back( mrpt::math::TPoint2D( 0.8, 0.4) );
123 robot_shape.push_back( mrpt::math::TPoint2D(-0.5, 0.5) );
124 }
125 };
126 TAlgorithmParams params; //!< Parameters specific to this path solver algorithm
127
129 {
132
133 mrpt::math::TPose2D world_bbox_min,world_bbox_max; //!< Bounding box of the world, used to draw uniform random pose samples
134
135 mrpt::maps::CSimplePointsMap obstacles_points; //!< World obstacles, as a point cloud
136
138 start_pose(0,0,0),
139 goal_pose(0,0,0),
140 world_bbox_min(-10.,-10.0,-M_PI),
141 world_bbox_max( 10., 10.0, M_PI)
142 {
143 }
144 };
145
147 {
148 bool success; //!< Whether the target was reached or not
149 double computation_time; //!< Time spent (in secs)
150 double goal_distance; //!< Distance from best found path to goal
151 double path_cost; //!< Total cost of the best found path (cost ~~ Euclidean distance)
152 mrpt::utils::TNodeID best_goal_node_id; //!< The ID of the best target node in the tree
153 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 others)
154 TMoveTreeSE2_TP move_tree; //!< The generated motion tree that explores free space starting at "start"
155
157 success(false),
158 computation_time(0),
159 goal_distance( std::numeric_limits<double>::max() ),
160 path_cost( std::numeric_limits<double>::max() ),
161 best_goal_node_id(INVALID_NODEID)
162 {
163 }
164 };
165
166 /** Constructor */
168
169 /** Load all params from a config file source */
170 void loadConfig(const mrpt::utils::CConfigFileBase &cfgSource, const std::string &sSectionName = std::string("PTG_CONFIG"));
171
172 /** Must be called after setting all params (see `loadConfig()`) and before calling `solve()` */
174
175 /** The main API entry point: tries to find a planned path from 'goal' to 'target' */
176 void solve( const TPlannerInput &pi, TPlannerResult & result );
177
178 /** Options for renderMoveTree() */
180 {
181 mrpt::utils::TNodeID highlight_path_to_node_id; //!< Highlight the path from root towards this node (usually, the target)
182 size_t draw_shape_decimation; //!< (Default=1) Draw one out of N vehicle shapes along the highlighted path
183
188
189 double xyzcorners_scale; //!< A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape)
190 bool highlight_last_added_edge; //!< (Default=false)
191 double ground_xy_grid_frequency; //!< (Default=10 meters) Set to 0 to disable
192
195 mrpt::utils::TColor color_local_obstacles; //!< local obstacles color
196 mrpt::utils::TColor color_start; //!< START indication color
197 mrpt::utils::TColor color_goal; //!< END indication color
207
208 double vehicle_shape_z; //!< (Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps making it in the "first plane"
209 double vehicle_line_width; //!< Robot line width for visualization - default 2.0
210 bool draw_obstacles; //!< (Default=true)
211
212 std::string log_msg;
215
217 highlight_path_to_node_id( INVALID_NODEID ),
218 draw_shape_decimation(1),
219 x_rand_pose( NULL ),
220 x_nearest_pose( NULL ),
221 local_obs_from_nearest_pose( NULL ),
222 new_state( NULL ),
223 xyzcorners_scale(0),
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),
244 log_msg_scale(0.2)
245
246 {
247 }
248
250 };
251
254 const TPlannerInput &pi,
255 const TPlannerResult &result,
256 const TRenderPlannedPathOptions &options
257 );
258
260
261 mrpt::utils::CTimeLogger & getProfiler() { return m_timelogger; }
262 const mrpt::nav::TListPTGPtr & getPTGs() const { return m_PTGs;}
263
264 protected:
268 mrpt::maps::CSimplePointsMap m_local_obs; // Temporary map. Defined as a member to save realloc time between calls
269
271 const mrpt::maps::CPointsMap & in_map,
272 mrpt::maps::CPointsMap & out_map,
273 const mrpt::poses::CPose2D & asSeenFrom,
274 const double MAX_DIST_XY
275 );
276
278 const mrpt::maps::CSimplePointsMap &in_obstacles,
280 const double MAX_DIST,
281 std::vector<float> &out_TPObstacles
282 );
283
284 }; // end class PlannerRRT_SE2_TPS
285
286 /** @} */
287 }
288}
#define M_PI
Definition: bits.h:65
#define DEG2RAD
Definition: bits.h:86
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.
void initialize()
Must be called after setting all params (see loadConfig()) and before calling solve()
const mrpt::nav::TListPTGPtr & getPTGs() const
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.
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)
mrpt::utils::CTimeLogger & getProfiler()
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.
Definition: COpenGLScene.h:50
A class used to store a 2D pose.
Definition: CPose2D.h:37
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),...
Definition: CTimeLogger.h:36
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
#define INVALID_NODEID
Definition: types_simple.h:47
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.
STL namespace.
Lightweight 2D point.
Lightweight 3D point.
Lightweight 2D pose.
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)
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.
mrpt::math::TPose2D world_bbox_max
Bounding box of the world, used to draw uniform random pose samples.
mrpt::maps::CSimplePointsMap obstacles_points
World obstacles, as a point cloud.
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 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.
mrpt::utils::TColor color_local_obstacles
local obstacles color
double vehicle_shape_z
(Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps making it in the "first plane"
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
double xyzcorners_scale
A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape)
mrpt::utils::TColor color_start
START indication color.
double vehicle_line_width
Robot line width for visualization - default 2.0.
mrpt::utils::TColor color_goal
END indication color.
size_t draw_shape_decimation
(Default=1) Draw one out of N vehicle shapes along the highlighted path
mrpt::utils::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
A RGB color - 8bit.
Definition: TColor.h:26



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Sat Jan 21 06:46:15 UTC 2023