Main MRPT website > C++ reference for MRPT 1.9.9
impl_renderMoveTree.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, 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 #pragma once
11 
12 // For 3D log files
18 #include <mrpt/opengl/CText3D.h>
19 
20 namespace mrpt
21 {
22 namespace nav
23 {
24 template <typename node_pose_t, typename world_limits_t, typename tree_t>
28  const TPlannerResultTempl<tree_t>& result,
29  const TRenderPlannedPathOptions& options)
30 {
31  using std::string;
32 
33  // Build a model of the vehicle shape:
34  mrpt::opengl::CSetOfLines::Ptr gl_veh_shape =
35  mrpt::make_aligned_shared<mrpt::opengl::CSetOfLines>();
36  double
37  xyzcorners_scale; // Size of XYZ corners (scaled to vehicle dimensions)
38  {
39  gl_veh_shape->setLineWidth(options.vehicle_line_width);
40  gl_veh_shape->setColor_u8(options.color_vehicle);
41 
42  double max_veh_radius = 0.;
43  if (!params.robot_shape.empty())
44  {
45  gl_veh_shape->appendLine(
46  params.robot_shape[0].x, params.robot_shape[0].y, 0,
47  params.robot_shape[1].x, params.robot_shape[1].y, 0);
48  for (size_t i = 2; i <= params.robot_shape.size(); i++)
49  {
50  const size_t idx = i % params.robot_shape.size();
51  mrpt::keep_max(max_veh_radius, params.robot_shape[idx].norm());
52  gl_veh_shape->appendLineStrip(
53  params.robot_shape[idx].x, params.robot_shape[idx].y, 0);
54  }
55  }
56  else if (params.robot_shape_circular_radius > 0)
57  {
58  const int NUM_VERTICES = 10;
59  const double R = params.robot_shape_circular_radius;
60  for (int i = 0; i <= NUM_VERTICES; i++)
61  {
62  const size_t idx = i % NUM_VERTICES;
63  const size_t idxn = (i + 1) % NUM_VERTICES;
64  const double ang = idx * 2 * M_PI / (NUM_VERTICES - 1);
65  const double angn = idxn * 2 * M_PI / (NUM_VERTICES - 1);
66  gl_veh_shape->appendLine(
67  R * cos(ang), R * sin(ang), 0, R * cos(angn), R * sin(angn),
68  0);
69  }
70  mrpt::keep_max(max_veh_radius, R);
71  }
72 
73  xyzcorners_scale = max_veh_radius * 0.5;
74  }
75  // Override with user scale?
76  if (options.xyzcorners_scale != 0)
77  xyzcorners_scale = options.xyzcorners_scale;
78 
79  // "ground"
80  if (options.ground_xy_grid_frequency > 0)
81  {
83  mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>(
85  pi.world_bbox_max.y, 0, options.ground_xy_grid_frequency);
86  obj->setColor_u8(options.color_ground_xy_grid);
87  scene.insert(obj);
88  }
89 
90  // Original randomly-pick pose:
91  if (options.x_rand_pose)
92  {
94  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.0);
95  string m_name = "X_rand";
96  obj->setName(m_name);
97  obj->enableShowName();
98  obj->setPose(mrpt::poses::CPose3D(*options.x_rand_pose));
99  scene.insert(obj);
100  }
101 
102  // Nearest state pose:
103  if (options.x_nearest_pose)
104  {
106  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.0);
107  string m_name = "X_near";
108  obj->setName(m_name);
109  obj->enableShowName();
110  obj->setPose(mrpt::poses::CPose3D(*options.x_nearest_pose));
111  scene.insert(obj);
112  }
113 
114  // Determine the up-to-now best solution, so we can highlight the best path
115  // so far:
116  typename tree_t::path_t best_path;
118  result.move_tree.backtrackPath(
119  options.highlight_path_to_node_id, best_path);
120 
121  // make list of nodes in the way of the best path:
122  std::set<const typename tree_t::edge_t *> edges_best_path,
123  edges_best_path_decim;
124  if (!best_path.empty())
125  {
126  typename tree_t::path_t::const_iterator it_end = best_path.end();
127  typename tree_t::path_t::const_iterator it_end_1 = best_path.end();
128  std::advance(it_end_1, -1);
129 
130  for (typename tree_t::path_t::const_iterator it = best_path.begin();
131  it != it_end; ++it)
132  if (it->edge_to_parent) edges_best_path.insert(it->edge_to_parent);
133 
134  // Decimate the path (always keeping the first and last entry):
136  for (typename tree_t::path_t::const_iterator it = best_path.begin();
137  it != it_end;)
138  {
139  if (it->edge_to_parent)
140  edges_best_path_decim.insert(it->edge_to_parent);
141  if (it == it_end_1) break;
142  for (size_t k = 0; k < options.draw_shape_decimation; k++)
143  {
144  if (it == it_end || it == it_end_1) break;
145  ++it;
146  }
147  }
148  }
149 
150  // The starting pose vehicle shape must be inserted independently, because
151  // the rest are edges and we draw the END pose of each edge:
152  {
154  new mrpt::opengl::CSetOfLines(*gl_veh_shape));
156  shapePose.z_incr(options.vehicle_shape_z);
157  vehShape->setPose(shapePose);
158  scene.insert(vehShape);
159  }
160 
161  // Existing nodes & edges between them:
162  {
163  const typename tree_t::node_map_t& lstNodes =
164  result.move_tree.getAllNodes();
165 
166  for (typename tree_t::node_map_t::const_iterator itNode =
167  lstNodes.begin();
168  itNode != lstNodes.end(); ++itNode)
169  {
170  const typename tree_t::node_t& node = itNode->second;
171 
172  mrpt::math::TPose2D parent_state;
173  if (node.parent_id != INVALID_NODEID)
174  {
175  parent_state = lstNodes.find(node.parent_id)->second.state;
176  }
177  const mrpt::math::TPose2D& trg_state = node.state;
178 
179  const bool is_new_one = (itNode == (lstNodes.end() - 1));
180  const bool is_best_path =
181  edges_best_path.count(node.edge_to_parent) != 0;
182  const bool is_best_path_and_draw_shape =
183  edges_best_path_decim.count(node.edge_to_parent) != 0;
184 
185  // Draw children nodes:
186  {
187  const float corner_scale =
188  xyzcorners_scale * (is_new_one ? 1.5f : 1.0f);
189 
192  obj->setPose(mrpt::poses::CPose3D(trg_state));
193  scene.insert(obj);
194 
195  // Insert vehicle shapes along optimal path:
196  if (is_best_path_and_draw_shape)
197  {
199  new mrpt::opengl::CSetOfLines(*gl_veh_shape));
200  mrpt::poses::CPose3D shapePose =
201  mrpt::poses::CPose3D(trg_state);
202  shapePose.z_incr(options.vehicle_shape_z);
203  vehShape->setPose(shapePose);
204  scene.insert(vehShape);
205  }
206  }
207 
208  // Draw line parent -> children nodes.
209  if (node.parent_id != INVALID_NODEID)
210  {
211  // Draw actual PT path between parent and children nodes:
212  ASSERT_(node.edge_to_parent);
214  m_PTGs[node.edge_to_parent->ptg_index].get();
215 
216  // Create the path shape, in relative coords to the parent node:
218  mrpt::make_aligned_shared<mrpt::opengl::CSetOfLines>();
219  obj->setPose(
220  mrpt::poses::CPose3D(parent_state)); // Points are relative
221  // to this pose: let
222  // OpenGL to deal with
223  // the coords.
224  // composition
225 
227  node.edge_to_parent->ptg_K, *obj, 0.25f /*decimation*/,
228  node.edge_to_parent->ptg_dist /*max path length*/);
229 
230  if (is_new_one && options.highlight_last_added_edge)
231  {
232  // Last edge format:
233  obj->setColor_u8(options.color_last_edge);
234  obj->setLineWidth(options.width_last_edge);
235  }
236  else
237  {
238  // Normal format:
239  obj->setColor_u8(options.color_normal_edge);
240  obj->setLineWidth(options.width_normal_edge);
241  }
242  if (is_best_path)
243  {
244  obj->setColor_u8(options.color_optimal_edge);
245  obj->setLineWidth(options.width_optimal_edge);
246  }
247 
248  scene.insert(obj);
249  }
250  }
251  }
252 
253  // The new node:
254  if (options.new_state)
255  {
257  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.2);
258  string m_name = "X_new";
259  obj->setName(m_name);
260  obj->enableShowName();
261  obj->setPose(mrpt::poses::CPose3D(*options.new_state));
262  scene.insert(obj);
263  }
264 
265  // Obstacles:
266  if (options.draw_obstacles)
267  {
269  mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
270 
271  obj->loadFromPointsMap(&pi.obstacles_points);
272  obj->setPose(
275  0.0, 0.0, 0.0))); // Points are relative to the origin
276 
277  obj->setPointSize(options.point_size_obstacles);
278  obj->setColor_u8(options.color_obstacles);
279  scene.insert(obj);
280  }
281 
282  // The current set of local obstacles:
283  // Draw this AFTER the global map so it's visible:
284  if (options.draw_obstacles && options.local_obs_from_nearest_pose &&
285  options.x_nearest_pose)
286  {
288  mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
289 
290  obj->loadFromPointsMap(options.local_obs_from_nearest_pose);
291  obj->setPose(*options.x_nearest_pose); // Points are relative to this
292  // pose: let OpenGL to deal with
293  // the coords. composition
294  obj->setPointSize(options.point_size_local_obstacles);
295  obj->setColor_u8(options.color_local_obstacles);
296  scene.insert(obj);
297  }
298 
299  // Start:
300  {
302  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.5);
303  obj->setName("START");
304  obj->enableShowName();
305  obj->setColor_u8(options.color_start);
306  obj->setPose(pi.start_pose);
307  scene.insert(obj);
308  }
309 
310  // Target:
311  {
313  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.5);
314  string m_name = "GOAL";
315  obj->setName(m_name);
316  obj->enableShowName();
317  obj->setColor_u8(options.color_goal);
318  obj->setPose(pi.goal_pose);
319  scene.insert(obj);
320  }
321 
322  // Log msg:
323  if (!options.log_msg.empty())
324  {
326  mrpt::make_aligned_shared<mrpt::opengl::CText3D>(
327  options.log_msg, "sans", options.log_msg_scale);
328  gl_txt->setLocation(options.log_msg_position);
329  scene.insert(gl_txt);
330  }
331 
332 } // end renderMoveTree()
333 }
334 }
double vehicle_shape_z
(Default=0.01) Height (Z coordinate) for the vehicle shapes.
virtual void renderPathAsSimpleLine(const uint16_t k, mrpt::opengl::CSetOfLines &gl_obj, const double decimate_distance=0.1, const double max_path_distance=-1.0) const
Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line)...
world_limits_t world_bbox_min
Bounding box of the world, used to draw uniform random pose samples.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
double xyzcorners_scale
A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape) ...
mrpt::img::TColor color_start
START indication color.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This is the base class for any user-defined PTG.
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 vehicle_line_width
Robot line width for visualization - default 2.0.
GLsizei const GLchar ** string
Definition: glext.h:4101
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::maps::CSimplePointsMap obstacles_points
World obstacles, as a point cloud.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
CSetOfObjects::Ptr CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
Returns three arrows representing a X,Y,Z 3D corner (just thick lines instead of complex arrows for f...
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:171
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:59
size_t draw_shape_decimation
(Default=1) Draw one out of N vehicle shapes along the highlighted path
#define INVALID_NODEID
Definition: TNodeID.h:20
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)
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:33
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
GLenum const GLfloat * params
Definition: glext.h:3534
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019