MRPT  2.0.4
PlannerRRT_common.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
12 #include <mrpt/graphs/TNodeID.h>
14 #include <mrpt/math/TPoint3D.h>
18 #include <cstdlib> // size_t
19 #include <string>
20 
21 namespace mrpt::nav
22 {
23 /** \addtogroup nav_planners Path planning
24  * \ingroup mrpt_nav_grp
25  * @{ */
26 
27 template <typename node_pose_t, typename world_limits_t>
29 {
30  node_pose_t start_pose, goal_pose;
31  /** Bounding box of the world, used to draw uniform random pose samples */
32  world_limits_t world_bbox_min, world_bbox_max;
33  /** World obstacles, as a point cloud */
35 };
36 
37 template <typename tree_t>
39 {
40  /** Whether the target was reached or not */
41  bool success{false};
42  /** Time spent (in secs) */
43  double computation_time{0};
44  /** Distance from best found path to goal */
45  double goal_distance;
46  /** Total cost of the best found path (cost ~~ Euclidean distance) */
47  double path_cost;
48  /** The ID of the best target node in the tree */
50  /** The set of target nodes within an acceptable distance to target
51  * (including `best_goal_node_id` and others) */
52  std::set<mrpt::graphs::TNodeID> acceptable_goal_node_ids;
53  /** The generated motion tree that explores free space starting at "start"
54  */
55  tree_t move_tree;
56 
58  : goal_distance(std::numeric_limits<double>::max()),
59  path_cost(std::numeric_limits<double>::max()),
61  {
62  }
63 };
64 
66 {
67  /** Maximum distance from a pose to target to accept it as a valid solution
68  * (meters). (Both acceptedDistToTarget & acceptedAngToTarget must be
69  * satisfied) */
70  double acceptedDistToTarget{0.1};
71  /** Maximum angle from a pose to target to accept it as a valid solution
72  * (rad). (Both acceptedDistToTarget & acceptedAngToTarget must be
73  * satisfied) */
75 
76  /** In seconds. 0 means no limit until a solution is found. */
77  double maxComputationTime{0.0};
78  /** In seconds. 0 means the first valid path will be returned. Otherwise,
79  * the algorithm will try to refine and find a better one. */
80  double minComputationTime{0.0};
81 
82  RRTEndCriteria() = default;
83 };
84 
86 {
87  /** The robot shape used when computing collisions; it's loaded from the
88  * config file/text as a single 2xN matrix in MATLAB format, first row are
89  * Xs, second are Ys, e.g:
90  * \code
91  * robot_shape = [-0.2 0.2 0.2 -0.2; -0.1 -0.1 0.1 0.1]
92  * \endcode
93  * \note PTGs will use only one of either `robot_shape` or
94  * `robot_shape_circular_radius`
95  */
97  /** The radius of a circular-shape-model of the robot shape.
98  * \note PTGs will use only one of either `robot_shape` or
99  * `robot_shape_circular_radius`
100  */
102 
103  /** (Default: ".") */
105 
106  /** Probabily of picking the goal as random target (in [0,1], default=0.05)
107  */
108  double goalBias{0.05};
109  /** (Very sensitive parameter!) Max length of each edge path (in meters,
110  * default=1.0) */
111  double maxLength{1.0};
112  /** Minimum distance [meters] to nearest node to accept creating a new one
113  * (default=0.10). (Any of minDistanceBetweenNewNodes and
114  * minAngBetweenNewNodes must be satisfied) */
116  /** Minimum angle [rad] to nearest node to accept creating a new one
117  * (default=15 deg) (Any of minDistanceBetweenNewNodes and
118  * minAngBetweenNewNodes must be satisfied) */
120  /** Display PTG construction info (default=true) */
121  bool ptg_verbose{true};
122 
123  /** Frequency (in iters) of saving tree state to debug log files viewable in
124  * SceneViewer3D (default=0, disabled) */
125  size_t save_3d_log_freq{0};
126 
128 };
129 
130 /** Virtual base class for TP-Space-based path planners */
132 {
133  public:
135  /** Parameters specific to this path solver algorithm */
137 
138  /** ctor */
140 
142  const mrpt::nav::TListPTGPtr& getPTGs() const { return m_PTGs; }
143  /** Options for renderMoveTree() */
145  {
146  /** Highlight the path from root towards this node (usually, the target)
147  */
149  /** (Default=1) Draw one out of N vehicle shapes along the highlighted
150  * path */
152 
157 
158  /** A scale factor to all XYZ corners (default=0, means auto determien
159  * from vehicle shape) */
160  double xyzcorners_scale{0};
161  /** (Default=false) */
163  /** (Default=10 meters) Set to 0 to disable */
165 
166  /** Robot color */
168  /** obstacles color */
170  /** local obstacles color */
172  /** START indication color */
174  /** END indication color */
180  float width_last_edge{3.f};
181  float width_normal_edge{1.f};
182  float width_optimal_edge{4.f};
185 
186  /** (Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps
187  * making it in the "first plane" */
188  double vehicle_shape_z{0.01};
189  /** Robot line width for visualization - default 2.0 */
190  double vehicle_line_width{2.0};
191  /** (Default=true) */
192  bool draw_obstacles{true};
193 
194  std::string log_msg;
196  double log_msg_scale{0.2};
197 
200  x_rand_pose(nullptr),
201  x_nearest_pose(nullptr),
203  new_state(nullptr),
204  color_vehicle(0xFF, 0x00, 0x00, 0xFF),
205  color_obstacles(0x00, 0x00, 0xFF, 0x40),
206  color_local_obstacles(0x00, 0x00, 0xFF),
207  color_start(0x00, 0x00, 0x00, 0x00),
208  color_goal(0x00, 0x00, 0x00, 0x00),
209  color_ground_xy_grid(0xFF, 0xFF, 0xFF),
210  color_normal_edge(0x22, 0x22, 0x22, 0x40),
211  color_last_edge(0xff, 0xff, 0x00),
212  color_optimal_edge(0x00, 0x00, 0x00),
213  log_msg_position(0, 0, 0)
214  {
215  }
216 
217  virtual ~TRenderPlannedPathOptions() = default;
218  };
219 
220  template <typename node_pose_t, typename world_limits_t, typename tree_t>
221  void renderMoveTree(
224  const TPlannerResultTempl<tree_t>& result,
225  const TRenderPlannedPathOptions& options);
226 
227  protected:
229  bool m_initialized_PTG{false};
231  mrpt::maps::CSimplePointsMap m_local_obs; // Temporary map. Defined as a
232  // member to save realloc time
233  // between calls
234 
235  /** Load all PTG params from a config file source */
237  const mrpt::config::CConfigFileBase& cfgSource,
238  const std::string& sSectionName = std::string("PTG_CONFIG"));
239 
240  /** Must be called after setting all params (see
241  * `internal_loadConfig_PTG()`) and before calling `solve()` */
243 
245  const mrpt::maps::CPointsMap& in_map, mrpt::maps::CPointsMap& out_map,
246  const mrpt::poses::CPose2D& asSeenFrom, const double MAX_DIST_XY);
247 
248  void spaceTransformer(
249  const mrpt::maps::CSimplePointsMap& in_obstacles,
251  const double MAX_DIST, std::vector<double>& out_TPObstacles);
252 
254  const int tp_space_k_direction,
255  const mrpt::maps::CSimplePointsMap& in_obstacles,
257  const double MAX_DIST, double& out_TPObstacle_k);
258 
259 }; // end class PlannerTPS_VirtualBase
260 /** @} */
261 } // namespace mrpt::nav
262 #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 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).
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:67
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...
constexpr double DEG2RAD(const double x)
Degrees to radians.
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). ...
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.
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:39
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:56
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
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:19
mrpt::system::CTimeLogger & getProfiler()
A RGB color - 8bit.
Definition: TColor.h:25
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)
std::string ptg_cache_files_directory
(Default: ".")
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020