Main MRPT website > C++ reference for MRPT 1.5.6
PlannerRRT_SE2_TPS.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, 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 #include "nav-precomp.h" // Precomp header
11 
14 #include <mrpt/utils/CTicTac.h>
15 #include <mrpt/random.h>
16 #include <mrpt/system/filesystem.h>
17 
18 using namespace mrpt::nav;
19 using namespace mrpt::utils;
20 using namespace mrpt::math;
21 using namespace mrpt::poses;
22 using namespace std;
23 
24 MRPT_TODO("Optimize getNearestNode() with KD-tree!")
25 
27  m_initialized(false)
28 {
29 }
30 
31 /** Load all params from a config file source */
33 {
35 }
36 
37 /** Must be called after setting all params (see `loadConfig()`) and before calling `solve()` */
39 {
41 
42  m_initialized = true;
43 }
44 
45 /** The main API entry point: tries to find a planned path from 'goal' to 'target' */
49 {
50  mrpt::utils::CTimeLoggerEntry tle(m_timelogger,"PT_RRT::solve");
51 
52  // Sanity checks:
53  ASSERTMSG_(m_initialized, "initialize() must be called before!");
54 
55  // Calc maximum vehicle shape radius:
56  double max_veh_radius=0.;
57  for (const auto & ptg : m_PTGs)
58  mrpt::utils::keep_max(max_veh_radius, ptg->getMaxRobotRadius());
59 
60  // [Algo `tp_space_rrt`: Line 1]: Init tree adding the initial pose
61  if (result.move_tree.getAllNodes().empty())
62  {
63  result.move_tree.root = 0;
64  result.move_tree.insertNode( result.move_tree.root, TNodeSE2_TP( pi.start_pose ) );
65  }
66 
67  mrpt::utils::CTicTac working_time;
68  working_time.Tic();
69  size_t rrt_iter_counter=0;
70 
71  size_t SAVE_3D_TREE_LOG_DECIMATION_CNT=0;
72  static size_t SAVE_LOG_SOLVE_COUNT=0;
73  SAVE_LOG_SOLVE_COUNT++;
74 
75  // Keep track of the best solution so far:
76  // By reusing the contents of "result" we make the algorithm re-callable ("any-time" algorithm) to refine results
77 
78  // [Algo `tp_space_rrt`: Line 2]: Iterate
79  // ------------------------------------------
80  for (;;)
81  {
82  // Check end conditions:
83  const double elap_tim = working_time.Tac();
84  if (
85  (end_criteria.maxComputationTime>0 && elap_tim>end_criteria.maxComputationTime) // Max comp time
86  || (result.goal_distance<end_criteria.acceptedDistToTarget && elap_tim>=end_criteria.minComputationTime) // Reach closer than this to target
87  )
88  {
89  break;
90  }
91 
92  // [Algo `tp_space_rrt`: Line 3]: sample random state (with goal biasing)
93  // -----------------------------------------
94  node_pose_t x_rand;
95  //bool rand_is_target=false;
96  if (mrpt::random::randomGenerator.drawUniform(0.0,1.0) < params.goalBias) {
97  x_rand = pi.goal_pose;
98  //rand_is_target=true;
99  }
100  else {
101  // Sample uniform:
102  for (int i=0;i<node_pose_t::static_size;i++)
103  x_rand[i] = mrpt::random::randomGenerator.drawUniform( pi.world_bbox_min[i], pi.world_bbox_max[i]);
104  }
105  const CPose2D x_rand_pose(x_rand);
106 
107  // [Algo `tp_space_rrt`: Line 4]: Init empty solution set
108  // -----------------------------------------
109  typedef std::map<double,TMoveEdgeSE2_TP> sorted_solution_list_t;
110  sorted_solution_list_t candidate_new_nodes; // Map: cost -> info. Pick begin() to select the lowest-cose one.
111 
112  const PoseDistanceMetric<TNodeSE2> distance_evaluator_se2; // Plain distances in SE(2), not along PTGs
113  bool is_new_best_solution = false; // Just for logging purposes
114 
115 //#define DO_LOG_TXTS
116  std::string sLogTxt;
117 
118  // [Algo `tp_space_rrt`: Line 5]: For each PTG
119  // -----------------------------------------
120  const size_t nPTGs = m_PTGs.size();
121  for (size_t idxPTG=0;idxPTG<nPTGs;++idxPTG)
122  {
123  rrt_iter_counter++;
124 
125  // [Algo `tp_space_rrt`: Line 5]: Search nearest neig. to x_rand
126  // -----------------------------------------------
127  const PoseDistanceMetric<TNodeSE2_TP> distance_evaluator(*m_PTGs[idxPTG]);
128 
129  const TNodeSE2_TP query_node(x_rand);
130 
131  m_timelogger.enter("TMoveTree::getNearestNode");
132  mrpt::utils::TNodeID x_nearest_id = result.move_tree.getNearestNode(query_node, distance_evaluator );
133  m_timelogger.leave("TMoveTree::getNearestNode");
134 
135  if (x_nearest_id==INVALID_NODEID)
136  {
137  // We can't find any close node, at least with this PTG's paths: skip
138 
139  // Before that, save log:
140  if (params.save_3d_log_freq>0 && (++SAVE_3D_TREE_LOG_DECIMATION_CNT >= params.save_3d_log_freq))
141  {
142  SAVE_3D_TREE_LOG_DECIMATION_CNT=0; // Reset decimation counter
143  TRenderPlannedPathOptions render_options;
144  render_options.highlight_path_to_node_id = result.best_goal_node_id;
145  render_options.highlight_last_added_edge = false;
146  render_options.x_rand_pose = &x_rand_pose;
147  render_options.log_msg = "SKIP: Can't find any close node";
149  render_options.ground_xy_grid_frequency = 1.0;
150 
152  renderMoveTree(scene, pi,result,render_options);
153  mrpt::system::createDirectory("./rrt_log_trees");
154  scene.saveToFile( mrpt::format("./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),static_cast<unsigned int>(rrt_iter_counter) ) );
155  }
156 
157  continue; // Skip
158  }
159 
160  const TNodeSE2_TP & x_nearest_node = result.move_tree.getAllNodes().find(x_nearest_id)->second;
161 
162  // [Algo `tp_space_rrt`: Line 6]: Relative target
163  // -----------------------------------------------
164  const CPose2D x_nearest_pose( x_nearest_node.state );
165  const CPose2D x_rand_rel = x_rand_pose - x_nearest_pose;
166 
167  // [Algo `tp_space_rrt`: Line 7]: Relative target in TP-Space
168  // ------------------------------------------------------------
169  const double D_max = std::min(params.maxLength, m_PTGs[idxPTG]->getRefDistance() );
170 
171  double d_rand; // Coordinates in TP-space
172  int k_rand; // k_rand is the index of target_alpha in PTGs corresponding to a specific d_rand
173  //bool tp_point_is_exact =
174  m_PTGs[idxPTG]->inverseMap_WS2TP(
175  x_rand_rel.x(), x_rand_rel.y(),
176  k_rand, d_rand );
177  d_rand *= m_PTGs[idxPTG]->getRefDistance(); // distance to target, in "real meters"
178 
179  float d_free;
180  //bool local_obs_ok = false; // Just for 3D log files: indicates whether obstacle points have been recomputed
181 
182  // [Algo `tp_space_rrt`: Line 8]: TP-Obstacles
183  // ------------------------------------------------------------
184  // Transform obstacles as seen from x_nearest_node -> TP_obstacles
185  double TP_Obstacles_k_rand = .0; //vector<double> TP_Obstacles;
186  const double MAX_DIST_FOR_OBSTACLES = 1.5*m_PTGs[idxPTG]->getRefDistance(); // Maximum Euclidean distance (radius) for considering obstacles around the current robot pose
187 
188  ASSERT_ABOVE_(m_PTGs[idxPTG]->getRefDistance(),1.1*max_veh_radius); // Make sure the PTG covers at least a bit more than the vehicle shape!! (should be much, much higher)
189 
190  {
191  CTimeLoggerEntry tle(m_timelogger,"PT_RRT::solve.changeCoordinatesReference");
192  transformPointcloudWithSquareClipping(pi.obstacles_points,m_local_obs,CPose2D(x_nearest_node.state),MAX_DIST_FOR_OBSTACLES);
193  //local_obs_ok=true;
194  }
195  {
196  CTimeLoggerEntry tle(m_timelogger,"PT_RRT::solve.SpaceTransformer");
197  spaceTransformerOneDirectionOnly(k_rand, m_local_obs, m_PTGs[idxPTG].pointer(), MAX_DIST_FOR_OBSTACLES, TP_Obstacles_k_rand);
198  }
199 
200  // directions k_rand in TP_obstacles[k_rand] = d_free
201  // this is the collision free distance to the TP_target
202  d_free = TP_Obstacles_k_rand; // TP_Obstacles[k_rand];
203 
204  // [Algo `tp_space_rrt`: Line 10]: d_new
205  // ------------------------------------------------------------
206  double d_new = std::min(D_max, d_rand); //distance of the new candidate state in TP-space
207 
208  //mrpt::poses::CPose2D *log_new_state_ptr=NULL; // For graphical logs only
209 
210 #ifdef DO_LOG_TXTS
211  sLogTxt += mrpt::format("tp_idx=%u tp_exact=%c\n d_free: %f d_rand=%f d_new=%f\n",static_cast<unsigned int>(idxPTG), tp_point_is_exact ? 'Y':'N',d_free,d_rand,d_new);
212  sLogTxt += mrpt::format(" nearest:%s\n",x_nearest_pose.asString().c_str());
213 #endif
214 
215  // [Algo `tp_space_rrt`: Line 13]: Do we have free space?
216  // ------------------------------------------------------------
217  if ( d_free>=d_new )
218  {
219  // [Algo `tp_space_rrt`: Line 14]: PTG function
220  // ------------------------------------------------------------
221  //given d_rand and k_rand provides x,y,phi of the point in c-space
222  uint32_t nStep;
223  m_PTGs[idxPTG]->getPathStepForDist(k_rand, d_new, nStep);
224 
225  mrpt::math::TPose2D rel_pose;
226  m_PTGs[idxPTG]->getPathPose(k_rand, nStep, rel_pose);
227 
228  mrpt::math::wrapToPiInPlace(rel_pose.phi); // wrap to [-pi,pi] -->avoid out of bounds errors
229 
230  // [Algo `tp_space_rrt`: Line 15]: pose composition
231  // ------------------------------------------------------------
232  const mrpt::poses::CPose2D new_state_rel(rel_pose);
233  mrpt::poses::CPose2D new_state = x_nearest_pose+new_state_rel; //compose the new_motion as the last nmotion and the new state
234  //log_new_state_ptr = &new_state;
235 
236  // Check whether there's already a too-close node around:
237  // --------------------------------------------------------
238  bool accept_this_node = true;
239 
240 
241  // Is this a potential solution
242  const double goal_dist = new_state.distance2DTo(pi.goal_pose.x,pi.goal_pose.y);
243  const double goal_ang = std::abs( mrpt::math::angDistance(new_state.phi(), pi.goal_pose.phi ) );
244  const bool is_acceptable_goal =
245  (goal_dist<end_criteria.acceptedDistToTarget) &&
246  (goal_ang <end_criteria.acceptedAngToTarget);
247 
248  mrpt::utils::TNodeID new_nearest_id=INVALID_NODEID;
249  if (!is_acceptable_goal) // Only check for nearby nodes if this is not a solution!
250  {
251  double new_nearest_dist;
252  const TNodeSE2 new_state_node(new_state);
253 
254  m_timelogger.enter("TMoveTree::getNearestNode");
255  new_nearest_id = result.move_tree.getNearestNode(new_state_node, distance_evaluator_se2,&new_nearest_dist, &result.acceptable_goal_node_ids );
256  m_timelogger.leave("TMoveTree::getNearestNode");
257 
258  if (new_nearest_id!=INVALID_NODEID)
259  {
260  // Also check angular distance:
261  const double new_nearest_ang = std::abs( mrpt::math::angDistance(new_state.phi(), result.move_tree.getAllNodes().find(new_nearest_id)->second.state.phi ) );
262  accept_this_node = (new_nearest_dist>=params.minDistanceBetweenNewNodes || new_nearest_ang >= params.minAngBetweenNewNodes);
263  }
264  }
265 
266  if (!accept_this_node)
267  {
268 #ifdef DO_LOG_TXTS
269  if (new_nearest_id!=INVALID_NODEID) {
270  sLogTxt += mrpt::format(" -> new node NOT accepted for closeness to: %s\n",result.move_tree.getAllNodes().find(new_nearest_id)->second.state.asString().c_str());
271  }
272 #endif
273  continue; // Too close node, skip!
274  }
275 
276  // [Algo `tp_space_rrt`: Line 16]: Add to candidate solution set
277  // ------------------------------------------------------------
278  // Create "movement" (tree edge) object:
279  TMoveEdgeSE2_TP new_edge(x_nearest_id, mrpt::math::TPose2D(new_state));
280 
281  new_edge.cost = d_new;
282  new_edge.ptg_index= idxPTG;
283  new_edge.ptg_K = k_rand;
284  new_edge.ptg_dist = d_new;
285 
286  candidate_new_nodes[new_edge.cost] = new_edge;
287 
288  } // end if the path is obstacle free
289  else
290  {
291 #ifdef DO_LOG_TXTS
292  sLogTxt += mrpt::format(" -> d_free NOT < d_rand\n");
293 #endif
294  }
295 
296 
297  } // end for idxPTG
298 
299  // [Algo `tp_space_rrt`: Line 19]: Any solution found?
300  // ------------------------------------------------------------
301  if (!candidate_new_nodes.empty())
302  {
303  const TMoveEdgeSE2_TP & best_edge = candidate_new_nodes.begin()->second;
304  const TNodeSE2_TP new_state_node(best_edge.end_state);
305 
306  // Insert into the tree:
307  const mrpt::utils::TNodeID new_child_id = result.move_tree.getNextFreeNodeID();
308  result.move_tree.insertNodeAndEdge(best_edge.parent_id, new_child_id, new_state_node, best_edge);
309 
310  // Distance to goal:
311  const double goal_dist = mrpt::poses::CPose2D(best_edge.end_state).distance2DTo(pi.goal_pose.x,pi.goal_pose.y);
312  const double goal_ang = std::abs( mrpt::math::angDistance(best_edge.end_state.phi, pi.goal_pose.phi ) );
313 
314  const bool is_acceptable_goal =
315  (goal_dist<end_criteria.acceptedDistToTarget) &&
316  (goal_ang <end_criteria.acceptedAngToTarget);
317 
318  if (is_acceptable_goal)
319  result.acceptable_goal_node_ids.insert(new_child_id);
320 
321  // Total path length:
322  double this_path_cost = std::numeric_limits<double>::max();
323  if (is_acceptable_goal) // Don't waste time computing path length if it doesn't matter anyway
324  {
325  TMoveTreeSE2_TP::path_t candidate_solution_path;
326  result.move_tree.backtrackPath(new_child_id,candidate_solution_path);
327  this_path_cost=0;
328  for (TMoveTreeSE2_TP::path_t::const_iterator it=candidate_solution_path.begin();it!=candidate_solution_path.end();++it)
329  if (it->edge_to_parent)
330  this_path_cost+=it->edge_to_parent->cost;
331  }
332 
333  // Check if this should be the new optimal path:
334  if (is_acceptable_goal && this_path_cost<result.path_cost)
335  {
336  result.goal_distance = goal_dist;
337  result.path_cost = this_path_cost;
338 
339  result.best_goal_node_id = new_child_id;
340  is_new_best_solution=true;
341  }
342  } // end if any candidate found
343 
344  // Graphical logging, if enabled:
345  // ------------------------------------------------------
346  if (params.save_3d_log_freq>0 && (++SAVE_3D_TREE_LOG_DECIMATION_CNT >= params.save_3d_log_freq || is_new_best_solution))
347  {
348  CTimeLoggerEntry tle(m_timelogger,"PT_RRT::solve.generate_log_files");
349  SAVE_3D_TREE_LOG_DECIMATION_CNT=0; // Reset decimation counter
350 
351  // Render & save to file:
352  TRenderPlannedPathOptions render_options;
353  render_options.highlight_path_to_node_id = result.best_goal_node_id;
354  render_options.x_rand_pose = &x_rand_pose;
355  //render_options.x_nearest_pose = &x_nearest_pose;
356  //if (local_obs_ok) render_options.local_obs_from_nearest_pose = &m_local_obs;
357  //render_options.new_state = log_new_state_ptr;
358  render_options.highlight_last_added_edge = true;
359  render_options.ground_xy_grid_frequency = 1.0;
360 
361  render_options.log_msg = sLogTxt;
363 
365  renderMoveTree(scene, pi,result,render_options);
366 
367  mrpt::system::createDirectory("./rrt_log_trees");
368  scene.saveToFile( mrpt::format("./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),static_cast<unsigned int>(rrt_iter_counter) ) );
369  }
370 
371 
372  } // end loop until end conditions
373 
374 
375  // [Algo `tp_space_rrt`: Line 17]: Tree back trace
376  // ------------------------------------------------------------
377  result.success = (result.goal_distance<end_criteria.acceptedDistToTarget);
378  result.computation_time = working_time.Tac();
379 
380 } // end solve()
381 
382 
383 
mrpt::utils::TNodeID parent_id
The ID of the parent node in the tree.
Definition: TMoveTree.h:166
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
#define min(a, b)
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition: TMoveTree.h:211
#define ASSERT_ABOVE_(__A, __B)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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.
Definition: TMoveTree.h:218
world_limits_t world_bbox_max
Bounding box of the world, used to draw uniform random pose samples.
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
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.
Definition: wrap2pi.h:91
#define INVALID_NODEID
std::list< node_t > path_t
A topological path up-tree.
Definition: TMoveTree.h:72
const Scalar * const_iterator
Definition: eigen_plugins.h:24
int ptg_index
indicate the type of trajectory used for this motion
Definition: TMoveTree.h:169
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
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
Definition: TMoveTree.h:170
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...
Definition: TMoveTree.h:168
This class allows loading and storing values and vectors of different types from a configuration text...
MRPT_TODO("Modify ping to run on Windows + Test this")
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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.
Definition: CTicTac.h:24
const node_map_t & getAllNodes() const
Definition: TMoveTree.h:126
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)
Definition: TMoveTree.h:119
TNodeID root
The root of the tree.
Definition: CDirectedTree.h:66
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::utils::TNodeID best_goal_node_id
The ID of the best target node in the tree.
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.
Definition: TMoveTree.h:76
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)
Definition: TMoveTree.h:105
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve() ...
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...
Definition: TMoveTree.h:131
void solve(const TPlannerInput &pi, TPlannerResult &result)
The main API entry point: tries to find a planned path from 'goal' to 'target'.
mrpt::maps::CSimplePointsMap obstacles_points
World obstacles, as a point cloud.
double goal_distance
Distance from best found path to goal.
double computation_time
Time spent (in secs)
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
Definition: CTimeLogger.h:127
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
Lightweight 2D pose.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
GLsizei const GLvoid * pointer
Definition: glew.h:1428
GLfloat * params
Definition: glew.h:1436
double ptg_dist
identify the lenght of the trajectory for this motion
Definition: TMoveTree.h:171
An edge for the move tree used for planning in SE2 and TP-space.
Definition: TMoveTree.h:164
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
Definition: TMoveTree.h:167
Lightweight 3D point.
unsigned __int32 uint32_t
Definition: rptypes.h:49
#define ASSERTMSG_(f, __ERROR_MSG)
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...
double distance2DTo(double ax, double ay) const
Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists).
Definition: CPoseOrPoint.h:165
mrpt::utils::TNodeID getNextFreeNodeID() const
Definition: TMoveTree.h:124
tree_t move_tree
The generated motion tree that explores free space starting at "start".



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018