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)
110 for (
int i = 0; i < node_pose_t::static_size; i++)
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;
194 std::min(
params.maxLength, m_PTGs[idxPTG]->getRefDistance());
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;
249 double d_new = std::min(
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);
408 for (
auto it = candidate_solution_path.begin();
409 it != candidate_solution_path.end(); ++it)
410 if (it->edge_to_parent)
411 this_path_cost += it->edge_to_parent->cost;
415 if (is_acceptable_goal && this_path_cost < result.
path_cost)
421 is_new_best_solution =
true;
427 if (
params.save_3d_log_freq > 0 &&
428 (++SAVE_3D_TREE_LOG_DECIMATION_CNT >=
params.save_3d_log_freq ||
429 is_new_best_solution))
432 m_timelogger,
"PT_RRT::solve.generate_log_files");
433 SAVE_3D_TREE_LOG_DECIMATION_CNT = 0;
446 render_options.
log_msg = sLogTxt;
451 renderMoveTree(scene, pi, result, render_options);
455 "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
456 static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
457 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::graphs::TNodeID getNearestNode(const NODE_TYPE_FOR_METRIC &query_pt, const PoseDistanceMetric< NODE_TYPE_FOR_METRIC > &distanceMetricEvaluator, double *out_distance=nullptr, const std::set< mrpt::graphs::TNodeID > *ignored_nodes=nullptr) const
Finds the nearest node to a given pose, using the given metric.
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)
TPoint3D_< double > TPoint3D
Lightweight 3D point.
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)
double phi() const
Get the phi angle of the 2D pose (in radians)
double x() const
Common members of all points & poses classes.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
MRPT_TODO("toPointCloud / calibration")
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
double distance2DTo(double ax, double ay) const
Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists).
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
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.
mrpt::config::CConfigFileBase CConfigFileBase
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.
void Tic() noexcept
Starts the stopwatch.
Options for renderMoveTree()
bool success
Whether the target was reached or not.
mrpt::vision::TStereoCalibParams params
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.
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'.
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.
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
std::string std::string format(std::string_view fmt, ARGS &&... args)
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |