Main MRPT website > C++ reference for MRPT 1.9.9
PlannerRRT_common.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-2018, 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 <mrpt/graphs/TNodeID.h>
18 #include <string>
19 #include <cstdlib> // size_t
20 
21 namespace mrpt
22 {
23 namespace nav
24 {
25 /** \addtogroup nav_planners Path planning
26 * \ingroup mrpt_nav_grp
27 * @{ */
28 
29 template <typename node_pose_t, typename world_limits_t>
31 {
32  node_pose_t start_pose, goal_pose;
33  /** Bounding box of the world, used to draw uniform random pose samples */
34  world_limits_t world_bbox_min, world_bbox_max;
35  /** World obstacles, as a point cloud */
37 };
38 
39 template <typename tree_t>
41 {
42  /** Whether the target was reached or not */
43  bool success;
44  /** Time spent (in secs) */
46  /** Distance from best found path to goal */
47  double goal_distance;
48  /** Total cost of the best found path (cost ~~ Euclidean distance) */
49  double path_cost;
50  /** The ID of the best target node in the tree */
52  /** The set of target nodes within an acceptable distance to target
53  * (including `best_goal_node_id` and others) */
54  std::set<mrpt::graphs::TNodeID> acceptable_goal_node_ids;
55  /** The generated motion tree that explores free space starting at "start"
56  */
57  tree_t move_tree;
58 
60  : success(false),
62  goal_distance(std::numeric_limits<double>::max()),
63  path_cost(std::numeric_limits<double>::max()),
65  {
66  }
67 };
68 
70 {
71  /** Maximum distance from a pose to target to accept it as a valid solution
72  * (meters). (Both acceptedDistToTarget & acceptedAngToTarget must be
73  * satisfied) */
75  /** Maximum angle from a pose to target to accept it as a valid solution
76  * (rad). (Both acceptedDistToTarget & acceptedAngToTarget must be
77  * satisfied) */
79 
80  /** In seconds. 0 means no limit until a solution is found. */
82  /** In seconds. 0 means the first valid path will be returned. Otherwise,
83  * the algorithm will try to refine and find a better one. */
85 
87  : acceptedDistToTarget(0.1),
89  maxComputationTime(0.0),
91  {
92  }
93 };
94 
96 {
97  /** The robot shape used when computing collisions; it's loaded from the
98  * config file/text as a single 2xN matrix in MATLAB format, first row are
99  * Xs, second are Ys, e.g:
100  * \code
101  * robot_shape = [-0.2 0.2 0.2 -0.2; -0.1 -0.1 0.1 0.1]
102  * \endcode
103  * \note PTGs will use only one of either `robot_shape` or
104  * `robot_shape_circular_radius`
105  */
107  /** The radius of a circular-shape-model of the robot shape.
108  * \note PTGs will use only one of either `robot_shape` or
109  * `robot_shape_circular_radius`
110  */
112 
113  /** (Default: ".") */
115 
116  /** Probabily of picking the goal as random target (in [0,1], default=0.05)
117  */
118  double goalBias;
119  /** (Very sensitive parameter!) Max length of each edge path (in meters,
120  * default=1.0) */
121  double maxLength;
122  /** Minimum distance [meters] to nearest node to accept creating a new one
123  * (default=0.10). (Any of minDistanceBetweenNewNodes and
124  * minAngBetweenNewNodes must be satisfied) */
126  /** Minimum angle [rad] to nearest node to accept creating a new one
127  * (default=15 deg) (Any of minDistanceBetweenNewNodes and
128  * minAngBetweenNewNodes must be satisfied) */
130  /** Display PTG construction info (default=true) */
132 
133  /** Frequency (in iters) of saving tree state to debug log files viewable in
134  * SceneViewer3D (default=0, disabled) */
136 
138 };
139 
140 /** Virtual base class for TP-Space-based path planners */
142 {
143  public:
145  /** Parameters specific to this path solver algorithm */
147 
148  /** ctor */
150 
152  const mrpt::nav::TListPTGPtr& getPTGs() const { return m_PTGs; }
153  /** Options for renderMoveTree() */
155  {
156  /** Highlight the path from root towards this node (usually, the target)
157  */
159  /** (Default=1) Draw one out of N vehicle shapes along the highlighted
160  * path */
162 
167 
168  /** A scale factor to all XYZ corners (default=0, means auto determien
169  * from vehicle shape) */
171  /** (Default=false) */
173  /** (Default=10 meters) Set to 0 to disable */
175 
176  /** Robot color */
178  /** obstacles color */
180  /** local obstacles color */
182  /** START indication color */
184  /** END indication color */
195 
196  /** (Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps
197  * making it in the "first plane" */
199  /** Robot line width for visualization - default 2.0 */
201  /** (Default=true) */
203 
207 
211  x_rand_pose(NULL),
212  x_nearest_pose(NULL),
214  new_state(NULL),
215  xyzcorners_scale(0),
218  color_vehicle(0xFF, 0x00, 0x00, 0xFF),
219  color_obstacles(0x00, 0x00, 0xFF, 0x40),
220  color_local_obstacles(0x00, 0x00, 0xFF),
221  color_start(0x00, 0x00, 0x00, 0x00),
222  color_goal(0x00, 0x00, 0x00, 0x00),
223  color_ground_xy_grid(0xFF, 0xFF, 0xFF),
224  color_normal_edge(0x22, 0x22, 0x22, 0x40),
225  color_last_edge(0xff, 0xff, 0x00),
226  color_optimal_edge(0x00, 0x00, 0x00),
227  width_last_edge(3.f),
228  width_normal_edge(1.f),
229  width_optimal_edge(4.f),
232  vehicle_shape_z(0.01),
233  vehicle_line_width(2.0),
234  draw_obstacles(true),
235  log_msg_position(0, 0, 0),
236  log_msg_scale(0.2)
237  {
238  }
239 
241  };
242 
243  template <typename node_pose_t, typename world_limits_t, typename tree_t>
244  void renderMoveTree(
247  const TPlannerResultTempl<tree_t>& result,
248  const TRenderPlannedPathOptions& options);
249 
250  protected:
254  mrpt::maps::CSimplePointsMap m_local_obs; // Temporary map. Defined as a
255  // member to save realloc time
256  // between calls
257 
258  /** Load all PTG params from a config file source */
260  const mrpt::config::CConfigFileBase& cfgSource,
261  const std::string& sSectionName = std::string("PTG_CONFIG"));
262 
263  /** Must be called after setting all params (see
264  * `internal_loadConfig_PTG()`) and before calling `solve()` */
266 
268  const mrpt::maps::CPointsMap& in_map, mrpt::maps::CPointsMap& out_map,
269  const mrpt::poses::CPose2D& asSeenFrom, const double MAX_DIST_XY);
270 
271  void spaceTransformer(
272  const mrpt::maps::CSimplePointsMap& in_obstacles,
274  const double MAX_DIST, std::vector<double>& out_TPObstacles);
275 
277  const int tp_space_k_direction,
278  const mrpt::maps::CSimplePointsMap& in_obstacles,
280  const double MAX_DIST, double& out_TPObstacle_k);
281 
282 }; // end class PlannerTPS_VirtualBase
283 /** @} */
284 }
285 }
286 
287 // Impl of renderMoveTree<>
288 #include "impl_renderMoveTree.h"
double minComputationTime
In seconds.
double vehicle_shape_z
(Default=0.01) Height (Z coordinate) for the vehicle shapes.
double minDistanceBetweenNewNodes
Minimum distance [meters] to nearest node to accept creating a new one (default=0.10).
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.
std::set< mrpt::graphs::TNodeID > acceptable_goal_node_ids
The set of target nodes within an acceptable distance to target (including best_goal_node_id and othe...
mrpt::graphs::TNodeID best_goal_node_id
The ID of the best target node in the tree.
mrpt::system::CTimeLogger m_timelogger
double DEG2RAD(const double x)
Degrees to radians.
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...
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)
void spaceTransformer(const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, std::vector< double > &out_TPObstacles)
world_limits_t world_bbox_min
Bounding box of the world, used to draw uniform random pose samples.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
size_t save_3d_log_freq
Frequency (in iters) of saving tree state to debug log files viewable in SceneViewer3D (default=0...
double xyzcorners_scale
A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape) ...
double acceptedAngToTarget
Maximum angle from a pose to target to accept it as a valid solution (rad).
#define MAX_DIST(s)
Definition: deflate.h:285
mrpt::img::TColor color_start
START indication color.
double robot_shape_circular_radius
The radius of a circular-shape-model of the robot shape.
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator::Ptr > TListPTGPtr
A list of PTGs (smart pointers)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
bool success
Whether the target was reached or not.
Virtual base class for TP-Space-based path planners.
double vehicle_line_width
Robot line width for visualization - default 2.0.
const mrpt::nav::TListPTGPtr & getPTGs() const
double acceptedDistToTarget
Maximum distance from a pose to target to accept it as a valid solution (meters). ...
GLsizei const GLchar ** string
Definition: glext.h:4101
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
double path_cost
Total cost of the best found path (cost ~~ Euclidean distance)
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve() ...
double maxComputationTime
In seconds.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::maps::CSimplePointsMap obstacles_points
World obstacles, as a point cloud.
RRTAlgorithmParams params
Parameters specific to this path solver algorithm.
double goal_distance
Distance from best found path to goal.
double computation_time
Time spent (in secs)
static void transformPointcloudWithSquareClipping(const mrpt::maps::CPointsMap &in_map, mrpt::maps::CPointsMap &out_map, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY)
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
mrpt::maps::CSimplePointsMap m_local_obs
bool ptg_verbose
Display PTG construction info (default=true)
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:59
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:17
size_t draw_shape_decimation
(Default=1) Draw one out of N vehicle shapes along the highlighted path
mrpt::math::TPolygon2D robot_shape
The robot shape used when computing collisions; it&#39;s loaded from the config file/text as a single 2xN...
double goalBias
Probabily of picking the goal as random target (in [0,1], default=0.05)
#define INVALID_NODEID
Definition: TNodeID.h:20
mrpt::system::CTimeLogger & getProfiler()
A RGB color - 8bit.
Definition: TColor.h:22
mrpt::graphs::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
void renderMoveTree(mrpt::opengl::COpenGLScene &scene, const TPlannerInputTempl< node_pose_t, world_limits_t > &pi, const TPlannerResultTempl< tree_t > &result, const TRenderPlannedPathOptions &options)
Lightweight 3D point.
std::string ptg_cache_files_directory
(Default: ".")
2D polygon, inheriting from std::vector<TPoint2D>.
mrpt::img::TColor color_local_obstacles
local obstacles color
tree_t move_tree
The generated motion tree that explores free space starting at "start".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019