Go to the documentation of this file.
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)
89 elap_tim >= end_criteria.minComputationTime)
114 const CPose2D x_rand_pose(x_rand);
118 using sorted_solution_list_t = std::map<double, TMoveEdgeSE2_TP>;
119 sorted_solution_list_t candidate_new_nodes;
124 distance_evaluator_se2;
125 bool is_new_best_solution =
false;
132 const size_t nPTGs = m_PTGs.size();
133 for (
size_t idxPTG = 0; idxPTG < nPTGs; ++idxPTG)
144 m_timelogger.enter(
"TMoveTree::getNearestNode");
147 m_timelogger.leave(
"TMoveTree::getNearestNode");
155 if (
params.save_3d_log_freq > 0 &&
156 (++SAVE_3D_TREE_LOG_DECIMATION_CNT >=
159 SAVE_3D_TREE_LOG_DECIMATION_CNT =
166 render_options.
log_msg =
"SKIP: Can't find any close node";
172 renderMoveTree(scene, pi, result, render_options);
175 "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
176 static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
177 static_cast<unsigned int>(rrt_iter_counter)));
189 const CPose2D x_rand_rel = x_rand_pose - x_nearest_pose;
200 m_PTGs[idxPTG]->inverseMap_WS2TP(
201 x_rand_rel.
x(), x_rand_rel.
y(), k_rand, d_rand);
213 double TP_Obstacles_k_rand = .0;
214 const double MAX_DIST_FOR_OBSTACLES =
215 1.5 * m_PTGs[idxPTG]->getRefDistance();
222 m_PTGs[idxPTG]->getRefDistance(),
223 1.1 * max_veh_radius);
229 m_timelogger,
"PT_RRT::solve.changeCoordinatesReference");
230 transformPointcloudWithSquareClipping(
237 m_timelogger,
"PT_RRT::solve.SpaceTransformer");
238 spaceTransformerOneDirectionOnly(
239 k_rand, m_local_obs, m_PTGs[idxPTG].get(),
240 MAX_DIST_FOR_OBSTACLES, TP_Obstacles_k_rand);
245 d_free = TP_Obstacles_k_rand;
258 "tp_idx=%u tp_exact=%c\n d_free: %f d_rand=%f d_new=%f\n",
259 static_cast<unsigned int>(idxPTG),
260 tp_point_is_exact ?
'Y' :
'N', d_free, d_rand, d_new);
262 " nearest:%s\n", x_nearest_pose.asString().c_str());
274 m_PTGs[idxPTG]->getPathStepForDist(k_rand, d_new, nStep);
277 m_PTGs[idxPTG]->getPathPose(k_rand, nStep, rel_pose);
287 x_nearest_pose + new_state_rel;
294 bool accept_this_node =
true;
297 const double goal_dist =
299 const double goal_ang = std::abs(
301 const bool is_acceptable_goal =
302 (goal_dist < end_criteria.acceptedDistToTarget) &&
303 (goal_ang < end_criteria.acceptedAngToTarget);
306 if (!is_acceptable_goal)
309 double new_nearest_dist;
312 m_timelogger.enter(
"TMoveTree::getNearestNode");
314 new_state_node, distance_evaluator_se2,
316 m_timelogger.leave(
"TMoveTree::getNearestNode");
321 const double new_nearest_ang =
324 .find(new_nearest_id)
325 ->second.state.phi));
328 params.minDistanceBetweenNewNodes ||
329 new_nearest_ang >=
params.minAngBetweenNewNodes);
333 if (!accept_this_node)
339 " -> new node NOT accepted for closeness to: %s\n",
341 .find(new_nearest_id)
342 ->second.state.asString()
354 new_edge.
cost = d_new;
356 new_edge.
ptg_K = k_rand;
359 candidate_new_nodes[new_edge.
cost] = new_edge;
373 if (!candidate_new_nodes.empty())
376 candidate_new_nodes.begin()->second;
383 best_edge.
parent_id, new_child_id, new_state_node, best_edge);
386 const double goal_dist =
392 const bool is_acceptable_goal =
393 (goal_dist < end_criteria.acceptedDistToTarget) &&
394 (goal_ang < end_criteria.acceptedAngToTarget);
396 if (is_acceptable_goal)
400 double this_path_cost = std::numeric_limits<double>::max();
401 if (is_acceptable_goal)
406 new_child_id, candidate_solution_path);
409 candidate_solution_path.begin();
410 it != candidate_solution_path.end(); ++it)
411 if (it->edge_to_parent)
412 this_path_cost += it->edge_to_parent->cost;
416 if (is_acceptable_goal && this_path_cost < result.
path_cost)
422 is_new_best_solution =
true;
428 if (
params.save_3d_log_freq > 0 &&
429 (++SAVE_3D_TREE_LOG_DECIMATION_CNT >=
params.save_3d_log_freq ||
430 is_new_best_solution))
433 m_timelogger,
"PT_RRT::solve.generate_log_files");
434 SAVE_3D_TREE_LOG_DECIMATION_CNT = 0;
447 render_options.
log_msg = sLogTxt;
452 renderMoveTree(scene, pi, result, render_options);
456 "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
457 static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
458 static_cast<unsigned int>(rrt_iter_counter)));
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
#define ASSERT_ABOVE_(__A, __B)
mrpt::math::TPose2D asTPose() const
void insertNodeAndEdge(const mrpt::graphs::TNodeID parent_id, const mrpt::graphs::TNodeID new_child_id, const NODE_TYPE_DATA &new_child_node_data, const EDGE_TYPE &new_edge_data)
const Scalar * const_iterator
mrpt::graphs::TNodeID best_goal_node_id
The ID of the best target node in the tree.
A high-performance stopwatch, with typical resolution of nanoseconds.
double phi
Orientation (rads)
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
void backtrackPath(const mrpt::graphs::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...
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
mrpt::math::TPoint3D log_msg_position
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.
mrpt::graphs::TNodeID getNextFreeNodeID() const
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
double goal_distance
Distance from best found path to goal.
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...
double Tac() noexcept
Stops the stopwatch.
int ptg_K
identify the trajectory number K of the type ptg_index
double cost
cost associated to each motion, this should be defined by the user according to a spefic cost functio...
void initialize()
Must be called after setting all params (see loadConfig()) and before calling solve()
Pose metric for SE(2) limited to a given PTG manifold.
bool highlight_last_added_edge
(Default=false)
const mrpt::poses::CPose2D * x_rand_pose
mrpt::graphs::TNodeID getNearestNode(const NODE_TYPE_FOR_METRIC &query_pt, const PoseDistanceMetric< NODE_TYPE_FOR_METRIC > &distanceMetricEvaluator, double *out_distance=NULL, const std::set< mrpt::graphs::TNodeID > *ignored_nodes=NULL) const
Finds the nearest node to a given pose, using the given metric.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
This class allows loading and storing values and vectors of different types from a configuration text...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
double x() const
Common members of all points & poses classes.
double path_cost
Total cost of the best found path (cost ~~ Euclidean distance)
TP Space-based RRT path planning for SE(2) (planar) robots.
std::list< node_t > path_t
A topological path up-tree.
void Tic() noexcept
Starts the stopwatch.
Options for renderMoveTree()
bool success
Whether the target was reached or not.
mrpt::graphs::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve()
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
void insertNode(const mrpt::graphs::TNodeID node_id, const NODE_TYPE_DATA &node_data)
Insert a node without edges (should be used only for a tree root node)
tree_t move_tree
The generated motion tree that explores free space starting at "start".
TNodeID root
The root of the tree.
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double distance2DTo(double ax, double ay) const
Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists).
This base provides a set of functions for maths stuff.
const node_map_t & getAllNodes() const
double computation_time
Time spent (in secs)
void solve(const TPlannerInput &pi, TPlannerResult &result)
The main API entry point: tries to find a planned path from 'goal' to 'target'.
GLsizei const GLchar ** string
An edge for the move tree used for planning in SE2 and TP-space.
mrpt::graphs::TNodeID parent_id
The ID of the parent node in the tree.
void loadConfig(const mrpt::config::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all params from a config file source.
bool createDirectory(const std::string &dirName)
Creates a directory.
int ptg_index
indicate the type of trajectory used for this motion
double ptg_dist
identify the lenght of the trajectory for this motion
unsigned __int32 uint32_t
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
GLenum const GLfloat * params
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |