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::nav
21 {
22 template <typename node_pose_t, typename world_limits_t, typename tree_t>
26  const TPlannerResultTempl<tree_t>& result,
27  const TRenderPlannedPathOptions& options)
28 {
29  using std::string;
30 
31  // Build a model of the vehicle shape:
32  mrpt::opengl::CSetOfLines::Ptr gl_veh_shape =
33  mrpt::make_aligned_shared<mrpt::opengl::CSetOfLines>();
34  double
35  xyzcorners_scale; // Size of XYZ corners (scaled to vehicle dimensions)
36  {
37  gl_veh_shape->setLineWidth(options.vehicle_line_width);
38  gl_veh_shape->setColor_u8(options.color_vehicle);
39 
40  double max_veh_radius = 0.;
41  if (!params.robot_shape.empty())
42  {
43  gl_veh_shape->appendLine(
44  params.robot_shape[0].x, params.robot_shape[0].y, 0,
45  params.robot_shape[1].x, params.robot_shape[1].y, 0);
46  for (size_t i = 2; i <= params.robot_shape.size(); i++)
47  {
48  const size_t idx = i % params.robot_shape.size();
49  mrpt::keep_max(max_veh_radius, params.robot_shape[idx].norm());
50  gl_veh_shape->appendLineStrip(
51  params.robot_shape[idx].x, params.robot_shape[idx].y, 0);
52  }
53  }
54  else if (params.robot_shape_circular_radius > 0)
55  {
56  const int NUM_VERTICES = 10;
57  const double R = params.robot_shape_circular_radius;
58  for (int i = 0; i <= NUM_VERTICES; i++)
59  {
60  const size_t idx = i % NUM_VERTICES;
61  const size_t idxn = (i + 1) % NUM_VERTICES;
62  const double ang = idx * 2 * M_PI / (NUM_VERTICES - 1);
63  const double angn = idxn * 2 * M_PI / (NUM_VERTICES - 1);
64  gl_veh_shape->appendLine(
65  R * cos(ang), R * sin(ang), 0, R * cos(angn), R * sin(angn),
66  0);
67  }
68  mrpt::keep_max(max_veh_radius, R);
69  }
70 
71  xyzcorners_scale = max_veh_radius * 0.5;
72  }
73  // Override with user scale?
74  if (options.xyzcorners_scale != 0)
75  xyzcorners_scale = options.xyzcorners_scale;
76 
77  // "ground"
78  if (options.ground_xy_grid_frequency > 0)
79  {
81  mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>(
83  pi.world_bbox_max.y, 0, options.ground_xy_grid_frequency);
84  obj->setColor_u8(options.color_ground_xy_grid);
85  scene.insert(obj);
86  }
87 
88  // Original randomly-pick pose:
89  if (options.x_rand_pose)
90  {
92  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.0);
93  string m_name = "X_rand";
94  obj->setName(m_name);
95  obj->enableShowName();
96  obj->setPose(mrpt::poses::CPose3D(*options.x_rand_pose));
97  scene.insert(obj);
98  }
99 
100  // Nearest state pose:
101  if (options.x_nearest_pose)
102  {
104  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.0);
105  string m_name = "X_near";
106  obj->setName(m_name);
107  obj->enableShowName();
108  obj->setPose(mrpt::poses::CPose3D(*options.x_nearest_pose));
109  scene.insert(obj);
110  }
111 
112  // Determine the up-to-now best solution, so we can highlight the best path
113  // so far:
114  typename tree_t::path_t best_path;
116  result.move_tree.backtrackPath(
117  options.highlight_path_to_node_id, best_path);
118 
119  // make list of nodes in the way of the best path:
120  std::set<const typename tree_t::edge_t *> edges_best_path,
121  edges_best_path_decim;
122  if (!best_path.empty())
123  {
124  typename tree_t::path_t::const_iterator it_end = best_path.end();
125  typename tree_t::path_t::const_iterator it_end_1 = best_path.end();
126  std::advance(it_end_1, -1);
127 
128  for (typename tree_t::path_t::const_iterator it = best_path.begin();
129  it != it_end; ++it)
130  if (it->edge_to_parent) edges_best_path.insert(it->edge_to_parent);
131 
132  // Decimate the path (always keeping the first and last entry):
134  for (typename tree_t::path_t::const_iterator it = best_path.begin();
135  it != it_end;)
136  {
137  if (it->edge_to_parent)
138  edges_best_path_decim.insert(it->edge_to_parent);
139  if (it == it_end_1) break;
140  for (size_t k = 0; k < options.draw_shape_decimation; k++)
141  {
142  if (it == it_end || it == it_end_1) break;
143  ++it;
144  }
145  }
146  }
147 
148  // The starting pose vehicle shape must be inserted independently, because
149  // the rest are edges and we draw the END pose of each edge:
150  {
152  new mrpt::opengl::CSetOfLines(*gl_veh_shape));
154  shapePose.z_incr(options.vehicle_shape_z);
155  vehShape->setPose(shapePose);
156  scene.insert(vehShape);
157  }
158 
159  // Existing nodes & edges between them:
160  {
161  const typename tree_t::node_map_t& lstNodes =
162  result.move_tree.getAllNodes();
163 
164  for (typename tree_t::node_map_t::const_iterator itNode =
165  lstNodes.begin();
166  itNode != lstNodes.end(); ++itNode)
167  {
168  const typename tree_t::node_t& node = itNode->second;
169 
170  mrpt::math::TPose2D parent_state;
171  if (node.parent_id != INVALID_NODEID)
172  {
173  parent_state = lstNodes.find(node.parent_id)->second.state;
174  }
175  const mrpt::math::TPose2D& trg_state = node.state;
176 
177  const bool is_new_one = (itNode == (lstNodes.end() - 1));
178  const bool is_best_path =
179  edges_best_path.count(node.edge_to_parent) != 0;
180  const bool is_best_path_and_draw_shape =
181  edges_best_path_decim.count(node.edge_to_parent) != 0;
182 
183  // Draw children nodes:
184  {
185  const float corner_scale =
186  xyzcorners_scale * (is_new_one ? 1.5f : 1.0f);
187 
190  obj->setPose(mrpt::poses::CPose3D(trg_state));
191  scene.insert(obj);
192 
193  // Insert vehicle shapes along optimal path:
194  if (is_best_path_and_draw_shape)
195  {
197  new mrpt::opengl::CSetOfLines(*gl_veh_shape));
198  mrpt::poses::CPose3D shapePose =
199  mrpt::poses::CPose3D(trg_state);
200  shapePose.z_incr(options.vehicle_shape_z);
201  vehShape->setPose(shapePose);
202  scene.insert(vehShape);
203  }
204  }
205 
206  // Draw line parent -> children nodes.
207  if (node.parent_id != INVALID_NODEID)
208  {
209  // Draw actual PT path between parent and children nodes:
210  ASSERT_(node.edge_to_parent);
212  m_PTGs[node.edge_to_parent->ptg_index].get();
213 
214  // Create the path shape, in relative coords to the parent node:
216  mrpt::make_aligned_shared<mrpt::opengl::CSetOfLines>();
217  obj->setPose(
218  mrpt::poses::CPose3D(parent_state)); // Points are relative
219  // to this pose: let
220  // OpenGL to deal with
221  // the coords.
222  // composition
223 
225  node.edge_to_parent->ptg_K, *obj, 0.25f /*decimation*/,
226  node.edge_to_parent->ptg_dist /*max path length*/);
227 
228  if (is_new_one && options.highlight_last_added_edge)
229  {
230  // Last edge format:
231  obj->setColor_u8(options.color_last_edge);
232  obj->setLineWidth(options.width_last_edge);
233  }
234  else
235  {
236  // Normal format:
237  obj->setColor_u8(options.color_normal_edge);
238  obj->setLineWidth(options.width_normal_edge);
239  }
240  if (is_best_path)
241  {
242  obj->setColor_u8(options.color_optimal_edge);
243  obj->setLineWidth(options.width_optimal_edge);
244  }
245 
246  scene.insert(obj);
247  }
248  }
249  }
250 
251  // The new node:
252  if (options.new_state)
253  {
255  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.2);
256  string m_name = "X_new";
257  obj->setName(m_name);
258  obj->enableShowName();
259  obj->setPose(mrpt::poses::CPose3D(*options.new_state));
260  scene.insert(obj);
261  }
262 
263  // Obstacles:
264  if (options.draw_obstacles)
265  {
267  mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
268 
269  obj->loadFromPointsMap(&pi.obstacles_points);
270  obj->setPose(
273  0.0, 0.0, 0.0))); // Points are relative to the origin
274 
275  obj->setPointSize(options.point_size_obstacles);
276  obj->setColor_u8(options.color_obstacles);
277  scene.insert(obj);
278  }
279 
280  // The current set of local obstacles:
281  // Draw this AFTER the global map so it's visible:
282  if (options.draw_obstacles && options.local_obs_from_nearest_pose &&
283  options.x_nearest_pose)
284  {
286  mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
287 
288  obj->loadFromPointsMap(options.local_obs_from_nearest_pose);
289  obj->setPose(*options.x_nearest_pose); // Points are relative to this
290  // pose: let OpenGL to deal with
291  // the coords. composition
292  obj->setPointSize(options.point_size_local_obstacles);
293  obj->setColor_u8(options.color_local_obstacles);
294  scene.insert(obj);
295  }
296 
297  // Start:
298  {
300  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.5);
301  obj->setName("START");
302  obj->enableShowName();
303  obj->setColor_u8(options.color_start);
304  obj->setPose(pi.start_pose);
305  scene.insert(obj);
306  }
307 
308  // Target:
309  {
311  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.5);
312  string m_name = "GOAL";
313  obj->setName(m_name);
314  obj->enableShowName();
315  obj->setColor_u8(options.color_goal);
316  obj->setPose(pi.goal_pose);
317  scene.insert(obj);
318  }
319 
320  // Log msg:
321  if (!options.log_msg.empty())
322  {
324  mrpt::make_aligned_shared<mrpt::opengl::CText3D>(
325  options.log_msg, "sans", options.log_msg_scale);
326  gl_txt->setLocation(options.log_msg_position);
327  scene.insert(gl_txt);
328  }
329 
330 } // end renderMoveTree()
331 }
332 
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
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:38
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
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:19
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:31
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020