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 
16 #include <mrpt/utils/CTimeLogger.h>
17 #include <numeric>
18 
19 #include <mrpt/nav/link_pragmas.h>
20 
21 namespace 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()` */
173  void initialize();
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 
194  mrpt::utils::TColor color_obstacles; //!< obstacles color
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.
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.
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
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.
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.1 for MRPT 1.4.0 SVN: at Sun Mar 7 18:43:46 UTC 2021