rviz_visual_tools_demo.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, PickNik Consulting
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Demo implementation of rviz_visual_tools
37  To use, add a Rviz Marker Display subscribed to topic /rviz_visual_tools
38 */
39 
40 // ROS
41 #include <ros/ros.h>
42 
43 // For visualizing things in rviz
45 
46 // C++
47 #include <string>
48 #include <vector>
49 
50 namespace rvt = rviz_visual_tools;
51 
52 namespace rviz_visual_tools
53 {
55 {
56 private:
57  // A shared node handle
59 
60  // For visualizing things in rviz
62 
63  std::string name_;
64 
65 public:
69  RvizVisualToolsDemo() : name_("rviz_demo")
70  {
71  visual_tools_.reset(new rvt::RvizVisualTools("world", "/rviz_visual_tools"));
72  visual_tools_->loadMarkerPub(); // create publisher before waiting
73 
74  ROS_INFO("Sleeping 5 seconds before running demo");
75  ros::Duration(5.0).sleep();
76 
77  // Clear messages
78  visual_tools_->deleteAllMarkers();
79  visual_tools_->enableBatchPublishing();
80  }
81 
82  void publishLabelHelper(const Eigen::Affine3d& pose, const std::string& label)
83  {
84  Eigen::Affine3d pose_copy = pose;
85  pose_copy.translation().x() -= 0.2;
86  visual_tools_->publishText(pose_copy, label, rvt::WHITE, rvt::XXLARGE, false);
87  }
88 
89  void testRows(double& x_location)
90  {
91  // Create pose
92  Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
93  Eigen::Affine3d pose2 = Eigen::Affine3d::Identity();
94 
95  pose1.translation().x() = x_location;
96 
97  double space_between_rows = 0.2;
98  double y = 0;
99  double step;
100 
101  // --------------------------------------------------------------------
102  ROS_INFO_STREAM_NAMED(name_, "Displaying range of colors red->green");
103  step = 0.02;
104  for (double i = 0; i <= 1.0; i += 0.02)
105  {
106  geometry_msgs::Vector3 scale = visual_tools_->getScale(MEDIUM);
107  std_msgs::ColorRGBA color = visual_tools_->getColorScale(i);
108  visual_tools_->publishSphere(visual_tools_->convertPose(pose1), color, scale, "Sphere");
109  if (i == 0.0)
110  {
111  publishLabelHelper(pose1, "Sphere Color Range");
112  }
113  pose1.translation().x() += step;
114  }
115  visual_tools_->trigger();
116 
117  // --------------------------------------------------------------------
118  ROS_INFO_STREAM_NAMED(name_, "Displaying Coordinate Axis");
119  pose1.translation().x() = 0;
120  y += space_between_rows;
121  pose1.translation().y() = y;
122  step = 0.025;
123  for (double i = 0; i <= 1.0; i += step)
124  {
125  visual_tools_->publishAxis(pose1);
126  if (i == 0.0)
127  {
128  publishLabelHelper(pose1, "Coordinate Axis");
129  }
130 
131  pose1.translation().x() += step;
132  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitX()) *
133  Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitY()) *
134  Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
135  }
136  visual_tools_->trigger();
137 
138  // --------------------------------------------------------------------
139  ROS_INFO_STREAM_NAMED(name_, "Displaying Arrows");
140  pose1 = Eigen::Affine3d::Identity();
141  y += space_between_rows;
142  pose1.translation().y() = y;
143  step = 0.025;
144  for (double i = 0; i <= 1.0; i += step)
145  {
146  visual_tools_->publishArrow(pose1, rvt::RAND);
147  if (i == 0.0)
148  {
149  publishLabelHelper(pose1, "Arrows");
150  }
151 
152  pose1.translation().x() += step;
153  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
154  }
155  visual_tools_->trigger();
156 
157  // --------------------------------------------------------------------
158  ROS_INFO_STREAM_NAMED(name_, "Displaying Rectangular Cuboid");
159  double cuboid_max_size = 0.075;
160  double cuboid_min_size = 0.01;
161  pose1 = Eigen::Affine3d::Identity();
162  pose2 = Eigen::Affine3d::Identity();
163  y += space_between_rows;
164  pose1.translation().y() = y;
165  pose2.translation().y() = y;
166  step = 0.1;
167  for (double i = 0; i <= 1.0; i += step)
168  {
169  pose2 = pose1;
170  pose2.translation().x() += i * cuboid_max_size + cuboid_min_size;
171  pose2.translation().y() += i * cuboid_max_size + cuboid_min_size;
172  pose2.translation().z() += i * cuboid_max_size + cuboid_min_size;
173  visual_tools_->publishCuboid(pose1.translation(), pose2.translation(), rvt::RAND);
174 
175  if (i == 0.0)
176  {
177  publishLabelHelper(pose1, "Cuboid");
178  }
179 
180  pose1.translation().x() += step;
181  }
182  visual_tools_->trigger();
183 
184  // --------------------------------------------------------------------
185  ROS_INFO_STREAM_NAMED(name_, "Displaying Lines");
186  double line_max_size = 0.075;
187  double line_min_size = 0.01;
188  pose1 = Eigen::Affine3d::Identity();
189  pose2 = Eigen::Affine3d::Identity();
190  y += space_between_rows;
191  pose1.translation().y() = y;
192  pose2.translation().y() = y;
193  step = 0.1;
194  for (double i = 0; i <= 1.0; i += step)
195  {
196  pose2 = pose1;
197  pose2.translation().x() += i * line_max_size + line_min_size;
198  pose2.translation().y() += i * line_max_size + line_min_size;
199  pose2.translation().z() += i * line_max_size + line_min_size;
200  visual_tools_->publishLine(pose1.translation(), pose2.translation(), rvt::RAND);
201 
202  if (i == 0.0)
203  {
204  publishLabelHelper(pose1, "Line");
205  }
206 
207  pose1.translation().x() += step;
208  }
209  visual_tools_->trigger();
210 
211  // --------------------------------------------------------------------
212  ROS_INFO_STREAM_NAMED(name_, "Displaying Cylinder");
213  pose1 = Eigen::Affine3d::Identity();
214  y += space_between_rows;
215  pose1.translation().y() = y;
216  step = 0.025;
217  for (double i = 0; i <= 1.0; i += step)
218  {
219  visual_tools_->publishCylinder(pose1, rvt::RAND);
220  if (i == 0.0)
221  {
222  publishLabelHelper(pose1, "Cylinder");
223  }
224 
225  pose1.translation().x() += step;
226  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
227  }
228  visual_tools_->trigger();
229 
230  // --------------------------------------------------------------------
231  ROS_INFO_STREAM_NAMED(name_, "Displaying Axis Cone");
232  pose1 = Eigen::Affine3d::Identity();
233  y += space_between_rows;
234  pose1.translation().y() = y;
235  step = 0.025;
236  double angle_step = 0.1;
237  double angle = 1;
238 
239  for (double i = 0; i <= 1.0; i += step)
240  {
241  visual_tools_->publishCone(pose1, M_PI / angle, rvt::RAND, 0.05);
242  if (i == 0.0)
243  {
244  publishLabelHelper(pose1, "Cone");
245  }
246 
247  pose1.translation().x() += step;
248  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
249  angle += angle_step;
250  }
251  visual_tools_->trigger();
252 
253  // --------------------------------------------------------------------
254  ROS_INFO_STREAM_NAMED(name_, "Displaying Wireframe Cuboid");
255  pose1 = Eigen::Affine3d::Identity();
256  y += space_between_rows;
257  pose1.translation().y() = y;
258  step = 0.1;
259  // TODO(davetcoleman): use generateRandomCuboid()
260  Eigen::Vector3d min_point, max_point;
261  min_point << -0.05, -0.05, -0.05;
262  max_point << 0.05, 0.05, 0.05;
263  for (double i = 0; i <= 1.0; i += step)
264  {
265  visual_tools_->publishWireframeCuboid(pose1, min_point, max_point, rvt::RAND);
266  if (i == 0.0)
267  {
268  publishLabelHelper(pose1, "Wireframe Cuboid");
269  }
270 
271  pose1.translation().x() += step;
272  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
273  }
274  visual_tools_->trigger();
275 
276  // --------------------------------------------------------------------
277  ROS_INFO_STREAM_NAMED(name_, "Displaying Sized Wireframe Cuboid");
278  pose1 = Eigen::Affine3d::Identity();
279  y += space_between_rows;
280  pose1.translation().y() = y;
281  step = 0.1;
282  double depth = 0.05, width = 0.05, height = 0.05;
283  for (double i = 0; i <= 1.0; i += step)
284  {
285  visual_tools_->publishWireframeCuboid(pose1, depth, width, height, rvt::RAND);
286  if (i == 0.0)
287  {
288  publishLabelHelper(pose1, "Wireframe Cuboid");
289  }
290 
291  pose1.translation().x() += step;
292  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
293  }
294  visual_tools_->trigger();
295 
296  // --------------------------------------------------------------------
297  ROS_INFO_STREAM_NAMED(name_, "Displaying Planes");
298  pose1 = Eigen::Affine3d::Identity();
299  y += space_between_rows;
300  pose1.translation().y() = y;
301  step = 0.2;
302  double max_plane_size = 0.075;
303  double min_plane_size = 0.01;
304  for (double i = 0; i <= 1.0; i += step)
305  {
306  visual_tools_->publishXYPlane(pose1, rvt::RED, i * max_plane_size + min_plane_size);
307  visual_tools_->publishXZPlane(pose1, rvt::GREEN, i * max_plane_size + min_plane_size);
308  visual_tools_->publishYZPlane(pose1, rvt::BLUE, i * max_plane_size + min_plane_size);
309  if (i == 0.0)
310  {
311  publishLabelHelper(pose1, "Planes");
312  }
313 
314  pose1.translation().x() += step;
315  }
316  visual_tools_->trigger();
317 
318  // --------------------------------------------------------------------
319  ROS_INFO_STREAM_NAMED(name_, "Displaying Graph");
320  pose1 = Eigen::Affine3d::Identity();
321  y += space_between_rows;
322  pose1.translation().y() = y;
323  step = 0.1;
324  graph_msgs::GeometryGraph graph;
325  for (double i = 0; i <= 1.0; i += step)
326  {
327  graph.nodes.push_back(visual_tools_->convertPose(pose1).position);
328  graph_msgs::Edges edges;
329  if (i > 0)
330  {
331  edges.node_ids.push_back(0);
332  }
333  graph.edges.push_back(edges);
334 
335  if (i == 0.0)
336  {
337  publishLabelHelper(pose1, "Graph");
338  }
339 
340  pose1.translation().x() += step;
341  pose1.translation().z() += visual_tools_->dRand(-0.1, 0.1);
342  }
343  visual_tools_->publishGraph(graph, rvt::ORANGE, 0.005);
344  visual_tools_->trigger();
345 
346  // --------------------------------------------------------------------
347  // TODO(davetcoleman): publishMesh
348 
349  // --------------------------------------------------------------------
350  // TODO(davetcoleman): publishPolygon
351 
352  // --------------------------------------------------------------------
353  ROS_INFO_STREAM_NAMED(name_, "Displaying Labeled Coordinate Axis");
354  pose1.translation().x() = 0;
355  y += space_between_rows;
356  pose1.translation().y() = y;
357  pose1.translation().z() = 0;
358  step = 0.2;
359  for (double i = 0; i <= 1.0; i += step)
360  {
361  visual_tools_->publishAxisLabeled(pose1, "label of axis");
362  if (i == 0.0)
363  {
364  publishLabelHelper(pose1, "Labeled Axis");
365  }
366 
367  pose1.translation().x() += step;
368  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitX()) *
369  Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitY()) *
370  Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
371  }
372  visual_tools_->trigger();
373 
374  // --------------------------------------------------------------------
375  ROS_INFO_STREAM_NAMED(name_, "Displaying Multi-Color Path");
376  pose1 = Eigen::Affine3d::Identity();
377  pose2 = Eigen::Affine3d::Identity();
378  y += space_between_rows;
379  pose1.translation().y() = y;
380  step = 0.1;
381 
383  std::vector<rviz_visual_tools::colors> colors;
384  unsigned index(0);
385  for (double i = 0; i < 1.0; i += step)
386  {
387  pose1.translation().y() = y;
388  if (++index % 2 == 0)
389  {
390  pose1.translation().y() += step / 2.0;
391  colors.push_back(rviz_visual_tools::WHITE);
392  }
393  else
394  {
395  pose1.translation().y() -= step / 2.0;
396  colors.push_back(rviz_visual_tools::BLUE);
397  }
398  path.emplace_back(pose1.translation());
399  pose1.translation().x() += step;
400 
401  if (i == 0.0)
402  {
403  publishLabelHelper(pose1, "Path");
404  }
405  }
406  visual_tools_->publishPath(path, colors);
407  visual_tools_->trigger();
408 
409  // Set x location for next visualization function
410  x_location += 1.25;
411  }
412 
414  void testSize(double& x_location, scales scale)
415  {
416  // Create pose
417  Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
418  Eigen::Affine3d pose2 = Eigen::Affine3d::Identity();
419 
420  // Reusable vector of 2 colors
421  std::vector<colors> colors;
422  colors.push_back(RED);
423  colors.push_back(GREEN);
424 
425  // Reusable points vector
428 
429  double step = 0.25; // space between each row
430 
431  // Show test label
432  pose1.translation().x() = x_location - 0.1;
433  visual_tools_->publishText(pose1, "Testing consistency of " + visual_tools_->scaleToString(scale) + " marker scale",
434  WHITE, XLARGE, false);
435 
436  pose1.translation().x() = x_location;
437 
438  // TODO(dave): publishCone() - no scale version available
439  // TODO(dave): publishXYPlane() - no scale version available
440  // TODO(dave): publishXZPlane() - no scale version available
441  // TODO(dave): publishYZPlane() - no scale version available
442 
443  // Sphere
444  visual_tools_->publishSphere(pose1, BLUE, scale);
445  pose1.translation().y() += step;
446 
447  // Spheres
448  points1.clear();
449  points1.emplace_back(pose1.translation());
450  pose1.translation().x() += step;
451  points1.emplace_back(pose1.translation());
452  visual_tools_->publishSpheres(points1, BLUE, scale);
453  pose1.translation().x() = x_location; // reset
454  pose1.translation().y() += step;
455 
456  // Spheres with colors
457  points1.clear();
458  points1.emplace_back(pose1.translation());
459  pose1.translation().x() += step;
460  points1.emplace_back(pose1.translation());
461  visual_tools_->publishSpheres(points1, colors, scale);
462  pose1.translation().x() = x_location; // reset
463  pose1.translation().y() += step;
464 
465  // YArrow
466  visual_tools_->publishYArrow(pose1, BLUE, scale);
467  pose1.translation().y() += step;
468 
469  // ZArrow
470  visual_tools_->publishZArrow(pose1, GREEN, scale);
471  pose1.translation().y() += step;
472 
473  // XArrow
474  visual_tools_->publishXArrow(pose1, RED, scale);
475  pose1.translation().y() += step;
476 
477  // Arrow (x arrow)
478  visual_tools_->publishArrow(pose1, RED, scale);
479  pose1.translation().y() += step;
480 
481  // Line
482  pose2 = pose1;
483  pose2.translation().x() += step / 2.0;
484  visual_tools_->publishLine(pose1, pose2, PURPLE, scale);
485  pose1.translation().y() += step;
486 
487  // Lines
488  points1.clear();
489  points2.clear();
490  pose2 = pose1;
491  pose2.translation().x() += step / 2.0;
492  points1.emplace_back(pose1.translation());
493  points2.emplace_back(pose2.translation());
494  pose1.translation().x() += step / 2.0;
495 
496  pose2 = pose1;
497  pose2.translation().x() += step / 2.0;
498  // points1.push_back(pose1.translation());
499  // points2.push_back(pose2.translation());
500  colors.clear(); // temp
501  colors.push_back(ORANGE);
502  visual_tools_->publishLines(points1, points2, colors, scale);
503  pose1.translation().x() = x_location; // reset
504  pose1.translation().y() += step;
505 
506  // TODO(dave): publishPath
507  // TODO(dave): publishPolygon
508  // TODO(dave): publishWireframeCuboid
509  // TODO(dave): publishWireframeRectangle
510 
511  // Axis Labeled
512  visual_tools_->publishAxisLabeled(pose1, "Axis", scale);
513  pose1.translation().y() += step;
514 
515  // Axis
516  visual_tools_->publishAxis(pose1, scale);
517  pose1.translation().y() += step;
518 
519  // TODO(dave): publishAxis
520 
521  // Cylinder
522  pose2 = pose1;
523  pose2.translation().x() += step / 2.0;
524  visual_tools_->publishCylinder(pose1.translation(), pose2.translation(), BLUE, scale);
525  pose1.translation().y() += step;
526 
527  // TODO(dave): publishMesh
528 
529  // TODO(dave): publishGraph
530 
531  // Text
532  visual_tools_->publishText(pose1, "Text", WHITE, scale, false);
533  pose1.translation().y() += step;
534 
535  // Display test
536  visual_tools_->trigger();
537 
538  // Set x location for next visualization function
539  x_location += 0.5;
540  }
541 
543  void testSizes(double& x_location)
544  {
545  ROS_INFO_STREAM_NAMED(name_, "Testing sizes of marker scale");
546 
547  // Create pose
548  Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
549  Eigen::Affine3d pose2;
550 
551  // Show test label
552  pose1.translation().x() = x_location - 0.1;
553  visual_tools_->publishText(pose1, "Testing sizes of marker scale", WHITE, XLARGE, false);
554 
555  pose1.translation().x() = x_location;
556  pose2.translation().x() = x_location;
557 
558  // Sphere
559  for (scales scale = XXXXSMALL; scale <= XXXXLARGE; /*inline*/)
560  {
561  if (scale == MEDIUM)
562  {
563  visual_tools_->publishSphere(pose1, GREEN, scale);
564  }
565  else
566  {
567  visual_tools_->publishSphere(pose1, GREY, scale);
568  }
569  visual_tools_->publishText(pose2, "Size " + visual_tools_->scaleToString(scale), WHITE, scale, false);
570 
571  scale = static_cast<scales>(static_cast<int>(scale) + 1);
572  pose1.translation().y() += visual_tools_->getScale(scale).x + 0.1;
573 
574  // Text location
575  pose2.translation().y() = pose1.translation().y();
576  pose2.translation().x() = x_location + visual_tools_->getScale(scale).x * 1.3;
577  }
578 
579  // Display test
580  visual_tools_->trigger();
581 
582  // Set x location for next visualization function
583  x_location += 0.5;
584  }
585 }; // end class
586 
587 } // namespace rviz_visual_tools
588 
589 int main(int argc, char** argv)
590 {
591  ros::init(argc, argv, "visual_tools_demo");
592  ROS_INFO_STREAM("Visual Tools Demo");
593 
594  // Allow the action server to recieve and send ros messages
596  spinner.start();
597 
599 
600  double x_location = 0;
601  demo.testRows(x_location);
602  demo.testSize(x_location, rviz_visual_tools::MEDIUM);
603  demo.testSize(x_location, rviz_visual_tools::LARGE);
604  demo.testSizes(x_location);
605 
606  ROS_INFO_STREAM("Shutting down.");
607 
608  return 0;
609 }
void testSize(double &x_location, scales scale)
Compare sizes of markers using all MEDIUM-scale markers.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
void publishLabelHelper(const Eigen::Affine3d &pose, const std::string &label)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_INFO_STREAM_NAMED(name, args)
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void spinner()
std::shared_ptr< RvizVisualTools > RvizVisualToolsPtr
void testSizes(double &x_location)
Compare every size range.
#define ROS_INFO(...)
unsigned int step
#define ROS_INFO_STREAM(args)


rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Mon Jun 10 2019 15:05:18