MRPT  2.0.4
impl_renderMoveTree.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
12 // For 3D log files
17 #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 =
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  }
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  {
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  auto it_end = best_path.end();
125  auto it_end_1 = best_path.end();
126  std::advance(it_end_1, -1);
127 
128  for (auto it = best_path.begin(); it != it_end; ++it)
129  if (it->edge_to_parent) edges_best_path.insert(it->edge_to_parent);
130 
131  // Decimate the path (always keeping the first and last entry):
133  for (auto it = best_path.begin(); it != it_end;)
134  {
135  if (it->edge_to_parent)
136  edges_best_path_decim.insert(it->edge_to_parent);
137  if (it == it_end_1) break;
138  for (size_t k = 0; k < options.draw_shape_decimation; k++)
139  {
140  if (it == it_end || it == it_end_1) break;
141  ++it;
142  }
143  }
144  }
145 
146  // The starting pose vehicle shape must be inserted independently, because
147  // the rest are edges and we draw the END pose of each edge:
148  {
150  new mrpt::opengl::CSetOfLines(*gl_veh_shape));
152  shapePose.z_incr(options.vehicle_shape_z);
153  vehShape->setPose(shapePose);
154  scene.insert(vehShape);
155  }
156 
157  // Existing nodes & edges between them:
158  {
159  const typename tree_t::node_map_t& lstNodes =
160  result.move_tree.getAllNodes();
161 
162  for (auto itNode = lstNodes.begin(); itNode != lstNodes.end(); ++itNode)
163  {
164  const typename tree_t::node_t& node = itNode->second;
165 
166  mrpt::math::TPose2D parent_state;
167  if (node.parent_id != INVALID_NODEID)
168  {
169  parent_state = lstNodes.find(node.parent_id)->second.state;
170  }
171  const mrpt::math::TPose2D& trg_state = node.state;
172 
173  const bool is_new_one = (itNode == (lstNodes.end() - 1));
174  const bool is_best_path =
175  edges_best_path.count(node.edge_to_parent) != 0;
176  const bool is_best_path_and_draw_shape =
177  edges_best_path_decim.count(node.edge_to_parent) != 0;
178 
179  // Draw children nodes:
180  {
181  const float corner_scale =
182  xyzcorners_scale * (is_new_one ? 1.5f : 1.0f);
183 
186  obj->setPose(mrpt::poses::CPose3D(trg_state));
187  scene.insert(obj);
188 
189  // Insert vehicle shapes along optimal path:
190  if (is_best_path_and_draw_shape)
191  {
193  new mrpt::opengl::CSetOfLines(*gl_veh_shape));
194  auto shapePose = mrpt::poses::CPose3D(trg_state);
195  shapePose.z_incr(options.vehicle_shape_z);
196  vehShape->setPose(shapePose);
197  scene.insert(vehShape);
198  }
199  }
200 
201  // Draw line parent -> children nodes.
202  if (node.parent_id != INVALID_NODEID)
203  {
204  // Draw actual PT path between parent and children nodes:
205  ASSERT_(node.edge_to_parent);
207  m_PTGs[node.edge_to_parent->ptg_index].get();
208 
209  // Create the path shape, in relative coords to the parent node:
212  obj->setPose(
213  mrpt::poses::CPose3D(parent_state)); // Points are relative
214  // to this pose: let
215  // OpenGL to deal with
216  // the coords.
217  // composition
218 
220  node.edge_to_parent->ptg_K, *obj, 0.25f /*decimation*/,
221  node.edge_to_parent->ptg_dist /*max path length*/);
222 
223  if (is_new_one && options.highlight_last_added_edge)
224  {
225  // Last edge format:
226  obj->setColor_u8(options.color_last_edge);
227  obj->setLineWidth(options.width_last_edge);
228  }
229  else
230  {
231  // Normal format:
232  obj->setColor_u8(options.color_normal_edge);
233  obj->setLineWidth(options.width_normal_edge);
234  }
235  if (is_best_path)
236  {
237  obj->setColor_u8(options.color_optimal_edge);
238  obj->setLineWidth(options.width_optimal_edge);
239  }
240 
241  scene.insert(obj);
242  }
243  }
244  }
245 
246  // The new node:
247  if (options.new_state)
248  {
250  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.2);
251  string m_name = "X_new";
252  obj->setName(m_name);
253  obj->enableShowName();
254  obj->setPose(mrpt::poses::CPose3D(*options.new_state));
255  scene.insert(obj);
256  }
257 
258  // Obstacles:
259  if (options.draw_obstacles)
260  {
263 
264  obj->loadFromPointsMap(&pi.obstacles_points);
266  0.0, 0.0, 0.0))); // Points are relative to the origin
267 
268  obj->setPointSize(options.point_size_obstacles);
269  obj->setColor_u8(options.color_obstacles);
270  scene.insert(obj);
271  }
272 
273  // The current set of local obstacles:
274  // Draw this AFTER the global map so it's visible:
275  if (options.draw_obstacles && options.local_obs_from_nearest_pose &&
276  options.x_nearest_pose)
277  {
280 
281  obj->loadFromPointsMap(options.local_obs_from_nearest_pose);
282  obj->setPose(*options.x_nearest_pose); // Points are relative to this
283  // pose: let OpenGL to deal with
284  // the coords. composition
285  obj->setPointSize(options.point_size_local_obstacles);
286  obj->setColor_u8(options.color_local_obstacles);
287  scene.insert(obj);
288  }
289 
290  // Start:
291  {
293  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.5);
294  obj->setName("START");
295  obj->enableShowName();
296  obj->setColor_u8(options.color_start);
297  obj->setPose(pi.start_pose);
298  scene.insert(obj);
299  }
300 
301  // Target:
302  {
304  mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale * 1.5);
305  string m_name = "GOAL";
306  obj->setName(m_name);
307  obj->enableShowName();
308  obj->setColor_u8(options.color_goal);
309  obj->setPose(pi.goal_pose);
310  scene.insert(obj);
311  }
312 
313  // Log msg:
314  if (!options.log_msg.empty())
315  {
317  options.log_msg, "sans", options.log_msg_scale);
318  gl_txt->setLocation(options.log_msg_position);
319  scene.insert(gl_txt);
320  }
321 
322 } // end renderMoveTree()
323 } // namespace mrpt::nav
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.
double xyzcorners_scale
A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape) ...
static Ptr Create(Args &&... args)
Definition: CPointCloud.h:49
mrpt::img::TColor color_start
START indication color.
double robot_shape_circular_radius
The radius of a circular-shape-model of the robot shape.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This is the base class for any user-defined PTG.
double vehicle_line_width
Robot line width for visualization - default 2.0.
static Ptr Create(Args &&... args)
Definition: CText3D.h:44
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
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...
mrpt::maps::CSimplePointsMap obstacles_points
World obstacles, as a point cloud.
RRTAlgorithmParams params
Parameters specific to this path solver algorithm.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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:155
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:56
size_t draw_shape_decimation
(Default=1) Draw one out of N vehicle shapes along the highlighted path
mrpt::math::TPolygon2D robot_shape
The robot shape used when computing collisions; it&#39;s loaded from the config file/text as a single 2xN...
#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:32
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)...
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
static Ptr Create(Args &&... args)
Definition: CSetOfLines.h:35
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020