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
CSetOfLines.h
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::draw_shape_decimation
size_t draw_shape_decimation
(Default=1) Draw one out of N vehicle shapes along the highlighted path
Definition: PlannerRRT_common.h:151
mrpt::opengl::CPointCloud::Ptr
std::shared_ptr< mrpt::opengl ::CPointCloud > Ptr
Definition: CPointCloud.h:49
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::ground_xy_grid_frequency
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
Definition: PlannerRRT_common.h:164
mrpt::nav::TPlannerInputTempl::goal_pose
node_pose_t goal_pose
Definition: PlannerRRT_common.h:30
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg_scale
double log_msg_scale
Definition: PlannerRRT_common.h:196
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::opengl::COpenGLScene::insert
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).
Definition: COpenGLScene.cpp:177
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:31
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_obstacles
mrpt::img::TColor color_obstacles
obstacles color
Definition: PlannerRRT_common.h:169
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::width_last_edge
float width_last_edge
Definition: PlannerRRT_common.h:180
mrpt::nav::TPlannerInputTempl::world_bbox_min
world_limits_t world_bbox_min
Bounding box of the world, used to draw uniform random pose samples.
Definition: PlannerRRT_common.h:32
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:56
mrpt::opengl::CSetOfLines
A set of independent lines (or segments), one line with its own start and end positions (X,...
Definition: CSetOfLines.h:32
mrpt::opengl::CPointCloud::Create
static Ptr Create(Args &&... args)
Definition: CPointCloud.h:49
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::width_normal_edge
float width_normal_edge
Definition: PlannerRRT_common.h:181
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg_position
mrpt::math::TPoint3D log_msg_position
Definition: PlannerRRT_common.h:195
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::point_size_obstacles
int point_size_obstacles
Definition: PlannerRRT_common.h:183
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg
std::string log_msg
Definition: PlannerRRT_common.h:194
R
const float R
Definition: CKinematicChain.cpp:137
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
stock_objects.h
CPointCloud.h
mrpt::opengl::stock_objects::CornerXYZSimple
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...
Definition: StockObjects.cpp:343
mrpt::nav::PlannerTPS_VirtualBase::params
RRTAlgorithmParams params
Parameters specific to this path solver algorithm.
Definition: PlannerRRT_common.h:136
mrpt::opengl::CSetOfLines::Create
static Ptr Create(Args &&... args)
Definition: CSetOfLines.h:35
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_last_added_edge
bool highlight_last_added_edge
(Default=false)
Definition: PlannerRRT_common.h:162
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::x_rand_pose
const mrpt::poses::CPose2D * x_rand_pose
Definition: PlannerRRT_common.h:153
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_local_obstacles
mrpt::img::TColor color_local_obstacles
local obstacles color
Definition: PlannerRRT_common.h:171
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::xyzcorners_scale
double xyzcorners_scale
A scale factor to all XYZ corners (default=0, means auto determien from vehicle shape)
Definition: PlannerRRT_common.h:160
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_start
mrpt::img::TColor color_start
START indication color
Definition: PlannerRRT_common.h:173
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
COpenGLScene.h
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::draw_obstacles
bool draw_obstacles
(Default=true)
Definition: PlannerRRT_common.h:192
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_goal
mrpt::img::TColor color_goal
END indication color
Definition: PlannerRRT_common.h:175
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::vehicle_line_width
double vehicle_line_width
Robot line width for visualization - default 2.0.
Definition: PlannerRRT_common.h:190
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::opengl::CSetOfLines::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfLines > Ptr
Definition: CSetOfLines.h:35
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::vehicle_shape_z
double vehicle_shape_z
(Default=0.01) Height (Z coordinate) for the vehicle shapes.
Definition: PlannerRRT_common.h:188
mrpt::nav::TPlannerInputTempl::start_pose
node_pose_t start_pose
Definition: PlannerRRT_common.h:30
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_ground_xy_grid
mrpt::img::TColor color_ground_xy_grid
Definition: PlannerRRT_common.h:176
mrpt::nav::TPlannerResultTempl
Definition: PlannerRRT_common.h:38
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::keep_max
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.
Definition: core/include/mrpt/core/bits_math.h:152
mrpt::nav::TPlannerInputTempl::world_bbox_max
world_limits_t world_bbox_max
Definition: PlannerRRT_common.h:32
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions
Options for renderMoveTree()
Definition: PlannerRRT_common.h:144
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_vehicle
mrpt::img::TColor color_vehicle
Robot color
Definition: PlannerRRT_common.h:167
mrpt::nav::RRTAlgorithmParams::robot_shape_circular_radius
double robot_shape_circular_radius
The radius of a circular-shape-model of the robot shape.
Definition: PlannerRRT_common.h:101
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_path_to_node_id
mrpt::graphs::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
Definition: PlannerRRT_common.h:148
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::new_state
const mrpt::poses::CPose2D * new_state
Definition: PlannerRRT_common.h:156
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_optimal_edge
mrpt::img::TColor color_optimal_edge
Definition: PlannerRRT_common.h:179
mrpt::nav::TPlannerInputTempl::obstacles_points
mrpt::maps::CSimplePointsMap obstacles_points
World obstacles, as a point cloud.
Definition: PlannerRRT_common.h:34
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::width_optimal_edge
float width_optimal_edge
Definition: PlannerRRT_common.h:182
mrpt::nav::TPlannerResultTempl::move_tree
tree_t move_tree
The generated motion tree that explores free space starting at "start".
Definition: PlannerRRT_common.h:55
mrpt::opengl::stock_objects::CornerXYZ
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
Definition: StockObjects.cpp:136
mrpt::nav::CParameterizedTrajectoryGenerator::renderPathAsSimpleLine
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)...
Definition: CParameterizedTrajectoryGenerator.cpp:228
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::point_size_local_obstacles
int point_size_local_obstacles
Definition: PlannerRRT_common.h:184
mrpt::nav::TPlannerInputTempl
Definition: PlannerRRT_common.h:28
mrpt::opengl::CText3D::Create
static Ptr Create(Args &&... args)
Definition: CText3D.h:44
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::x_nearest_pose
const mrpt::poses::CPose2D * x_nearest_pose
Definition: PlannerRRT_common.h:154
INVALID_NODEID
#define INVALID_NODEID
Definition: TNodeID.h:19
mrpt::nav::RRTAlgorithmParams::robot_shape
mrpt::math::TPolygon2D robot_shape
The robot shape used when computing collisions; it's loaded from the config file/text as a single 2xN...
Definition: PlannerRRT_common.h:96
mrpt::nav::PlannerTPS_VirtualBase::m_PTGs
mrpt::nav::TListPTGPtr m_PTGs
Definition: PlannerRRT_common.h:230
mrpt::opengl::CText3D::Ptr
std::shared_ptr< mrpt::opengl ::CText3D > Ptr
Definition: CText3D.h:44
CGridPlaneXY.h
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::local_obs_from_nearest_pose
const mrpt::maps::CPointsMap * local_obs_from_nearest_pose
Definition: PlannerRRT_common.h:155
mrpt::nav::PlannerTPS_VirtualBase::renderMoveTree
void renderMoveTree(mrpt::opengl::COpenGLScene &scene, const TPlannerInputTempl< node_pose_t, world_limits_t > &pi, const TPlannerResultTempl< tree_t > &result, const TRenderPlannedPathOptions &options)
Definition: impl_renderMoveTree.h:23
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:78
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_normal_edge
mrpt::img::TColor color_normal_edge
Definition: PlannerRRT_common.h:177
mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::color_last_edge
mrpt::img::TColor color_last_edge
Definition: PlannerRRT_common.h:178
CText3D.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020