Main MRPT website > C++ reference for MRPT 1.5.7
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-2016, 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 {
32  using std::string;
33 
34  // Build a model of the vehicle shape:
35  mrpt::opengl::CSetOfLinesPtr gl_veh_shape = mrpt::opengl::CSetOfLines::Create();
36  double xyzcorners_scale; // Size of XYZ corners (scaled to vehicle dimensions)
37  {
38  gl_veh_shape->setLineWidth(options.vehicle_line_width);
39  gl_veh_shape->setColor_u8(options.color_vehicle);
40 
41  double max_veh_radius = 0.;
42  if (!params.robot_shape.empty())
43  {
44  gl_veh_shape->appendLine(
45  params.robot_shape[0].x, params.robot_shape[0].y, 0,
46  params.robot_shape[1].x, params.robot_shape[1].y, 0);
47  for (size_t i = 2; i <= params.robot_shape.size(); i++)
48  {
49  const size_t idx = i % params.robot_shape.size();
50  mrpt::utils::keep_max(max_veh_radius, params.robot_shape[idx].norm());
51  gl_veh_shape->appendLineStrip(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,
66  R*cos(angn), R*sin(angn), 0);
67  }
68  mrpt::utils::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  {
80  mrpt::opengl::CGridPlaneXYPtr obj = mrpt::opengl::CGridPlaneXY::Create(pi.world_bbox_min.x, pi.world_bbox_max.x, pi.world_bbox_min.y, pi.world_bbox_max.y, 0, options.ground_xy_grid_frequency);
81  obj->setColor_u8(options.color_ground_xy_grid);
82  scene.insert(obj);
83  }
84 
85  // Original randomly-pick pose:
86  if (options.x_rand_pose)
87  {
88  mrpt::opengl::CSetOfObjectsPtr obj = mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale*1.0);
89  string m_name = "X_rand";
90  obj->setName(m_name);
91  obj->enableShowName();
92  obj->setPose(mrpt::poses::CPose3D(*options.x_rand_pose));
93  scene.insert(obj);
94  }
95 
96  // Nearest state pose:
97  if (options.x_nearest_pose)
98  {
99  mrpt::opengl::CSetOfObjectsPtr obj = mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale*1.0);
100  string m_name = "X_near";
101  obj->setName(m_name);
102  obj->enableShowName();
103  obj->setPose(mrpt::poses::CPose3D(*options.x_nearest_pose));
104  scene.insert(obj);
105  }
106 
107  // Determine the up-to-now best solution, so we can highlight the best path so far:
108  typename tree_t::path_t best_path;
110  result.move_tree.backtrackPath(options.highlight_path_to_node_id, best_path);
111 
112  // make list of nodes in the way of the best path:
113  std::set<const typename tree_t::edge_t*> edges_best_path, edges_best_path_decim;
114  if (!best_path.empty())
115  {
116  typename tree_t::path_t::const_iterator it_end = best_path.end();
117  typename tree_t::path_t::const_iterator it_end_1 = best_path.end(); std::advance(it_end_1, -1);
118 
119  for (typename tree_t::path_t::const_iterator it = best_path.begin(); it != it_end; ++it)
120  if (it->edge_to_parent)
121  edges_best_path.insert(it->edge_to_parent);
122 
123  // Decimate the path (always keeping the first and last entry):
125  for (typename tree_t::path_t::const_iterator it = best_path.begin(); it != it_end; )
126  {
127  if (it->edge_to_parent)
128  edges_best_path_decim.insert(it->edge_to_parent);
129  if (it == it_end_1) break;
130  for (size_t k = 0; k<options.draw_shape_decimation; k++) {
131  if (it == it_end || it == it_end_1) break;
132  ++it;
133  }
134  }
135  }
136 
137  // The starting pose vehicle shape must be inserted independently, because the rest are edges and we draw the END pose of each edge:
138  {
139  mrpt::opengl::CSetOfLinesPtr vehShape(new mrpt::opengl::CSetOfLines(*gl_veh_shape));
141  shapePose.z_incr(options.vehicle_shape_z);
142  vehShape->setPose(shapePose);
143  scene.insert(vehShape);
144  }
145 
146  // Existing nodes & edges between them:
147  {
148  const typename tree_t::node_map_t & lstNodes = result.move_tree.getAllNodes();
149 
150  for (typename tree_t::node_map_t::const_iterator itNode = lstNodes.begin(); itNode != lstNodes.end(); ++itNode)
151  {
152  const typename tree_t::node_t & node = itNode->second;
153 
154  mrpt::math::TPose2D parent_state;
155  if (node.parent_id != INVALID_NODEID)
156  {
157  parent_state = lstNodes.find(node.parent_id)->second.state;
158  }
159  const mrpt::math::TPose2D &trg_state = node.state;
160 
161  const bool is_new_one = (itNode == (lstNodes.end() - 1));
162  const bool is_best_path = edges_best_path.count(node.edge_to_parent) != 0;
163  const bool is_best_path_and_draw_shape = edges_best_path_decim.count(node.edge_to_parent) != 0;
164 
165  // Draw children nodes:
166  {
167  const float corner_scale = xyzcorners_scale* (is_new_one ? 1.5f : 1.0f);
168 
169  mrpt::opengl::CSetOfObjectsPtr obj = mrpt::opengl::stock_objects::CornerXYZSimple(corner_scale);
170  obj->setPose(mrpt::poses::CPose3D(trg_state));
171  scene.insert(obj);
172 
173  // Insert vehicle shapes along optimal path:
174  if (is_best_path_and_draw_shape)
175  {
176  mrpt::opengl::CSetOfLinesPtr vehShape(new mrpt::opengl::CSetOfLines(*gl_veh_shape));
177  mrpt::poses::CPose3D shapePose = mrpt::poses::CPose3D(trg_state);
178  shapePose.z_incr(options.vehicle_shape_z);
179  vehShape->setPose(shapePose);
180  scene.insert(vehShape);
181  }
182  }
183 
184  // Draw line parent -> children nodes.
185  if (node.parent_id != INVALID_NODEID)
186  {
187  // Draw actual PT path between parent and children nodes:
188  ASSERT_(node.edge_to_parent)
189  const mrpt::nav::CParameterizedTrajectoryGenerator * ptg = m_PTGs[node.edge_to_parent->ptg_index].pointer();
190 
191  // Create the path shape, in relative coords to the parent node:
192  mrpt::opengl::CSetOfLinesPtr obj = mrpt::opengl::CSetOfLines::Create();
193  obj->setPose(mrpt::poses::CPose3D(parent_state)); // Points are relative to this pose: let OpenGL to deal with the coords. composition
194 
195  ptg->renderPathAsSimpleLine(node.edge_to_parent->ptg_K, *obj, 0.25f /*decimation*/, node.edge_to_parent->ptg_dist /*max path length*/);
196 
197  if (is_new_one && options.highlight_last_added_edge)
198  {
199  // Last edge format:
200  obj->setColor_u8(options.color_last_edge);
201  obj->setLineWidth(options.width_last_edge);
202  }
203  else
204  {
205  // Normal format:
206  obj->setColor_u8(options.color_normal_edge);
207  obj->setLineWidth(options.width_normal_edge);
208  }
209  if (is_best_path)
210  {
211  obj->setColor_u8(options.color_optimal_edge);
212  obj->setLineWidth(options.width_optimal_edge);
213  }
214 
215  scene.insert(obj);
216  }
217  }
218  }
219 
220  // The new node:
221  if (options.new_state)
222  {
223  mrpt::opengl::CSetOfObjectsPtr obj = mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale*1.2);
224  string m_name = "X_new";
225  obj->setName(m_name);
226  obj->enableShowName();
227  obj->setPose(mrpt::poses::CPose3D(*options.new_state));
228  scene.insert(obj);
229  }
230 
231  // Obstacles:
232  if (options.draw_obstacles)
233  {
234  mrpt::opengl::CPointCloudPtr obj = mrpt::opengl::CPointCloud::Create();
235 
236  obj->loadFromPointsMap(&pi.obstacles_points);
237  obj->setPose(mrpt::poses::CPose3D(mrpt::poses::CPose2D(0.0, 0.0, 0.0))); // Points are relative to the origin
238 
239  obj->setPointSize(options.point_size_obstacles);
240  obj->setColor_u8(options.color_obstacles);
241  scene.insert(obj);
242  }
243 
244  // The current set of local obstacles:
245  // Draw this AFTER the global map so it's visible:
246  if (options.draw_obstacles && options.local_obs_from_nearest_pose && options.x_nearest_pose)
247  {
248  mrpt::opengl::CPointCloudPtr obj = mrpt::opengl::CPointCloud::Create();
249 
250  obj->loadFromPointsMap(options.local_obs_from_nearest_pose);
251  obj->setPose(*options.x_nearest_pose); // Points are relative to this pose: let OpenGL to deal with the coords. composition
252  obj->setPointSize(options.point_size_local_obstacles);
253  obj->setColor_u8(options.color_local_obstacles);
254  scene.insert(obj);
255  }
256 
257  // Start:
258  {
259  mrpt::opengl::CSetOfObjectsPtr obj = mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale*1.5);
260  obj->setName("START");
261  obj->enableShowName();
262  obj->setColor_u8(options.color_start);
263  obj->setPose(pi.start_pose);
264  scene.insert(obj);
265  }
266 
267  // Target:
268  {
269  mrpt::opengl::CSetOfObjectsPtr obj = mrpt::opengl::stock_objects::CornerXYZ(xyzcorners_scale*1.5);
270  string m_name = "GOAL";
271  obj->setName(m_name);
272  obj->enableShowName();
273  obj->setColor_u8(options.color_goal);
274  obj->setPose(pi.goal_pose);
275  scene.insert(obj);
276  }
277 
278  // Log msg:
279  if (!options.log_msg.empty())
280  {
281  mrpt::opengl::CText3DPtr gl_txt = mrpt::opengl::CText3D::Create(options.log_msg, "sans", options.log_msg_scale);
282  gl_txt->setLocation(options.log_msg_position);
283  scene.insert(gl_txt);
284  }
285 
286 } // end renderMoveTree()
287 
288 }
289 }
mrpt::utils::TColor color_goal
END indication color.
double vehicle_shape_z
(Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps making it in the "first plane" ...
#define ASSERT_ABOVE_(__A, __B)
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_max
Bounding box of the world, used to draw uniform random pose samples.
#define INVALID_NODEID
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
#define M_PI
Definition: bits.h:78
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
mrpt::utils::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
double xyzcorners_scale
A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape) ...
static CSetOfLinesPtr Create()
This is the base class for any user-defined PTG.
void insert(const CRenderizablePtr &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)...
CSetOfObjectsPtr OPENGL_IMPEXP 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...
double vehicle_line_width
Robot line width for visualization - default 2.0.
static CText3DPtr Create()
GLsizei const GLchar ** string
Definition: glext.h:3919
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
static CGridPlaneXYPtr Create()
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:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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:49
#define ASSERT_(f)
size_t draw_shape_decimation
(Default=1) Draw one out of N vehicle shapes along the highlighted path
mrpt::utils::TColor color_start
START indication color.
static CPointCloudPtr Create()
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:35
GLenum const GLfloat * params
Definition: glext.h:3514
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::utils::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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019