24 MRPT_TODO(
"Optimize getNearestNode() with KD-tree!")
52 ASSERTMSG_(m_initialized,
"initialize() must be called before!");
55 double max_veh_radius = 0.;
56 for (
const auto& ptg : m_PTGs)
69 size_t rrt_iter_counter = 0;
71 size_t SAVE_3D_TREE_LOG_DECIMATION_CNT = 0;
72 static size_t SAVE_LOG_SOLVE_COUNT = 0;
73 SAVE_LOG_SOLVE_COUNT++;
84 const double elap_tim = working_time.
Tac();
85 if ((end_criteria.maxComputationTime > 0 &&
86 elap_tim > end_criteria.maxComputationTime)
115 const CPose2D x_rand_pose(x_rand);
119 typedef std::map<double, TMoveEdgeSE2_TP> sorted_solution_list_t;
120 sorted_solution_list_t candidate_new_nodes;
125 distance_evaluator_se2;
126 bool is_new_best_solution =
false;
133 const size_t nPTGs = m_PTGs.size();
134 for (
size_t idxPTG = 0; idxPTG < nPTGs; ++idxPTG)
145 m_timelogger.enter(
"TMoveTree::getNearestNode");
148 m_timelogger.leave(
"TMoveTree::getNearestNode");
156 if (
params.save_3d_log_freq > 0 &&
157 (++SAVE_3D_TREE_LOG_DECIMATION_CNT >=
160 SAVE_3D_TREE_LOG_DECIMATION_CNT =
167 render_options.
log_msg =
"SKIP: Can't find any close node";
173 renderMoveTree(scene, pi, result, render_options);
177 "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
178 static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
179 static_cast<unsigned int>(rrt_iter_counter)));
191 const CPose2D x_rand_rel = x_rand_pose - x_nearest_pose;
202 m_PTGs[idxPTG]->inverseMap_WS2TP(
203 x_rand_rel.
x(), x_rand_rel.
y(), k_rand, d_rand);
215 double TP_Obstacles_k_rand = .0;
216 const double MAX_DIST_FOR_OBSTACLES =
217 1.5 * m_PTGs[idxPTG]->getRefDistance();
224 m_PTGs[idxPTG]->getRefDistance(),
225 1.1 * max_veh_radius);
231 m_timelogger,
"PT_RRT::solve.changeCoordinatesReference");
232 transformPointcloudWithSquareClipping(
239 m_timelogger,
"PT_RRT::solve.SpaceTransformer");
240 spaceTransformerOneDirectionOnly(
241 k_rand, m_local_obs, m_PTGs[idxPTG].
get(),
242 MAX_DIST_FOR_OBSTACLES, TP_Obstacles_k_rand);
247 d_free = TP_Obstacles_k_rand;
259 "tp_idx=%u tp_exact=%c\n d_free: %f d_rand=%f d_new=%f\n",
260 static_cast<unsigned int>(idxPTG),
261 tp_point_is_exact ?
'Y' :
'N', d_free, d_rand, d_new);
263 " nearest:%s\n", x_nearest_pose.asString().c_str());
275 m_PTGs[idxPTG]->getPathStepForDist(k_rand, d_new, nStep);
278 m_PTGs[idxPTG]->getPathPose(k_rand, nStep, rel_pose);
288 x_nearest_pose + new_state_rel;
295 bool accept_this_node =
true;
298 const double goal_dist =
300 const double goal_ang = std::abs(
302 const bool is_acceptable_goal =
303 (goal_dist < end_criteria.acceptedDistToTarget) &&
304 (goal_ang < end_criteria.acceptedAngToTarget);
307 if (!is_acceptable_goal)
310 double new_nearest_dist;
311 const TNodeSE2 new_state_node(new_state);
313 m_timelogger.enter(
"TMoveTree::getNearestNode");
315 new_state_node, distance_evaluator_se2,
317 m_timelogger.leave(
"TMoveTree::getNearestNode");
322 const double new_nearest_ang = std::abs(
325 .find(new_nearest_id)
326 ->second.state.phi));
329 params.minDistanceBetweenNewNodes ||
330 new_nearest_ang >=
params.minAngBetweenNewNodes);
334 if (!accept_this_node)
340 " -> new node NOT accepted for closeness to: %s\n",
342 .find(new_nearest_id)
343 ->second.state.asString()
356 new_edge.
cost = d_new;
358 new_edge.
ptg_K = k_rand;
361 candidate_new_nodes[new_edge.
cost] = new_edge;
375 if (!candidate_new_nodes.empty())
378 candidate_new_nodes.begin()->second;
385 best_edge.
parent_id, new_child_id, new_state_node, best_edge);
388 const double goal_dist =
391 const double goal_ang = std::abs(
395 const bool is_acceptable_goal =
396 (goal_dist < end_criteria.acceptedDistToTarget) &&
397 (goal_ang < end_criteria.acceptedAngToTarget);
399 if (is_acceptable_goal)
403 double this_path_cost = std::numeric_limits<double>::max();
404 if (is_acceptable_goal)
409 new_child_id, candidate_solution_path);
412 candidate_solution_path.begin();
413 it != candidate_solution_path.end(); ++it)
414 if (it->edge_to_parent)
415 this_path_cost += it->edge_to_parent->cost;
419 if (is_acceptable_goal && this_path_cost < result.
path_cost)
425 is_new_best_solution =
true;
431 if (
params.save_3d_log_freq > 0 &&
432 (++SAVE_3D_TREE_LOG_DECIMATION_CNT >=
params.save_3d_log_freq ||
433 is_new_best_solution))
436 m_timelogger,
"PT_RRT::solve.generate_log_files");
437 SAVE_3D_TREE_LOG_DECIMATION_CNT = 0;
450 render_options.
log_msg = sLogTxt;
455 renderMoveTree(scene, pi, result, render_options);
460 "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
461 static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
462 static_cast<unsigned int>(rrt_iter_counter)));
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
mrpt::utils::TNodeID parent_id
The ID of the parent node in the tree.
double x() const
Common members of all points & poses classes.
bool createDirectory(const std::string &dirName)
Creates a directory.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define ASSERT_ABOVE_(__A, __B)
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...
void internal_loadConfig_PTG(const mrpt::utils::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all PTG params from a config file source.
Pose metric for SE(2) limited to a given PTG manifold.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
std::list< node_t > path_t
A topological path up-tree.
const mrpt::poses::CPose2D * x_rand_pose
const Scalar * const_iterator
const node_map_t & getAllNodes() const
int ptg_index
indicate the type of trajectory used for this motion
mrpt::math::TPoint3D log_msg_position
void Tic()
Starts the stopwatch.
mrpt::utils::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
int ptg_K
identify the trajectory number K of the type ptg_index
void initialize()
Must be called after setting all params (see loadConfig()) and before calling solve() ...
double cost
cost associated to each motion, this should be defined by the user according to a spefic cost functio...
This class allows loading and storing values and vectors of different types from a configuration text...
bool highlight_last_added_edge
(Default=false)
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
This base provides a set of functions for maths stuff.
uint64_t TNodeID
The type for node IDs in graphs of different types.
void loadConfig(const mrpt::utils::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all params from a config file source.
bool success
Whether the target was reached or not.
This class implements a high-performance stopwatch.
Options for renderMoveTree()
void insertNode(const mrpt::utils::TNodeID node_id, const NODE_TYPE_DATA &node_data)
Insert a node without edges (should be used only for a tree root node)
mrpt::utils::TNodeID getNearestNode(const NODE_TYPE_FOR_METRIC &query_pt, const PoseDistanceMetric< NODE_TYPE_FOR_METRIC > &distanceMetricEvaluator, double *out_distance=NULL, const std::set< mrpt::utils::TNodeID > *ignored_nodes=NULL) const
Finds the nearest node to a given pose, using the given metric.
TNodeID root
The root of the tree.
double distance2DTo(double ax, double ay) const
Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists).
GLsizei const GLchar ** string
mrpt::utils::TNodeID best_goal_node_id
The ID of the best target node in the tree.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
double path_cost
Total cost of the best found path (cost ~~ Euclidean distance)
void insertNodeAndEdge(const mrpt::utils::TNodeID parent_id, const mrpt::utils::TNodeID new_child_id, const NODE_TYPE_DATA &new_child_node_data, const EDGE_TYPE &new_edge_data)
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve() ...
void solve(const TPlannerInput &pi, TPlannerResult &result)
The main API entry point: tries to find a planned path from 'goal' to 'target'.
void backtrackPath(const mrpt::utils::TNodeID target_node, path_t &out_path) const
Builds the path (sequence of nodes, with info about next edge) up-tree from a target_node towards the...
double goal_distance
Distance from best found path to goal.
double computation_time
Time spent (in secs)
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
mrpt::utils::TNodeID getNextFreeNodeID() const
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
double Tac()
Stops the stopwatch.
double ptg_dist
identify the lenght of the trajectory for this motion
An edge for the move tree used for planning in SE2 and TP-space.
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
unsigned __int32 uint32_t
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
#define ASSERTMSG_(f, __ERROR_MSG)
GLenum const GLfloat * params
TP Space-based RRT path planning for SE(2) (planar) robots.
double phi
Orientation (rads)
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
tree_t move_tree
The generated motion tree that explores free space starting at "start".