Main MRPT website > C++ reference for MRPT 1.5.7
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-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 
14 #include <mrpt/utils/CTimeLogger.h>
16 #include <mrpt/nav/link_pragmas.h>
17 #include <string>
18 #include <cstdlib> // size_t
19 
20 namespace mrpt
21 {
22  namespace nav
23  {
24  /** \addtogroup nav_planners Path planning
25  * \ingroup mrpt_nav_grp
26  * @{ */
27 
28  template <typename node_pose_t, typename world_limits_t>
30  {
31  node_pose_t start_pose, goal_pose;
32  world_limits_t world_bbox_min, world_bbox_max; //!< Bounding box of the world, used to draw uniform random pose samples
33  mrpt::maps::CSimplePointsMap obstacles_points; //!< World obstacles, as a point cloud
34  };
35 
36  template <typename tree_t>
38  {
39  bool success; //!< Whether the target was reached or not
40  double computation_time; //!< Time spent (in secs)
41  double goal_distance; //!< Distance from best found path to goal
42  double path_cost; //!< Total cost of the best found path (cost ~~ Euclidean distance)
43  mrpt::utils::TNodeID best_goal_node_id; //!< The ID of the best target node in the tree
44  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)
45  tree_t move_tree; //!< The generated motion tree that explores free space starting at "start"
46 
48  success(false),
50  goal_distance(std::numeric_limits<double>::max()),
51  path_cost(std::numeric_limits<double>::max()),
53  {
54  }
55  };
56 
58  {
59  double acceptedDistToTarget; //!< Maximum distance from a pose to target to accept it as a valid solution (meters). (Both acceptedDistToTarget & acceptedAngToTarget must be satisfied)
60  double acceptedAngToTarget; //!< Maximum angle from a pose to target to accept it as a valid solution (rad). (Both acceptedDistToTarget & acceptedAngToTarget must be satisfied)
61 
62  double maxComputationTime; //!< In seconds. 0 means no limit until a solution is found.
63  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.
64 
66  acceptedDistToTarget(0.1),
67  acceptedAngToTarget(mrpt::utils::DEG2RAD(180)),
68  maxComputationTime(0.0),
69  minComputationTime(0.0)
70  {
71  }
72  };
73 
75  {
76  /** The robot shape used when computing collisions; it's loaded from the
77  * config file/text as a single 2xN matrix in MATLAB format, first row are Xs, second are Ys, e.g:
78  * \code
79  * robot_shape = [-0.2 0.2 0.2 -0.2; -0.1 -0.1 0.1 0.1]
80  * \endcode
81  * \note PTGs will use only one of either `robot_shape` or `robot_shape_circular_radius`
82  */
84  /** The radius of a circular-shape-model of the robot shape.
85  * \note PTGs will use only one of either `robot_shape` or `robot_shape_circular_radius`
86  */
88 
90 
91  double goalBias; //!< Probabily of picking the goal as random target (in [0,1], default=0.05)
92  double maxLength; //!< (Very sensitive parameter!) Max length of each edge path (in meters, default=1.0)
93  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)
94  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)
95  bool ptg_verbose; //!< Display PTG construction info (default=true)
96 
97  size_t save_3d_log_freq; //!< Frequency (in iters) of saving tree state to debug log files viewable in SceneViewer3D (default=0, disabled)
98 
100  };
101 
102  /** Virtual base class for TP-Space-based path planners */
104  {
105  public:
107  RRTAlgorithmParams params; //!< Parameters specific to this path solver algorithm
108 
109  PlannerTPS_VirtualBase(); //!< ctor
110 
111  mrpt::utils::CTimeLogger & getProfiler() { return m_timelogger; }
112  const mrpt::nav::TListPTGPtr & getPTGs() const { return m_PTGs; }
113 
114  /** Options for renderMoveTree() */
116  {
117  mrpt::utils::TNodeID highlight_path_to_node_id; //!< Highlight the path from root towards this node (usually, the target)
118  size_t draw_shape_decimation; //!< (Default=1) Draw one out of N vehicle shapes along the highlighted path
119 
124 
125  double xyzcorners_scale; //!< A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape)
126  bool highlight_last_added_edge; //!< (Default=false)
127  double ground_xy_grid_frequency; //!< (Default=10 meters) Set to 0 to disable
128 
130  mrpt::utils::TColor color_obstacles; //!< obstacles color
131  mrpt::utils::TColor color_local_obstacles; //!< local obstacles color
132  mrpt::utils::TColor color_start; //!< START indication color
133  mrpt::utils::TColor color_goal; //!< END indication color
143 
144  double vehicle_shape_z; //!< (Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps making it in the "first plane"
145  double vehicle_line_width; //!< Robot line width for visualization - default 2.0
146  bool draw_obstacles; //!< (Default=true)
147 
151 
153  highlight_path_to_node_id(INVALID_NODEID),
154  draw_shape_decimation(1),
155  x_rand_pose(NULL),
156  x_nearest_pose(NULL),
157  local_obs_from_nearest_pose(NULL),
158  new_state(NULL),
159  xyzcorners_scale(0),
160  highlight_last_added_edge(false),
161  ground_xy_grid_frequency(10.0),
162  color_vehicle(0xFF, 0x00, 0x00, 0xFF),
163  color_obstacles(0x00, 0x00, 0xFF, 0x40),
164  color_local_obstacles(0x00, 0x00, 0xFF),
165  color_start(0x00, 0x00, 0x00, 0x00),
166  color_goal(0x00, 0x00, 0x00, 0x00),
167  color_ground_xy_grid(0xFF, 0xFF, 0xFF),
168  color_normal_edge(0x22, 0x22, 0x22, 0x40),
169  color_last_edge(0xff, 0xff, 0x00),
170  color_optimal_edge(0x00, 0x00, 0x00),
171  width_last_edge(3.f),
172  width_normal_edge(1.f),
173  width_optimal_edge(4.f),
174  point_size_obstacles(5),
175  point_size_local_obstacles(5),
176  vehicle_shape_z(0.01),
177  vehicle_line_width(2.0),
178  draw_obstacles(true),
179  log_msg_position(0, 0, 0),
180  log_msg_scale(0.2)
181  {
182  }
183 
185  };
186 
187  template <typename node_pose_t, typename world_limits_t, typename tree_t>
188  void renderMoveTree(
191  const TPlannerResultTempl<tree_t> &result,
192  const TRenderPlannedPathOptions &options
193  );
194 
195  protected:
199  mrpt::maps::CSimplePointsMap m_local_obs; // Temporary map. Defined as a member to save realloc time between calls
200 
201  /** Load all PTG params from a config file source */
202  void internal_loadConfig_PTG(const mrpt::utils::CConfigFileBase &cfgSource, const std::string &sSectionName = std::string("PTG_CONFIG"));
203 
204  /** Must be called after setting all params (see `internal_loadConfig_PTG()`) and before calling `solve()` */
205  void internal_initialize_PTG();
206 
207  static void transformPointcloudWithSquareClipping(
208  const mrpt::maps::CPointsMap & in_map,
209  mrpt::maps::CPointsMap & out_map,
210  const mrpt::poses::CPose2D & asSeenFrom,
211  const double MAX_DIST_XY
212  );
213 
214  void spaceTransformer(
215  const mrpt::maps::CSimplePointsMap &in_obstacles,
217  const double MAX_DIST,
218  std::vector<double> &out_TPObstacles
219  );
220 
221  void spaceTransformerOneDirectionOnly(
222  const int tp_space_k_direction,
223  const mrpt::maps::CSimplePointsMap &in_obstacles,
225  const double MAX_DIST,
226  double &out_TPObstacle_k
227  );
228 
229  }; // end class PlannerTPS_VirtualBase
230  /** @} */
231  }
232 }
233 
234 // Impl of renderMoveTree<>
235 #include "impl_renderMoveTree.h"
236 
mrpt::utils::TColor color_goal
END indication color.
double minComputationTime
In seconds. 0 means the first valid path will be returned. Otherwise, the algorithm will try to refin...
double vehicle_shape_z
(Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps making it in the "first plane" ...
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)
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...
world_limits_t world_bbox_max
Bounding box of the world, used to draw uniform random pose samples.
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...
#define INVALID_NODEID
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
mrpt::utils::CTimeLogger & getProfiler()
mrpt::utils::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
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). (Both acceptedDistToTarge...
This class allows loading and storing values and vectors of different types from a configuration text...
#define MAX_DIST(s)
Definition: deflate.h:280
double robot_shape_circular_radius
The radius of a circular-shape-model of the robot shape.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This is the base class for any user-defined PTG.
uint64_t TNodeID
The type for node IDs in graphs of different types.
bool success
Whether the target was reached or not.
Virtual base class for TP-Space-based path planners.
A RGB color - 8bit.
Definition: TColor.h:26
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). (Both acceptedDistT...
#define DEG2RAD
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt::utils::TNodeID best_goal_node_id
The ID of the best target node in the tree.
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)
double maxComputationTime
In seconds. 0 means no limit until a solution is found.
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)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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:49
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
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)
mrpt::utils::TColor color_start
START indication color.
std::vector< mrpt::nav::CParameterizedTrajectoryGeneratorPtr > TListPTGPtr
A list of PTGs (smart pointers)
Lightweight 3D point.
std::string ptg_cache_files_directory
(Default: ".")
2D polygon, inheriting from std::vector<TPoint2D>.
mrpt::utils::TColor color_local_obstacles
local obstacles color
mrpt::utils::CTimeLogger m_timelogger
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019