rviz_visual_tools.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 <dave@dav.ee>, Andy McEvoy
36  Desc: Helper functions for displaying basic shape markers in Rviz
37 */
38 
39 #include <rviz_visual_tools/rviz_visual_tools.h>
40 
41 // Conversions
42 #include <eigen_conversions/eigen_msg.h>
43 #include <tf_conversions/tf_eigen.h>
44 
45 // C++
46 #include <cmath> // for random poses
47 #include <set>
48 #include <string>
49 #include <utility>
50 #include <vector>
51 
52 namespace rviz_visual_tools
53 {
54 const std::string RvizVisualTools::name_ = "visual_tools";
55 
56 const std::array<colors, 14> RvizVisualTools::all_rand_colors_ = { RED, GREEN, BLUE, GREY, DARK_GREY,
57  WHITE, ORANGE, YELLOW, BROWN, PINK,
58  LIME_GREEN, PURPLE, CYAN, MAGENTA };
59 
60 RvizVisualTools::RvizVisualTools(std::string base_frame, std::string marker_topic, ros::NodeHandle nh)
61  : nh_(nh), marker_topic_(std::move(marker_topic)), base_frame_(std::move(base_frame))
62 {
63  initialize();
64 }
65 
66 void RvizVisualTools::initialize()
67 {
68  marker_lifetime_ = ros::Duration(0.0); // 0 - unlimited
69 
70  // Cache the reusable markers
71  loadRvizMarkers();
72 }
73 
74 bool RvizVisualTools::deleteAllMarkers()
75 {
76  // Helper for publishing rviz markers
77  return publishMarker(reset_marker_);
78 }
79 
80 void RvizVisualTools::resetMarkerCounts()
81 {
82  arrow_marker_.id++;
83  sphere_marker_.id++;
84  block_marker_.id++;
85  cylinder_marker_.id++;
86  mesh_marker_.id++;
87  text_marker_.id++;
88  cuboid_marker_.id++;
89  line_strip_marker_.id++;
90  line_list_marker_.id++;
91  spheres_marker_.id++;
92  triangle_marker_.id++;
93 }
94 
95 bool RvizVisualTools::loadRvizMarkers()
96 {
97  // Load reset marker -------------------------------------------------
98  reset_marker_.header.frame_id = base_frame_;
99  reset_marker_.header.stamp = ros::Time();
100  reset_marker_.ns = "deleteAllMarkers"; // helps during debugging
101  reset_marker_.action = 3; // TODO(davetcoleman): In ROS-J set to visualization_msgs::Marker::DELETEALL;
102  reset_marker_.pose.orientation.w = 1;
103 
104  // Load arrow ----------------------------------------------------
105 
106  arrow_marker_.header.frame_id = base_frame_;
107  // Set the namespace and id for this marker. This serves to create a unique
108  // ID
109  arrow_marker_.ns = "Arrow";
110  // Set the marker type.
111  arrow_marker_.type = visualization_msgs::Marker::ARROW;
112  // Set the marker action. Options are ADD and DELETE
113  arrow_marker_.action = visualization_msgs::Marker::ADD;
114  // Lifetime
115  arrow_marker_.lifetime = marker_lifetime_;
116 
117  // Load cuboid ----------------------------------------------------
118 
119  cuboid_marker_.header.frame_id = base_frame_;
120  // Set the namespace and id for this marker. This serves to create a unique
121  // ID
122  cuboid_marker_.ns = "Cuboid";
123  // Set the marker type.
124  cuboid_marker_.type = visualization_msgs::Marker::CUBE;
125  // Set the marker action. Options are ADD and DELETE
126  cuboid_marker_.action = visualization_msgs::Marker::ADD;
127  // Lifetime
128  cuboid_marker_.lifetime = marker_lifetime_;
129 
130  // Load line ----------------------------------------------------
131 
132  line_strip_marker_.header.frame_id = base_frame_;
133  // Set the namespace and id for this marker. This serves to create a unique
134  // ID
135  line_strip_marker_.ns = "Line";
136  // Set the marker type.
137  line_strip_marker_.type = visualization_msgs::Marker::LINE_STRIP;
138  // Set the marker action. Options are ADD and DELETE
139  line_strip_marker_.action = visualization_msgs::Marker::ADD;
140  // Lifetime
141  line_strip_marker_.lifetime = marker_lifetime_;
142 
143  // Load path ----------------------------------------------------
144 
145  line_list_marker_.header.frame_id = base_frame_;
146  // Set the namespace and id for this marker. This serves to create a unique
147  // ID
148  line_list_marker_.ns = "Line_List";
149  // Set the marker type.
150  line_list_marker_.type = visualization_msgs::Marker::LINE_LIST;
151  // Set the marker action. Options are ADD and DELETE
152  line_list_marker_.action = visualization_msgs::Marker::ADD;
153  // Lifetime
154  line_list_marker_.lifetime = marker_lifetime_;
155  // Constants
156  line_list_marker_.pose.position.x = 0.0;
157  line_list_marker_.pose.position.y = 0.0;
158  line_list_marker_.pose.position.z = 0.0;
159 
160  line_list_marker_.pose.orientation.x = 0.0;
161  line_list_marker_.pose.orientation.y = 0.0;
162  line_list_marker_.pose.orientation.z = 0.0;
163  line_list_marker_.pose.orientation.w = 1.0;
164 
165  // Load sphers ----------------------------------------------------
166 
167  spheres_marker_.header.frame_id = base_frame_;
168  // Set the namespace and id for this marker. This serves to create a unique
169  // ID
170  spheres_marker_.ns = "Spheres";
171  // Set the marker type.
172  spheres_marker_.type = visualization_msgs::Marker::SPHERE_LIST;
173  // Set the marker action. Options are ADD and DELETE
174  spheres_marker_.action = visualization_msgs::Marker::ADD;
175  // Lifetime
176  spheres_marker_.lifetime = marker_lifetime_;
177  // Constants
178  spheres_marker_.pose.position.x = 0.0;
179  spheres_marker_.pose.position.y = 0.0;
180  spheres_marker_.pose.position.z = 0.0;
181 
182  spheres_marker_.pose.orientation.x = 0.0;
183  spheres_marker_.pose.orientation.y = 0.0;
184  spheres_marker_.pose.orientation.z = 0.0;
185  spheres_marker_.pose.orientation.w = 1.0;
186 
187  // Load Block ----------------------------------------------------
188  block_marker_.header.frame_id = base_frame_;
189  // Set the namespace and id for this marker. This serves to create a unique
190  // ID
191  block_marker_.ns = "Block";
192  // Set the marker action. Options are ADD and DELETE
193  block_marker_.action = visualization_msgs::Marker::ADD;
194  // Set the marker type.
195  block_marker_.type = visualization_msgs::Marker::CUBE;
196  // Lifetime
197  block_marker_.lifetime = marker_lifetime_;
198 
199  // Load Cylinder ----------------------------------------------------
200  cylinder_marker_.header.frame_id = base_frame_;
201  // Set the marker action. Options are ADD and DELETE
202  cylinder_marker_.action = visualization_msgs::Marker::ADD;
203  // Set the marker type.
204  cylinder_marker_.type = visualization_msgs::Marker::CYLINDER;
205  // Lifetime
206  cylinder_marker_.lifetime = marker_lifetime_;
207 
208  // Load Mesh ----------------------------------------------------
209  mesh_marker_.header.frame_id = base_frame_;
210 
211  // Set the marker action. Options are ADD and DELETE
212  mesh_marker_.action = visualization_msgs::Marker::ADD;
213  // Set the marker type.
214  mesh_marker_.type = visualization_msgs::Marker::MESH_RESOURCE;
215  // Lifetime
216  mesh_marker_.lifetime = marker_lifetime_;
217 
218  // Load Sphere -------------------------------------------------
219  sphere_marker_.header.frame_id = base_frame_;
220  // Set the namespace and id for this marker. This serves to create a unique
221  // ID
222  sphere_marker_.ns = "Sphere";
223  // Set the marker type.
224  sphere_marker_.type = visualization_msgs::Marker::SPHERE;
225  // Set the marker action. Options are ADD and DELETE
226  sphere_marker_.action = visualization_msgs::Marker::ADD;
227  // Marker group position and orientation
228  sphere_marker_.pose.position.x = 0;
229  sphere_marker_.pose.position.y = 0;
230  sphere_marker_.pose.position.z = 0;
231  sphere_marker_.pose.orientation.x = 0.0;
232  sphere_marker_.pose.orientation.y = 0.0;
233  sphere_marker_.pose.orientation.z = 0.0;
234  sphere_marker_.pose.orientation.w = 1.0;
235  // Create a sphere point
236  // Add the point pair to the line message
237  sphere_marker_.points.resize(1);
238  sphere_marker_.colors.resize(1);
239  // Lifetime
240  sphere_marker_.lifetime = marker_lifetime_;
241 
242  // Load Text ----------------------------------------------------
243  // Set the namespace and id for this marker. This serves to create a unique
244  // ID
245  text_marker_.ns = "Text";
246  // Set the marker action. Options are ADD and DELETE
247  text_marker_.action = visualization_msgs::Marker::ADD;
248  // Set the marker type.
249  text_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
250  // Lifetime
251  text_marker_.lifetime = marker_lifetime_;
252 
253  // Load Triangle List -------------------------------------------
254  // Set the namespace and id for this marker. This serves to create a unique ID
255  triangle_marker_.header.frame_id = base_frame_;
256  triangle_marker_.ns = "Triangle";
257  // Set the marker action. Options are ADD and DELETE
258  triangle_marker_.action = visualization_msgs::Marker::ADD;
259  // Set the marker type
260  triangle_marker_.type = visualization_msgs::Marker::TRIANGLE_LIST;
261  // Lifetime
262  triangle_marker_.lifetime = marker_lifetime_;
263 
264  return true;
265 }
266 
267 void RvizVisualTools::loadMarkerPub(bool wait_for_subscriber, bool latched)
268 {
269  if (pub_rviz_markers_ != nullptr)
270  {
271  return;
272  }
273 
274  // Rviz marker publisher
275  pub_rviz_markers_ = nh_.advertise<visualization_msgs::MarkerArray>(marker_topic_, 10, latched);
276  ROS_DEBUG_STREAM_NAMED(name_, "Publishing Rviz markers on topic " << pub_rviz_markers_.getTopic());
277 
278  if (wait_for_subscriber)
279  {
280  waitForSubscriber(pub_rviz_markers_);
281  }
282 }
283 
284 void RvizVisualTools::waitForMarkerPub()
285 {
286  bool blocking = true;
287  waitForSubscriber(pub_rviz_markers_, 0, blocking);
288 }
289 
290 void RvizVisualTools::waitForMarkerPub(double wait_time)
291 {
292  bool blocking = false;
293  waitForSubscriber(pub_rviz_markers_, wait_time, blocking);
294 }
295 
296 bool RvizVisualTools::waitForSubscriber(const ros::Publisher& pub, double wait_time, bool blocking)
297 {
298  // Will wait at most this amount of time
299  ros::Time max_time(ros::Time::now() + ros::Duration(wait_time));
300 
301  // This is wrong. It returns only the number of subscribers that have already
302  // established their direct connections to this publisher
303  int num_existing_subscribers = pub.getNumSubscribers();
304 
305  // How often to check for subscribers
306  ros::Rate poll_rate(200);
307 
308  if (pub.getTopic().empty())
309  {
310  ROS_ERROR_STREAM_NAMED(name_, "loadMarkerPub() has not been called yet, unable to wait for subscriber.");
311  }
312 
313  if (blocking && num_existing_subscribers == 0)
314  {
315  ROS_INFO_STREAM_NAMED(name_, "Topic '" << pub.getTopic() << "' waiting for subscriber...");
316  }
317 
318  // Wait for subscriber
319  while (num_existing_subscribers == 0)
320  {
321  if (!blocking && ros::Time::now() > max_time) // Check if timed out
322  {
323  ROS_WARN_STREAM_NAMED(name_, "Topic '" << pub.getTopic() << "' unable to connect to any subscribers within "
324  << wait_time << " sec. It is possible initially published visual messages "
325  "will be lost.");
326  return false;
327  }
328  ros::spinOnce();
329 
330  // Sleep
331  poll_rate.sleep();
332 
333  // Check again
334  num_existing_subscribers = pub.getNumSubscribers();
335 
336  if (!ros::ok())
337  {
338  return false;
339  }
340  }
341  pub_rviz_markers_connected_ = true;
342 
343  return true;
344 }
345 
346 void RvizVisualTools::setLifetime(double lifetime)
347 {
348  marker_lifetime_ = ros::Duration(lifetime);
349 
350  // Update cached markers
351  arrow_marker_.lifetime = marker_lifetime_;
352  cuboid_marker_.lifetime = marker_lifetime_;
353  line_strip_marker_.lifetime = marker_lifetime_;
354  sphere_marker_.lifetime = marker_lifetime_;
355  block_marker_.lifetime = marker_lifetime_;
356  mesh_marker_.lifetime = marker_lifetime_;
357  cylinder_marker_.lifetime = marker_lifetime_;
358  text_marker_.lifetime = marker_lifetime_;
359 }
360 
361 colors RvizVisualTools::getRandColor()
362 {
363  const int rand_num = iRand(0, all_rand_colors_.size() - 1);
364  return all_rand_colors_[rand_num];
365 }
366 
367 std_msgs::ColorRGBA RvizVisualTools::getColor(colors color) const
368 {
369  std_msgs::ColorRGBA result;
370 
371  switch (color)
372  {
373  case RED:
374  result.r = 0.8;
375  result.g = 0.1;
376  result.b = 0.1;
377  result.a = alpha_;
378  break;
379  case GREEN:
380  result.r = 0.1;
381  result.g = 0.8;
382  result.b = 0.1;
383  result.a = alpha_;
384  break;
385  case GREY:
386  result.r = 0.9;
387  result.g = 0.9;
388  result.b = 0.9;
389  result.a = alpha_;
390  break;
391  case DARK_GREY:
392  result.r = 0.6;
393  result.g = 0.6;
394  result.b = 0.6;
395  result.a = alpha_;
396  break;
397  case WHITE:
398  result.r = 1.0;
399  result.g = 1.0;
400  result.b = 1.0;
401  result.a = alpha_;
402  break;
403  case ORANGE:
404  result.r = 1.0;
405  result.g = 0.5;
406  result.b = 0.0;
407  result.a = alpha_;
408  break;
409  case TRANSLUCENT_LIGHT:
410  result.r = 0.1;
411  result.g = 0.1;
412  result.b = 0.1;
413  result.a = 0.1;
414  break;
415  case TRANSLUCENT:
416  result.r = 0.1;
417  result.g = 0.1;
418  result.b = 0.1;
419  result.a = 0.25;
420  break;
421  case TRANSLUCENT_DARK:
422  result.r = 0.1;
423  result.g = 0.1;
424  result.b = 0.1;
425  result.a = 0.5;
426  break;
427  case BLACK:
428  result.r = 0.0;
429  result.g = 0.0;
430  result.b = 0.0;
431  result.a = alpha_;
432  break;
433  case YELLOW:
434  result.r = 1.0;
435  result.g = 1.0;
436  result.b = 0.0;
437  result.a = alpha_;
438  break;
439  case BROWN:
440  result.r = 0.597;
441  result.g = 0.296;
442  result.b = 0.0;
443  result.a = alpha_;
444  break;
445  case PINK:
446  result.r = 1.0;
447  result.g = 0.4;
448  result.b = 1;
449  result.a = alpha_;
450  break;
451  case LIME_GREEN:
452  result.r = 0.6;
453  result.g = 1.0;
454  result.b = 0.2;
455  result.a = alpha_;
456  break;
457  case CLEAR:
458  result.r = 1.0;
459  result.g = 1.0;
460  result.b = 1.0;
461  result.a = 0.0;
462  break;
463  case PURPLE:
464  result.r = 0.597;
465  result.g = 0.0;
466  result.b = 0.597;
467  result.a = alpha_;
468  break;
469  case CYAN:
470  result.r = 0.0;
471  result.g = 1.0;
472  result.b = 1.0;
473  result.a = alpha_;
474  break;
475  case MAGENTA:
476  result.r = 1.0;
477  result.g = 0.0;
478  result.b = 1.0;
479  result.a = alpha_;
480  break;
481  case RAND:
482  result = createRandColor();
483  break;
484  case DEFAULT:
485  ROS_WARN_STREAM_NAMED(name_, "The 'DEFAULT' color should probably not "
486  "be used with getColor(). Defaulting to "
487  "blue.");
488  case BLUE:
489  default:
490  result.r = 0.1;
491  result.g = 0.1;
492  result.b = 0.8;
493  result.a = alpha_;
494  break;
495  }
496 
497  return result;
498 }
499 
500 colors RvizVisualTools::intToRvizColor(std::size_t color)
501 {
502  // clang-format off
503  switch (color)
504  {
505  case 0: return BLACK; break;
506  case 1: return BROWN; break;
507  case 2: return BLUE; break;
508  case 3: return CYAN; break;
509  case 4: return GREY; break;
510  case 5: return DARK_GREY; break;
511  case 6: return GREEN; break;
512  case 7: return LIME_GREEN; break;
513  case 8: return MAGENTA; break;
514  case 9: return ORANGE; break;
515  case 10: return PURPLE; break;
516  case 11: return RED; break;
517  case 12: return PINK; break;
518  case 13: return WHITE; break;
519  case 14: return YELLOW; break;
520  case 15: return TRANSLUCENT; break;
521  case 16: return TRANSLUCENT_LIGHT; break;
522  case 17: return TRANSLUCENT_DARK; break;
523  case 18: return RAND; break;
524  case 19: return CLEAR; break;
525  case 20: return DEFAULT; break;
526  }
527  // clang-format on
528  return DEFAULT;
529 }
530 
531 scales RvizVisualTools::intToRvizScale(std::size_t scale)
532 {
533  // clang-format off
534  switch (scale)
535  {
536  case 1: return XXXXSMALL; break;
537  case 2: return XXXSMALL; break;
538  case 3: return XXSMALL; break;
539  case 4: return XSMALL; break;
540  case 5: return SMALL; break;
541  case 6: return MEDIUM; break;
542  case 7: return LARGE; break;
543  case 8: return XLARGE; break;
544  case 9: return XXLARGE; break;
545  case 10: return XXXLARGE; break;
546  case 11: return XXXXLARGE; break;
547  default: throw std::runtime_error("Unknown size");
548  }
549  // clang-format on
550  return MEDIUM; // dumy value
551 }
552 
553 std::string RvizVisualTools::scaleToString(scales scale)
554 {
555  // clang-format off
556  switch (scale)
557  {
558  case XXXXSMALL: return "XXXXSMALL"; break;
559  case XXXSMALL: return "XXXSMALL"; break;
560  case XXSMALL: return "XXSMALL"; break;
561  case XSMALL: return "XSMALL"; break;
562  case SMALL: return "SMALL"; break;
563  case MEDIUM: return "MEDIUM"; break;
564  case LARGE: return "LARGE"; break;
565  case XLARGE: return "XLARGE"; break;
566  case XXLARGE: return "XXLARGE"; break;
567  case XXXLARGE: return "XXXLARGE"; break;
568  case XXXXLARGE: return "XXXXLARGE"; break;
569  default: throw std::runtime_error("Unknown size");
570  }
571  // clang-format on
572  return "MEDIUM"; // dumy value
573 }
574 
575 std_msgs::ColorRGBA RvizVisualTools::createRandColor() const
576 {
577  std_msgs::ColorRGBA result;
578 
579  const std::size_t MAX_ATTEMPTS = 20; // bound the performance
580  std::size_t attempts = 0;
581 
582  // Make sure color is not *too* dark
583  do
584  {
585  result.r = fRand(0.0, 1.0);
586  result.g = fRand(0.0, 1.0);
587  result.b = fRand(0.0, 1.0);
588  // ROS_DEBUG_STREAM_NAMED(name_, "Looking for random color that is not too light, current value is "
589  //<< (result.r + result.g + result.b) << " attempt #" << attempts);
590  attempts++;
591  if (attempts > MAX_ATTEMPTS)
592  {
593  ROS_WARN_STREAM_NAMED(name_, "Unable to find appropriate random color after " << MAX_ATTEMPTS << " attempts");
594  break;
595  }
596  } while (result.r + result.g + result.b < 1.5); // 3 would be white
597 
598  // Set alpha value
599  result.a = alpha_;
600 
601  return result;
602 }
603 
604 double RvizVisualTools::slerp(double start, double end, double range, double value)
605 {
606  return start + (((end - start) / range) * value);
607 }
608 
609 std_msgs::ColorRGBA RvizVisualTools::getColorScale(double value) const
610 {
611  // User warning
612  if (value < 0)
613  {
614  ROS_WARN_STREAM_NAMED(name_, "Intensity value for color scale is below range [0,1], value: " << value);
615  value = 0;
616  }
617  else if (value > 1)
618  {
619  ROS_WARN_STREAM_NAMED(name_, "Intensity value for color scale is above range [0,1], value: " << value);
620  value = 1;
621  }
622 
623  std_msgs::ColorRGBA start;
624  std_msgs::ColorRGBA end;
625 
626  // For second half of color range move towards RED
627  if (value == 0.0)
628  {
629  return getColor(RED);
630  }
631  else if (value == 1.0)
632  {
633  return getColor(GREEN);
634  }
635  else if (value <= 0.5)
636  {
637  start = getColor(RED);
638  end = getColor(YELLOW);
639  }
640  else
641  {
642  start = getColor(YELLOW);
643  end = getColor(GREEN);
644  value = fmod(value, 0.5);
645  }
646 
647  std_msgs::ColorRGBA result;
648  result.r = slerp(start.r, end.r, 0.5, value);
649  result.g = slerp(start.g, end.g, 0.5, value);
650  result.b = slerp(start.b, end.b, 0.5, value);
651  result.a = alpha_;
652 
653  return result;
654 }
655 
656 geometry_msgs::Vector3 RvizVisualTools::getScale(scales scale, double marker_scale) const
657 {
658  if (scale == REGULAR)
659  {
660  ROS_WARN_STREAM_ONCE_NAMED(name_, "Scale size 'REGULAR' is deprecated, please switch to 'MEDIUM'");
661  scale = MEDIUM;
662  }
663 
664  geometry_msgs::Vector3 result;
665  double val(0.0);
666  switch (scale)
667  {
668  case XXXXSMALL:
669  val = 0.001;
670  break;
671  case XXXSMALL:
672  val = 0.0025;
673  break;
674  case XXSMALL:
675  val = 0.005;
676  break;
677  case XSMALL:
678  val = 0.0065;
679  break;
680  case SMALL:
681  val = 0.0075;
682  break;
683  case MEDIUM:
684  val = 0.01;
685  break;
686  case LARGE:
687  val = 0.025;
688  break;
689  case XLARGE:
690  val = 0.05;
691  break;
692  case XXLARGE:
693  val = 0.075;
694  break;
695  case XXXLARGE:
696  val = 0.1;
697  break;
698  case XXXXLARGE:
699  val = 0.5;
700  break;
701  default:
702  ROS_ERROR_STREAM_NAMED(name_, "Not implemented yet");
703  break;
704  }
705 
706  // Allows an individual marker size factor and a size factor for all markers
707  result.x = val * marker_scale * global_scale_;
708  result.y = val * marker_scale * global_scale_;
709  result.z = val * marker_scale * global_scale_;
710 
711  return result;
712 }
713 
714 Eigen::Vector3d RvizVisualTools::getCenterPoint(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const
715 {
716  Eigen::Vector3d center;
717  center[0] = (a[0] + b[0]) / 2.0;
718  center[1] = (a[1] + b[1]) / 2.0;
719  center[2] = (a[2] + b[2]) / 2.0;
720  return center;
721 }
722 
723 Eigen::Affine3d RvizVisualTools::getVectorBetweenPoints(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const
724 {
725  bool verbose = false;
726 
727  // TODO(davetcoleman): handle the error case when a & b are the same point.
728  // currently it returns nan for the quaternion
729 
730  // from
731  // http://answers.ros.org/question/31006/how-can-a-vector3-axis-be-used-to-produce-a-quaternion/
732 
733  // Solution pose
734  Eigen::Affine3d result_pose = Eigen::Affine3d::Identity();
735 
736  // Goal pose:
737  Eigen::Quaterniond q;
738  Eigen::Vector3d axis_vector = b - a;
739 
740  if (verbose)
741  {
742  std::cout << std::endl;
743  std::cout << "axis_vector " << std::endl;
744  printTranslation(axis_vector);
745  }
746 
747  axis_vector.normalize();
748 
749  if (verbose)
750  {
751  std::cout << std::endl;
752  std::cout << "axis_vector after normalize " << std::endl;
753  printTranslation(axis_vector);
754  }
755 
756  Eigen::Vector3d up_vector(0.0, 0.0, 1.0);
757  Eigen::Vector3d right_axis_vector = axis_vector.cross(up_vector);
758 
759  if (verbose)
760  {
761  std::cout << "right_axis_vector " << std::endl;
762  printTranslation(right_axis_vector);
763  }
764 
765  if (right_axis_vector[0] == 0.0 && right_axis_vector[1] == 0.0 && right_axis_vector[2] == 0.0)
766  {
767  if (verbose)
768  {
769  std::cout << "right axis vector is zero " << std::endl;
770  }
771  result_pose = Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY());
772  result_pose.translation() = a;
773  return result_pose;
774  }
775 
776  right_axis_vector.normalized();
777 
778  if (verbose)
779  {
780  std::cout << "right_axis_vector normalized " << std::endl;
781  printTranslation(right_axis_vector);
782  }
783 
784  double theta = axis_vector.dot(up_vector);
785  double angle_rotation = -1.0 * acos(theta);
786 
787  //-------------------------------------------
788  // Method 1 - TF - works
789 
790  // Convert vector to TF format
791  tf::Vector3 tf_right_axis_vector;
792  tf::vectorEigenToTF(right_axis_vector, tf_right_axis_vector);
793 
794  // Create quaternion using 'Axis angle Constructor'
795  // axis: The axis which the rotation is around
796  // angle: The magnitude of the rotation around the angle (Radians)
797  tf::Quaternion tf_q(tf_right_axis_vector, angle_rotation);
798 
799  // Convert back to Eigen
800  tf::quaternionTFToEigen(tf_q, q);
801  //-------------------------------------------
802 
803  if (verbose)
804  {
805  std::cout << "rotation matrix: " << std::endl;
806  std::cout << q.toRotationMatrix() << std::endl;
807  }
808 
809  //-------------------------------------------
810  // Method 2 - Eigen - broken TODO(davetcoleman)
811  // q = Eigen::AngleAxis<double>(angle_rotation, right_axis_vector);
812  //-------------------------------------------
813  // std::cout << q.toRotationMatrix() << std::endl;
814 
815  q.normalize();
816 
817  if (verbose)
818  {
819  std::cout << "rotation matrix after normalize: " << std::endl;
820  std::cout << q.toRotationMatrix() << std::endl;
821  }
822 
823  // Rotate so that vector points along line
824  result_pose = q * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY());
825  result_pose.translation() = a;
826 
827  return result_pose;
828 }
829 
830 bool RvizVisualTools::publishMarker(visualization_msgs::Marker& marker)
831 {
832  // Add single marker to array
833  marker.frame_locked = frame_locking_enabled_;
834  markers_.markers.push_back(marker);
835 
836  // Determine if we should publish now
837  if (!batch_publishing_enabled_)
838  {
839  return trigger();
840  }
841 
842  return true;
843 }
844 
845 void RvizVisualTools::enableBatchPublishing(bool enable)
846 {
847  batch_publishing_enabled_ = enable;
848 }
849 
850 void RvizVisualTools::enableFrameLocking(bool enable)
851 {
852  frame_locking_enabled_ = enable;
853 }
854 
855 bool RvizVisualTools::triggerEvery(std::size_t queueSize)
856 {
857  if (markers_.markers.size() >= queueSize || queueSize == 0)
858  {
859  return trigger();
860  }
861  return false;
862 }
863 
864 bool RvizVisualTools::trigger()
865 {
866  if (!batch_publishing_enabled_)
867  {
868  ROS_WARN_STREAM_NAMED(name_, "Batch publishing triggered but it was not enabled (unnecessary function call)");
869  }
870  if (markers_.markers.empty())
871  {
872  // ROS_WARN_STREAM_NAMED(name_, "Batch publishing triggered but queue is empty (unnecessary function call)");
873  return false;
874  }
875 
876  bool result = publishMarkers(markers_);
877 
878  markers_.markers.clear(); // remove all cached markers
879  return result;
880 }
881 
882 bool RvizVisualTools::triggerAndDisable()
883 {
884  trigger();
885  batch_publishing_enabled_ = false;
886  return true;
887 }
888 
889 bool RvizVisualTools::publishMarkers(visualization_msgs::MarkerArray& markers)
890 {
891  if (pub_rviz_markers_ == nullptr)
892  { // always check this before publishing
893  loadMarkerPub();
894  }
895 
896  // Check if connected to a subscriber
897  if (!pub_rviz_markers_waited_ && !pub_rviz_markers_connected_)
898  {
899  ROS_DEBUG_STREAM_NAMED(name_, "Waiting for subscribers before publishing markers...");
900  waitForSubscriber(pub_rviz_markers_);
901 
902  // Only wait for the publisher once, after that just ignore the lack of connection
903  pub_rviz_markers_waited_ = true;
904  }
905 
906  // Check if any actual markers exist to publish
907  if (markers.markers.empty())
908  {
909  return false;
910  }
911 
912  // Change all markers to be inverted in color
913  if (psychedelic_mode_)
914  {
915  for (auto& marker : markers.markers)
916  {
917  marker.color.r = 1.0 - marker.color.r;
918  marker.color.g = 1.0 - marker.color.g;
919  marker.color.b = 1.0 - marker.color.b;
920  for (auto& color : marker.colors)
921  {
922  color.r = 1.0 - color.r;
923  color.g = 1.0 - color.g;
924  color.b = 1.0 - color.b;
925  }
926  }
927  }
928 
929  // Publish
930  pub_rviz_markers_.publish(markers);
931  ros::spinOnce();
932  return true;
933 }
934 
935 bool RvizVisualTools::publishCone(const Eigen::Affine3d& pose, double angle, colors color, double scale)
936 {
937  return publishCone(convertPose(pose), angle, color, scale);
938 }
939 
940 bool RvizVisualTools::publishCone(const geometry_msgs::Pose& pose, double angle, colors color, double scale)
941 {
942  triangle_marker_.header.stamp = ros::Time::now();
943  triangle_marker_.id++;
944 
945  triangle_marker_.color = getColor(color);
946  triangle_marker_.pose = pose;
947 
948  geometry_msgs::Point p[3];
949  static const double delta_theta = M_PI / 16.0;
950  double theta = 0;
951 
952  triangle_marker_.points.clear();
953  for (std::size_t i = 0; i < 32; i++)
954  {
955  p[0].x = 0;
956  p[0].y = 0;
957  p[0].z = 0;
958 
959  p[1].x = scale;
960  p[1].y = scale * cos(theta) / angle;
961  p[1].z = scale * sin(theta) / angle;
962 
963  p[2].x = scale;
964  p[2].y = scale * cos(theta + delta_theta) / angle;
965  p[2].z = scale * sin(theta + delta_theta) / angle;
966 
967  triangle_marker_.points.push_back(p[0]);
968  triangle_marker_.points.push_back(p[1]);
969  triangle_marker_.points.push_back(p[2]);
970 
971  theta += delta_theta;
972  }
973 
974  triangle_marker_.scale.x = 1.0;
975  triangle_marker_.scale.y = 1.0;
976  triangle_marker_.scale.z = 1.0;
977 
978  return publishMarker(triangle_marker_);
979 }
980 
981 bool RvizVisualTools::publishXYPlane(const Eigen::Affine3d& pose, colors color, double scale)
982 {
983  return publishXYPlane(convertPose(pose), color, scale);
984 }
985 
986 bool RvizVisualTools::publishXYPlane(const geometry_msgs::Pose& pose, colors color, double scale)
987 {
988  triangle_marker_.header.stamp = ros::Time::now();
989  triangle_marker_.id++;
990 
991  triangle_marker_.color = getColor(color);
992  triangle_marker_.pose = pose;
993 
994  geometry_msgs::Point p[4];
995  p[0].x = 1.0 * scale;
996  p[0].y = 1.0 * scale;
997  p[0].z = 0.0;
998 
999  p[1].x = -1.0 * scale;
1000  p[1].y = 1.0 * scale;
1001  p[1].z = 0.0;
1002 
1003  p[2].x = -1.0 * scale;
1004  p[2].y = -1.0 * scale;
1005  p[2].z = 0.0;
1006 
1007  p[3].x = 1.0 * scale;
1008  p[3].y = -1.0 * scale;
1009  p[3].z = 0.0;
1010 
1011  triangle_marker_.scale.x = 1.0;
1012  triangle_marker_.scale.y = 1.0;
1013  triangle_marker_.scale.z = 1.0;
1014 
1015  triangle_marker_.points.clear();
1016  triangle_marker_.points.push_back(p[0]);
1017  triangle_marker_.points.push_back(p[1]);
1018  triangle_marker_.points.push_back(p[2]);
1019 
1020  triangle_marker_.points.push_back(p[2]);
1021  triangle_marker_.points.push_back(p[3]);
1022  triangle_marker_.points.push_back(p[0]);
1023 
1024  return publishMarker(triangle_marker_);
1025 }
1026 
1027 bool RvizVisualTools::publishXZPlane(const Eigen::Affine3d& pose, colors color, double scale)
1028 {
1029  return publishXZPlane(convertPose(pose), color, scale);
1030 }
1031 
1032 bool RvizVisualTools::publishXZPlane(const geometry_msgs::Pose& pose, colors color, double scale)
1033 {
1034  triangle_marker_.header.stamp = ros::Time::now();
1035  triangle_marker_.id++;
1036 
1037  triangle_marker_.color = getColor(color);
1038  triangle_marker_.pose = pose;
1039 
1040  geometry_msgs::Point p[4];
1041  p[0].x = 1.0 * scale;
1042  p[0].y = 0;
1043  p[0].z = 1.0 * scale;
1044 
1045  p[1].x = -1.0 * scale;
1046  p[1].y = 0;
1047  p[1].z = 1.0 * scale;
1048 
1049  p[2].x = -1.0 * scale;
1050  p[2].y = 0;
1051  p[2].z = -1.0 * scale;
1052 
1053  p[3].x = 1.0 * scale;
1054  p[3].y = 0;
1055  p[3].z = -1.0 * scale;
1056 
1057  triangle_marker_.scale.x = 1.0;
1058  triangle_marker_.scale.y = 1.0;
1059  triangle_marker_.scale.z = 1.0;
1060 
1061  triangle_marker_.points.clear();
1062  triangle_marker_.points.push_back(p[0]);
1063  triangle_marker_.points.push_back(p[1]);
1064  triangle_marker_.points.push_back(p[2]);
1065 
1066  triangle_marker_.points.push_back(p[2]);
1067  triangle_marker_.points.push_back(p[3]);
1068  triangle_marker_.points.push_back(p[0]);
1069 
1070  return publishMarker(triangle_marker_);
1071 }
1072 
1073 bool RvizVisualTools::publishYZPlane(const Eigen::Affine3d& pose, colors color, double scale)
1074 {
1075  return publishYZPlane(convertPose(pose), color, scale);
1076 }
1077 
1078 bool RvizVisualTools::publishYZPlane(const geometry_msgs::Pose& pose, colors color, double scale)
1079 {
1080  triangle_marker_.header.stamp = ros::Time::now();
1081  triangle_marker_.id++;
1082 
1083  triangle_marker_.color = getColor(color);
1084  triangle_marker_.pose = pose;
1085 
1086  geometry_msgs::Point p[4];
1087  p[0].x = 0;
1088  p[0].y = 1.0 * scale;
1089  p[0].z = 1.0 * scale;
1090 
1091  p[1].x = 0;
1092  p[1].y = -1.0 * scale;
1093  p[1].z = 1.0 * scale;
1094 
1095  p[2].x = 0;
1096  p[2].y = -1.0 * scale;
1097  p[2].z = -1.0 * scale;
1098 
1099  p[3].x = 0;
1100  p[3].y = 1.0 * scale;
1101  p[3].z = -1.0 * scale;
1102 
1103  triangle_marker_.scale.x = 1.0;
1104  triangle_marker_.scale.y = 1.0;
1105  triangle_marker_.scale.z = 1.0;
1106 
1107  triangle_marker_.points.clear();
1108  triangle_marker_.points.push_back(p[0]);
1109  triangle_marker_.points.push_back(p[1]);
1110  triangle_marker_.points.push_back(p[2]);
1111 
1112  triangle_marker_.points.push_back(p[2]);
1113  triangle_marker_.points.push_back(p[3]);
1114  triangle_marker_.points.push_back(p[0]);
1115 
1116  return publishMarker(triangle_marker_);
1117 }
1118 
1119 bool RvizVisualTools::publishSphere(const Eigen::Affine3d& pose, colors color, scales scale, const std::string& ns,
1120  std::size_t id)
1121 {
1122  return publishSphere(convertPose(pose), color, scale, ns, id);
1123 }
1124 
1125 bool RvizVisualTools::publishSphere(const Eigen::Vector3d& point, colors color, scales scale, const std::string& ns,
1126  std::size_t id)
1127 {
1128  geometry_msgs::Pose pose_msg;
1129  tf::pointEigenToMsg(point, pose_msg.position);
1130  return publishSphere(pose_msg, color, scale, ns, id);
1131 }
1132 
1133 bool RvizVisualTools::publishSphere(const Eigen::Vector3d& point, colors color, double scale, const std::string& ns,
1134  std::size_t id)
1135 {
1136  geometry_msgs::Pose pose_msg;
1137  tf::pointEigenToMsg(point, pose_msg.position);
1138  return publishSphere(pose_msg, color, scale, ns, id);
1139 }
1140 
1141 bool RvizVisualTools::publishSphere(const geometry_msgs::Point& point, colors color, scales scale,
1142  const std::string& ns, std::size_t id)
1143 {
1144  geometry_msgs::Pose pose_msg;
1145  pose_msg.position = point;
1146  return publishSphere(pose_msg, color, scale, ns, id);
1147 }
1148 
1149 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose& pose, colors color, scales scale, const std::string& ns,
1150  std::size_t id)
1151 {
1152  return publishSphere(pose, color, getScale(scale), ns, id);
1153 }
1154 
1155 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose& pose, colors color, double scale, const std::string& ns,
1156  std::size_t id)
1157 {
1158  geometry_msgs::Vector3 scale_msg;
1159  scale_msg.x = scale * global_scale_;
1160  scale_msg.y = scale * global_scale_;
1161  scale_msg.z = scale * global_scale_;
1162  return publishSphere(pose, color, scale_msg, ns, id);
1163 }
1164 
1165 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose& pose, colors color, const geometry_msgs::Vector3 scale,
1166  const std::string& ns, std::size_t id)
1167 {
1168  return publishSphere(pose, getColor(color), scale, ns, id);
1169 }
1170 
1171 bool RvizVisualTools::publishSphere(const Eigen::Affine3d& pose, const std_msgs::ColorRGBA& color,
1172  const geometry_msgs::Vector3 scale, const std::string& ns, std::size_t id)
1173 {
1174  return publishSphere(convertPose(pose), color, scale, ns, id);
1175 }
1176 
1177 bool RvizVisualTools::publishSphere(const Eigen::Vector3d& point, const std_msgs::ColorRGBA& color,
1178  const geometry_msgs::Vector3 scale, const std::string& ns, std::size_t id)
1179 {
1180  geometry_msgs::Pose pose_msg;
1181  tf::pointEigenToMsg(point, pose_msg.position);
1182  return publishSphere(pose_msg, color, scale, ns, id);
1183 }
1184 
1185 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color,
1186  const geometry_msgs::Vector3 scale, const std::string& ns, std::size_t id)
1187 {
1188  // Set the frame ID and timestamp
1189  sphere_marker_.header.stamp = ros::Time::now();
1190 
1191  // Overwrite ID or increment?
1192  if (id == 0)
1193  {
1194  sphere_marker_.id++;
1195  }
1196  else
1197  {
1198  sphere_marker_.id = id;
1199  }
1200 
1201  sphere_marker_.pose = pose;
1202  sphere_marker_.color = color;
1203  sphere_marker_.scale = scale;
1204  sphere_marker_.ns = ns;
1205 
1206  // Helper for publishing rviz markers
1207  return publishMarker(sphere_marker_);
1208 }
1209 
1210 bool RvizVisualTools::publishSphere(const geometry_msgs::PoseStamped& pose, colors color,
1211  const geometry_msgs::Vector3 scale, const std::string& ns, std::size_t id)
1212 {
1213  // Set the frame ID and timestamp
1214  sphere_marker_.header = pose.header;
1215 
1216  if (id == 0)
1217  {
1218  sphere_marker_.id++;
1219  }
1220  else
1221  {
1222  sphere_marker_.id = id;
1223  }
1224 
1225  sphere_marker_.pose = pose.pose;
1226  sphere_marker_.color = getColor(color);
1227  sphere_marker_.scale = scale;
1228  sphere_marker_.ns = ns;
1229 
1230  // Helper for publishing rviz markers
1231  publishMarker(sphere_marker_);
1232 
1233  sphere_marker_.header.frame_id = base_frame_; // restore default frame
1234  return true;
1235 }
1236 
1237 bool RvizVisualTools::publishXArrow(const Eigen::Affine3d& pose, colors color, scales scale, double length)
1238 {
1239  return publishArrow(convertPose(pose), color, scale, length);
1240 }
1241 
1242 bool RvizVisualTools::publishXArrow(const geometry_msgs::Pose& pose, colors color, scales scale, double length)
1243 {
1244  return publishArrow(pose, color, scale, length);
1245 }
1246 
1247 bool RvizVisualTools::publishXArrow(const geometry_msgs::PoseStamped& pose, colors color, scales scale, double length)
1248 {
1249  return publishArrow(pose, color, scale, length);
1250 }
1251 
1252 bool RvizVisualTools::publishYArrow(const Eigen::Affine3d& pose, colors color, scales scale, double length)
1253 {
1254  Eigen::Affine3d arrow_pose = pose * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
1255  return publishArrow(convertPose(arrow_pose), color, scale, length);
1256 }
1257 
1258 bool RvizVisualTools::publishYArrow(const geometry_msgs::Pose& pose, colors color, scales scale, double length)
1259 {
1260  Eigen::Affine3d arrow_pose = convertPose(pose) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
1261  return publishArrow(convertPose(arrow_pose), color, scale, length);
1262 }
1263 
1264 bool RvizVisualTools::publishYArrow(const geometry_msgs::PoseStamped& pose, colors color, scales scale, double length)
1265 {
1266  Eigen::Affine3d arrow_pose = convertPose(pose.pose) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
1267  geometry_msgs::PoseStamped new_pose = pose;
1268  new_pose.pose = convertPose(arrow_pose);
1269  return publishArrow(new_pose, color, scale, length);
1270 }
1271 
1272 bool RvizVisualTools::publishZArrow(const Eigen::Affine3d& pose, colors color, scales scale, double length,
1273  std::size_t id)
1274 {
1275  Eigen::Affine3d arrow_pose = pose * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1276  return publishArrow(convertPose(arrow_pose), color, scale, length, id);
1277 }
1278 
1279 bool RvizVisualTools::publishZArrow(const geometry_msgs::Pose& pose, colors color, scales scale, double length)
1280 {
1281  Eigen::Affine3d arrow_pose = convertPose(pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1282  return publishArrow(convertPose(arrow_pose), color, scale, length);
1283 }
1284 
1285 bool RvizVisualTools::publishZArrow(const geometry_msgs::PoseStamped& pose, colors color, scales scale, double length)
1286 {
1287  Eigen::Affine3d arrow_pose = convertPose(pose.pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1288  geometry_msgs::PoseStamped new_pose = pose;
1289  new_pose.pose = convertPose(arrow_pose);
1290  return publishArrow(new_pose, color, scale, length);
1291 }
1292 
1293 bool RvizVisualTools::publishZArrow(const geometry_msgs::PoseStamped& pose, colors color, scales scale, double length,
1294  std::size_t id)
1295 {
1296  Eigen::Affine3d arrow_pose = convertPose(pose.pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1297  geometry_msgs::PoseStamped new_pose = pose;
1298  new_pose.pose = convertPose(arrow_pose);
1299  return publishArrow(new_pose, color, scale, length, id);
1300 }
1301 
1302 bool RvizVisualTools::publishArrow(const Eigen::Affine3d& pose, colors color, scales scale, double length,
1303  std::size_t id)
1304 {
1305  return publishArrow(convertPose(pose), color, scale, length, id);
1306 }
1307 
1308 bool RvizVisualTools::publishArrow(const geometry_msgs::Pose& pose, colors color, scales scale, double length,
1309  std::size_t id)
1310 {
1311  // Set the frame ID and timestamp.
1312  arrow_marker_.header.stamp = ros::Time::now();
1313  arrow_marker_.header.frame_id = base_frame_;
1314 
1315  if (id == 0)
1316  {
1317  arrow_marker_.id++;
1318  }
1319  else
1320  {
1321  arrow_marker_.id = id;
1322  }
1323 
1324  arrow_marker_.pose = pose;
1325  arrow_marker_.color = getColor(color);
1326  arrow_marker_.scale = getScale(scale);
1327 
1328  // override previous x scale specified
1329  if (length == 0)
1330  { // auto set the scale
1331  arrow_marker_.scale.x *= 10.0;
1332  }
1333  else
1334  {
1335  arrow_marker_.scale.x = length;
1336  }
1337 
1338  // Helper for publishing rviz markers
1339  return publishMarker(arrow_marker_);
1340 }
1341 
1342 bool RvizVisualTools::publishArrow(const geometry_msgs::PoseStamped& pose, colors color, scales scale, double length,
1343  std::size_t id)
1344 {
1345  // Set the frame ID and timestamp.
1346  arrow_marker_.header = pose.header;
1347 
1348  if (id == 0)
1349  {
1350  arrow_marker_.id++;
1351  }
1352  else
1353  {
1354  arrow_marker_.id = id;
1355  }
1356 
1357  arrow_marker_.pose = pose.pose;
1358  arrow_marker_.color = getColor(color);
1359  arrow_marker_.scale = getScale(scale);
1360 
1361  // override previous x scale specified
1362  if (length == 0)
1363  { // auto set the scale
1364  arrow_marker_.scale.x *= 10.0;
1365  }
1366  else
1367  {
1368  arrow_marker_.scale.x = length;
1369  }
1370 
1371  // Helper for publishing rviz markers
1372  publishMarker(arrow_marker_);
1373 
1374  arrow_marker_.header.frame_id = base_frame_; // restore default frame
1375  return true;
1376 }
1377 
1378 bool RvizVisualTools::publishArrow(const geometry_msgs::Point& start, const geometry_msgs::Point& end, colors color,
1379  scales scale, std::size_t id)
1380 {
1381  // Set the frame ID and timestamp.
1382  arrow_marker_.header.stamp = ros::Time::now();
1383  arrow_marker_.header.frame_id = base_frame_;
1384 
1385  if (id == 0)
1386  {
1387  arrow_marker_.id++;
1388  }
1389  else
1390  {
1391  arrow_marker_.id = id;
1392  }
1393 
1394  arrow_marker_.points.clear();
1395  arrow_marker_.points.push_back(start);
1396  arrow_marker_.points.push_back(end);
1397  arrow_marker_.color = getColor(color);
1398  arrow_marker_.scale = getScale(scale);
1399 
1400  // override previous y & z scale specified
1401  // scale.x is the shaft diameter
1402  // scale.y is the head diameter
1403  // scale.z it specifies the head length.
1404  arrow_marker_.scale.y *= 2.0;
1405  arrow_marker_.scale.z *= 3.0;
1406 
1407  // Helper for publishing rviz markers
1408  return publishMarker(arrow_marker_);
1409 }
1410 
1411 bool RvizVisualTools::publishAxisLabeled(const Eigen::Affine3d& pose, const std::string& label, scales scale,
1412  colors color)
1413 {
1414  return publishAxisLabeled(convertPose(pose), label, scale, color);
1415 }
1416 
1417 bool RvizVisualTools::publishAxisLabeled(const geometry_msgs::Pose& pose, const std::string& label, scales scale,
1418  colors color)
1419 {
1420  publishAxis(pose, scale, label);
1421 
1422  // For avoiding overriden Axis and Text
1423  geometry_msgs::Pose pose_shifted = pose;
1424  pose_shifted.position.x -= 0.05;
1425  pose_shifted.position.y -= 0.05;
1426  pose_shifted.position.z -= 0.05;
1427  publishText(pose_shifted, label, color, static_cast<scales>(static_cast<int>(scale) + 1), false);
1428  return true;
1429 }
1430 
1431 bool RvizVisualTools::publishAxis(const geometry_msgs::Pose& pose, scales scale, const std::string& ns)
1432 {
1433  double radius = getScale(scale).x;
1434  return publishAxis(pose, radius * 10.0, radius, ns);
1435 }
1436 
1437 bool RvizVisualTools::publishAxis(const Eigen::Affine3d& pose, scales scale, const std::string& ns)
1438 {
1439  double radius = getScale(scale).x;
1440  return publishAxis(pose, radius * 10.0, radius, ns);
1441 }
1442 
1443 bool RvizVisualTools::publishAxis(const geometry_msgs::Pose& pose, double length, double radius, const std::string& ns)
1444 {
1445  return publishAxis(convertPose(pose), length, radius, ns);
1446 }
1447 
1448 bool RvizVisualTools::publishAxis(const Eigen::Affine3d& pose, double length, double radius, const std::string& ns)
1449 {
1450  // Use an internal function that will not actually publish anything, so that other makers can combine with an axis
1451  // without publishing
1452  publishAxisInternal(pose, length, radius, ns);
1453 
1454  return true;
1455 }
1456 
1457 bool RvizVisualTools::publishAxisInternal(const Eigen::Affine3d& pose, double length, double radius,
1458  const std::string& ns)
1459 {
1460  // Publish x axis
1461  Eigen::Affine3d x_pose =
1462  Eigen::Translation3d(length / 2.0, 0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY());
1463  x_pose = pose * x_pose;
1464  publishCylinder(x_pose, RED, length, radius, ns);
1465 
1466  // Publish y axis
1467  Eigen::Affine3d y_pose =
1468  Eigen::Translation3d(0, length / 2.0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX());
1469  y_pose = pose * y_pose;
1470  publishCylinder(y_pose, GREEN, length, radius, ns);
1471 
1472  // Publish z axis
1473  Eigen::Affine3d z_pose = Eigen::Translation3d(0, 0, length / 2.0) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
1474  z_pose = pose * z_pose;
1475  publishCylinder(z_pose, BLUE, length, radius, ns);
1476 
1477  return true;
1478 }
1479 
1480 bool RvizVisualTools::publishAxisPath(const EigenSTL::vector_Affine3d& path, scales scale, const std::string& ns)
1481 {
1482  // Create the cylinders
1483  for (const auto& i : path)
1484  {
1485  double radius = getScale(scale).x;
1486  publishAxisInternal(i, radius * 10.0, radius, ns);
1487  }
1488 
1489  return true;
1490 }
1491 
1492 bool RvizVisualTools::publishAxisPath(const EigenSTL::vector_Affine3d& path, double length, double radius,
1493  const std::string& ns)
1494 {
1495  // Create the cylinders
1496  for (const auto& i : path)
1497  {
1498  publishAxisInternal(i, length, radius, ns);
1499  }
1500 
1501  return true;
1502 }
1503 
1504 bool RvizVisualTools::publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color,
1505  scales scale, const std::string& ns)
1506 {
1507  double radius = getScale(scale).x;
1508  return publishCylinder(point1, point2, getColor(color), radius, ns);
1509 }
1510 
1511 bool RvizVisualTools::publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color,
1512  double radius, const std::string& ns)
1513 {
1514  return publishCylinder(point1, point2, getColor(color), radius, ns);
1515 }
1516 
1517 bool RvizVisualTools::publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
1518  const std_msgs::ColorRGBA& color, double radius, const std::string& ns)
1519 {
1520  // Distance between two points
1521  double height = (point1 - point2).lpNorm<2>();
1522 
1523  // Find center point
1524  Eigen::Vector3d pt_center = getCenterPoint(point1, point2);
1525 
1526  // Create vector
1527  Eigen::Affine3d pose;
1528  pose = getVectorBetweenPoints(pt_center, point2);
1529 
1530  // Convert pose to be normal to cylindar axis
1531  Eigen::Affine3d rotation;
1532  rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
1533  pose = pose * rotation;
1534 
1535  // Turn into msg
1536  return publishCylinder(convertPose(pose), color, height, radius, ns);
1537 }
1538 
1539 bool RvizVisualTools::publishCylinder(const Eigen::Affine3d& pose, colors color, double height, double radius,
1540  const std::string& ns)
1541 {
1542  return publishCylinder(convertPose(pose), color, height, radius, ns);
1543 }
1544 
1545 bool RvizVisualTools::publishCylinder(const geometry_msgs::Pose& pose, colors color, double height, double radius,
1546  const std::string& ns)
1547 {
1548  return publishCylinder(pose, getColor(color), height, radius, ns);
1549 }
1550 
1551 bool RvizVisualTools::publishCylinder(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color, double height,
1552  double radius, const std::string& ns)
1553 {
1554  // Set the timestamp
1555  cylinder_marker_.header.stamp = ros::Time::now();
1556  cylinder_marker_.ns = ns;
1557  cylinder_marker_.id++;
1558 
1559  // Set the pose
1560  cylinder_marker_.pose = pose;
1561 
1562  // Set marker size
1563  cylinder_marker_.scale.x = radius;
1564  cylinder_marker_.scale.y = radius;
1565  cylinder_marker_.scale.z = height;
1566 
1567  // Set marker color
1568  cylinder_marker_.color = color;
1569 
1570  // Helper for publishing rviz markers
1571  return publishMarker(cylinder_marker_);
1572 }
1573 
1574 bool RvizVisualTools::publishMesh(const Eigen::Affine3d& pose, const std::string& file_name, colors color, double scale,
1575  const std::string& ns, std::size_t id)
1576 {
1577  return publishMesh(convertPose(pose), file_name, color, scale, ns, id);
1578 }
1579 
1580 bool RvizVisualTools::publishMesh(const geometry_msgs::Pose& pose, const std::string& file_name, colors color,
1581  double scale, const std::string& ns, std::size_t id)
1582 {
1583  // Set the timestamp
1584  mesh_marker_.header.stamp = ros::Time::now();
1585 
1586  if (id == 0)
1587  {
1588  mesh_marker_.id++;
1589  }
1590  else
1591  {
1592  mesh_marker_.id = id;
1593  }
1594 
1595  // Set the mesh
1596  mesh_marker_.mesh_resource = file_name;
1597  mesh_marker_.mesh_use_embedded_materials = 1u;
1598 
1599  // Set the pose
1600  mesh_marker_.pose = pose;
1601 
1602  // Set marker size
1603  mesh_marker_.scale.x = scale;
1604  mesh_marker_.scale.y = scale;
1605  mesh_marker_.scale.z = scale;
1606 
1607  // Set the namespace and id for this marker. This serves to create a unique
1608  // ID
1609  mesh_marker_.ns = ns;
1610 
1611  // Set marker color
1612  mesh_marker_.color = getColor(color);
1613 
1614  // Helper for publishing rviz markers
1615  return publishMarker(mesh_marker_);
1616 }
1617 
1618 bool RvizVisualTools::publishGraph(const graph_msgs::GeometryGraph& graph, colors color, double radius)
1619 {
1620  // Track which pairs of nodes we've already connected since graph is
1621  // bi-directional
1622  typedef std::pair<std::size_t, std::size_t> node_ids;
1623  std::set<node_ids> added_edges;
1624  std::pair<std::set<node_ids>::iterator, bool> return_value;
1625  Eigen::Vector3d a, b;
1626  for (std::size_t i = 0; i < graph.nodes.size(); ++i)
1627  {
1628  for (std::size_t j = 0; j < graph.edges[i].node_ids.size(); ++j)
1629  {
1630  // Check if we've already added this pair of nodes (edge)
1631  return_value = added_edges.insert(node_ids(i, j));
1632  if (!return_value.second)
1633  {
1634  // Element already existed in set, so don't add a new collision object
1635  }
1636  else
1637  {
1638  // Create a cylinder from two points
1639  a = convertPoint(graph.nodes[i]);
1640  b = convertPoint(graph.nodes[graph.edges[i].node_ids[j]]);
1641 
1642  publishCylinder(a, b, color, radius);
1643  }
1644  }
1645  }
1646 
1647  return true;
1648 }
1649 
1650 bool RvizVisualTools::publishCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color)
1651 {
1652  return publishCuboid(convertPoint(point1), convertPoint(point2), color);
1653 }
1654 
1655 bool RvizVisualTools::publishCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
1656  colors color, const std::string& ns, std::size_t id)
1657 {
1658  // Set the timestamp
1659  cuboid_marker_.header.stamp = ros::Time::now();
1660  cuboid_marker_.ns = ns;
1661 
1662  if (id == 0)
1663  { // Provide a new id every call to this function
1664  cuboid_marker_.id++;
1665  }
1666  else
1667  { // allow marker to be overwritten
1668  cuboid_marker_.id = id;
1669  }
1670 
1671  cuboid_marker_.color = getColor(color);
1672 
1673  // Calculate center pose
1674  geometry_msgs::Pose pose;
1675  pose.position.x = (point1.x - point2.x) / 2.0 + point2.x;
1676  pose.position.y = (point1.y - point2.y) / 2.0 + point2.y;
1677  pose.position.z = (point1.z - point2.z) / 2.0 + point2.z;
1678  cuboid_marker_.pose = pose;
1679 
1680  // Calculate scale
1681  cuboid_marker_.scale.x = fabs(point1.x - point2.x);
1682  cuboid_marker_.scale.y = fabs(point1.y - point2.y);
1683  cuboid_marker_.scale.z = fabs(point1.z - point2.z);
1684 
1685  // Prevent scale from being zero
1686  if (cuboid_marker_.scale.x == 0.0)
1687  {
1688  cuboid_marker_.scale.x = SMALL_SCALE;
1689  }
1690  if (cuboid_marker_.scale.y == 0.0)
1691  {
1692  cuboid_marker_.scale.y = SMALL_SCALE;
1693  }
1694  if (cuboid_marker_.scale.z == 0.0)
1695  {
1696  cuboid_marker_.scale.z = SMALL_SCALE;
1697  }
1698 
1699  // Helper for publishing rviz markers
1700  return publishMarker(cuboid_marker_);
1701 }
1702 
1703 bool RvizVisualTools::publishCuboid(const Eigen::Affine3d& pose, double depth, double width, double height,
1704  colors color)
1705 {
1706  return publishCuboid(convertPose(pose), depth, width, height, color);
1707 }
1708 
1709 bool RvizVisualTools::publishCuboid(const geometry_msgs::Pose& pose, double depth, double width, double height,
1710  colors color)
1711 {
1712  cuboid_marker_.header.stamp = ros::Time::now();
1713 
1714  cuboid_marker_.id++;
1715  cuboid_marker_.color = getColor(color);
1716 
1717  cuboid_marker_.pose = pose;
1718 
1719  // Prevent scale from being zero
1720  if (depth <= 0)
1721  {
1722  cuboid_marker_.scale.x = SMALL_SCALE;
1723  }
1724  else
1725  {
1726  cuboid_marker_.scale.x = depth;
1727  }
1728 
1729  if (width <= 0)
1730  {
1731  cuboid_marker_.scale.y = SMALL_SCALE;
1732  }
1733  else
1734  {
1735  cuboid_marker_.scale.y = width;
1736  }
1737 
1738  if (height <= 0)
1739  {
1740  cuboid_marker_.scale.z = SMALL_SCALE;
1741  }
1742  else
1743  {
1744  cuboid_marker_.scale.z = height;
1745  }
1746 
1747  return publishMarker(cuboid_marker_);
1748 }
1749 
1750 bool RvizVisualTools::publishLine(const Eigen::Affine3d& point1, const Eigen::Affine3d& point2, colors color,
1751  scales scale)
1752 {
1753  return publishLine(convertPoseToPoint(point1), convertPoseToPoint(point2), color, scale);
1754 }
1755 
1756 bool RvizVisualTools::publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color,
1757  scales scale)
1758 {
1759  return publishLine(convertPoint(point1), convertPoint(point2), color, scale);
1760 }
1761 bool RvizVisualTools::publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color,
1762  double radius)
1763 {
1764  geometry_msgs::Vector3 scale;
1765  scale.x = radius * global_scale_;
1766  scale.y = radius * global_scale_;
1767  scale.z = radius * global_scale_;
1768  return publishLine(convertPoint(point1), convertPoint(point2), getColor(color), scale);
1769 }
1770 
1771 bool RvizVisualTools::publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
1772  const std_msgs::ColorRGBA& color, scales scale)
1773 {
1774  return publishLine(convertPoint(point1), convertPoint(point2), color, scale);
1775 }
1776 
1777 bool RvizVisualTools::publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
1778  const std_msgs::ColorRGBA& color, double radius)
1779 {
1780  geometry_msgs::Vector3 scale;
1781  scale.x = radius * global_scale_;
1782  scale.y = radius * global_scale_;
1783  scale.z = radius * global_scale_;
1784  return publishLine(convertPoint(point1), convertPoint(point2), color, scale);
1785 }
1786 
1787 bool RvizVisualTools::publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2, colors color,
1788  scales scale)
1789 {
1790  return publishLine(point1, point2, getColor(color), scale);
1791 }
1792 
1793 bool RvizVisualTools::publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
1794  const std_msgs::ColorRGBA& color, scales scale)
1795 {
1796  return publishLine(point1, point2, color, getScale(scale));
1797 }
1798 
1799 bool RvizVisualTools::publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
1800  const std_msgs::ColorRGBA& color, const geometry_msgs::Vector3& scale)
1801 {
1802  // Set the timestamp
1803  line_strip_marker_.header.stamp = ros::Time::now();
1804 
1805  line_strip_marker_.id++;
1806  line_strip_marker_.color = color;
1807  line_strip_marker_.scale = scale;
1808 
1809  line_strip_marker_.points.clear();
1810  line_strip_marker_.points.push_back(point1);
1811  line_strip_marker_.points.push_back(point2);
1812 
1813  // Helper for publishing rviz markers
1814  return publishMarker(line_strip_marker_);
1815 }
1816 
1817 bool RvizVisualTools::publishLines(const EigenSTL::vector_Vector3d& aPoints, const EigenSTL::vector_Vector3d& bPoints,
1818  const std::vector<colors>& colors, scales scale)
1819 {
1820  BOOST_ASSERT_MSG(aPoints.size() == bPoints.size() && bPoints.size() == colors.size(), "Mismatching vector sizes: "
1821  "aPoints, bPoints, and colors");
1822 
1823  std::vector<geometry_msgs::Point> aPoints_msg;
1824  std::vector<geometry_msgs::Point> bPoints_msg;
1825  std::vector<std_msgs::ColorRGBA> colors_msg;
1826 
1827  for (std::size_t i = 0; i < aPoints.size(); ++i)
1828  {
1829  // Convert Eigen to ROS Msg
1830  aPoints_msg.push_back(convertPoint(aPoints[i]));
1831  bPoints_msg.push_back(convertPoint(bPoints[i]));
1832  // Convert color to ROS Msg
1833  colors_msg.push_back(getColor(colors[i]));
1834  }
1835  BOOST_ASSERT_MSG(aPoints_msg.size() == bPoints_msg.size() && bPoints_msg.size() == colors_msg.size(), "Mismatched "
1836  "vector sizes");
1837 
1838  return publishLines(aPoints_msg, bPoints_msg, colors_msg, getScale(scale));
1839 }
1840 
1841 bool RvizVisualTools::publishLines(const std::vector<geometry_msgs::Point>& aPoints,
1842  const std::vector<geometry_msgs::Point>& bPoints,
1843  const std::vector<std_msgs::ColorRGBA>& colors, const geometry_msgs::Vector3& scale)
1844 {
1845  // Setup marker
1846  line_list_marker_.header.stamp = ros::Time();
1847  line_list_marker_.ns = "Line Array";
1848 
1849  // Provide a new id every call to this function
1850  line_list_marker_.id++;
1851 
1852  line_list_marker_.scale = scale;
1853  // line_list_marker_.color = getColor(BLUE); // This var is not used
1854 
1855  // Add each point pair to the line message
1856  line_list_marker_.points.clear();
1857  line_list_marker_.colors.clear();
1858  for (std::size_t i = 0; i < aPoints.size(); ++i)
1859  {
1860  line_list_marker_.points.push_back(aPoints[i]);
1861  line_list_marker_.points.push_back(bPoints[i]);
1862  line_list_marker_.colors.push_back(colors[i]);
1863  line_list_marker_.colors.push_back(colors[i]);
1864  }
1865 
1866  // Testing
1867  BOOST_ASSERT_MSG(line_list_marker_.colors.size() == line_list_marker_.points.size(), "Arrays mismatch in size");
1868  BOOST_ASSERT_MSG(line_list_marker_.colors.size() == aPoints.size() * 2, "Colors arrays mismatch in size");
1869 
1870  // Helper for publishing rviz markers
1871  return publishMarker(line_list_marker_);
1872 }
1873 
1874 bool RvizVisualTools::publishLineStrip(const std::vector<geometry_msgs::Point>& path, colors color, scales scale,
1875  const std::string& ns)
1876 {
1877  if (path.size() < 2)
1878  {
1879  ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
1880  return true;
1881  }
1882 
1883  line_strip_marker_.header.stamp = ros::Time();
1884  line_strip_marker_.ns = ns;
1885 
1886  // Provide a new id every call to this function
1887  line_strip_marker_.id++;
1888 
1889  std_msgs::ColorRGBA this_color = getColor(color);
1890  line_strip_marker_.scale = getScale(scale);
1891  line_strip_marker_.color = this_color;
1892  line_strip_marker_.points.clear();
1893  line_strip_marker_.colors.clear();
1894 
1895  for (std::size_t i = 1; i < path.size(); ++i)
1896  {
1897  // Add the point pair to the line message
1898  line_strip_marker_.points.push_back(path[i - 1]);
1899  line_strip_marker_.points.push_back(path[i]);
1900  line_strip_marker_.colors.push_back(this_color);
1901  line_strip_marker_.colors.push_back(this_color);
1902  }
1903 
1904  // Helper for publishing rviz markers
1905  return publishMarker(line_strip_marker_);
1906 }
1907 
1908 bool RvizVisualTools::publishPath(const std::vector<geometry_msgs::Pose>& path, colors color, scales scale,
1909  const std::string& ns)
1910 {
1911  std::vector<geometry_msgs::Point> point_path(path.size());
1912  for (std::size_t i = 0; i < path.size(); ++i)
1913  {
1914  point_path[i] = path[i].position;
1915  }
1916 
1917  return publishPath(point_path, color, getScale(scale).x, ns);
1918 }
1919 
1920 bool RvizVisualTools::publishPath(const std::vector<geometry_msgs::Point>& path, colors color, scales scale,
1921  const std::string& ns)
1922 {
1923  return publishPath(path, color, getScale(scale).x, ns);
1924 }
1925 
1926 bool RvizVisualTools::publishPath(const EigenSTL::vector_Affine3d& path, colors color, scales scale,
1927  const std::string& ns)
1928 {
1929  return publishPath(path, color, getScale(scale).x, ns);
1930 }
1931 
1932 bool RvizVisualTools::publishPath(const EigenSTL::vector_Vector3d& path, colors color, scales scale,
1933  const std::string& ns)
1934 {
1935  return publishPath(path, color, getScale(scale).x, ns);
1936 }
1937 
1938 bool RvizVisualTools::publishPath(const std::vector<geometry_msgs::Point>& path, colors color, double radius,
1939  const std::string& ns)
1940 {
1941  if (path.size() < 2)
1942  {
1943  ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
1944  return true;
1945  }
1946 
1947  // Create the cylinders
1948  for (std::size_t i = 1; i < path.size(); ++i)
1949  {
1950  publishCylinder(convertPoint(path[i - 1]), convertPoint(path[i]), color, radius, ns);
1951  }
1952 
1953  return true;
1954 }
1955 
1956 bool RvizVisualTools::publishPath(const EigenSTL::vector_Vector3d& path, colors color, double radius,
1957  const std::string& ns)
1958 {
1959  if (path.size() < 2)
1960  {
1961  ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
1962  return true;
1963  }
1964 
1965  // Create the cylinders
1966  for (std::size_t i = 1; i < path.size(); ++i)
1967  {
1968  publishCylinder(path[i - 1], path[i], color, radius, ns);
1969  }
1970 
1971  return true;
1972 }
1973 
1974 bool RvizVisualTools::publishPath(const EigenSTL::vector_Affine3d& path, colors color, double radius,
1975  const std::string& ns)
1976 {
1977  if (path.size() < 2)
1978  {
1979  ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
1980  return true;
1981  }
1982 
1983  // Create the cylinders
1984  for (std::size_t i = 1; i < path.size(); ++i)
1985  {
1986  publishCylinder(path[i - 1].translation(), path[i].translation(), color, radius, ns);
1987  }
1988 
1989  return true;
1990 }
1991 
1992 bool RvizVisualTools::publishPath(const EigenSTL::vector_Vector3d& path, const std::vector<colors>& colors,
1993  double radius, const std::string& ns)
1994 {
1995  if (path.size() < 2)
1996  {
1997  ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
1998  return true;
1999  }
2000 
2001  if (path.size() != colors.size())
2002  {
2003  ROS_ERROR_STREAM_NAMED(name_, "Skipping path because " << path.size() << " different from " << colors.size()
2004  << ".");
2005  return false;
2006  }
2007 
2008  // Create the cylinders
2009  for (std::size_t i = 1; i < path.size(); ++i)
2010  {
2011  publishCylinder(path[i - 1], path[i], colors[i], radius, ns);
2012  }
2013 
2014  return true;
2015 }
2016 
2017 bool RvizVisualTools::publishPath(const EigenSTL::vector_Vector3d& path, const std::vector<std_msgs::ColorRGBA>& colors,
2018  double radius, const std::string& ns)
2019 {
2020  if (path.size() < 2)
2021  {
2022  ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
2023  return true;
2024  }
2025 
2026  if (path.size() != colors.size())
2027  {
2028  ROS_ERROR_STREAM_NAMED(name_, "Skipping path because " << path.size() << " different from " << colors.size()
2029  << ".");
2030  return false;
2031  }
2032 
2033  // Create the cylinders
2034  for (std::size_t i = 1; i < path.size(); ++i)
2035  {
2036  publishCylinder(path[i - 1], path[i], colors[i], radius, ns);
2037  }
2038 
2039  return true;
2040 }
2041 
2042 bool RvizVisualTools::publishPolygon(const geometry_msgs::Polygon& polygon, colors color, scales scale,
2043  const std::string& ns)
2044 {
2045  std::vector<geometry_msgs::Point> points;
2046  geometry_msgs::Point temp;
2047  geometry_msgs::Point first; // remember first point because we will connect
2048  // first and last points
2049  // for last line
2050  for (std::size_t i = 0; i < polygon.points.size(); ++i)
2051  {
2052  temp.x = polygon.points[i].x;
2053  temp.y = polygon.points[i].y;
2054  temp.z = polygon.points[i].z;
2055  if (i == 0)
2056  {
2057  first = temp;
2058  }
2059  points.push_back(temp);
2060  }
2061  points.push_back(first); // connect first and last points for last line
2062 
2063  return publishPath(points, color, scale, ns);
2064 }
2065 
2066 bool RvizVisualTools::publishWireframeCuboid(const Eigen::Affine3d& pose, double depth, double width, double height,
2067  colors color, const std::string& ns, std::size_t id)
2068 {
2069  Eigen::Vector3d min_point, max_point;
2070  min_point << -depth / 2, -width / 2, -height / 2;
2071  max_point << depth / 2, width / 2, height / 2;
2072  return publishWireframeCuboid(pose, min_point, max_point, color, ns, id);
2073 }
2074 
2075 bool RvizVisualTools::publishWireframeCuboid(const Eigen::Affine3d& pose, const Eigen::Vector3d& min_point,
2076  const Eigen::Vector3d& max_point, colors color, const std::string& ns,
2077  std::size_t id)
2078 {
2079  // Extract 8 cuboid vertices
2080  Eigen::Vector3d p1(min_point[0], min_point[1], min_point[2]);
2081  Eigen::Vector3d p2(min_point[0], min_point[1], max_point[2]);
2082  Eigen::Vector3d p3(max_point[0], min_point[1], max_point[2]);
2083  Eigen::Vector3d p4(max_point[0], min_point[1], min_point[2]);
2084  Eigen::Vector3d p5(min_point[0], max_point[1], min_point[2]);
2085  Eigen::Vector3d p6(min_point[0], max_point[1], max_point[2]);
2086  Eigen::Vector3d p7(max_point[0], max_point[1], max_point[2]);
2087  Eigen::Vector3d p8(max_point[0], max_point[1], min_point[2]);
2088 
2089  p1 = pose * p1;
2090  p2 = pose * p2;
2091  p3 = pose * p3;
2092  p4 = pose * p4;
2093  p5 = pose * p5;
2094  p6 = pose * p6;
2095  p7 = pose * p7;
2096  p8 = pose * p8;
2097 
2098  // Setup marker
2099  line_list_marker_.header.stamp = ros::Time();
2100  line_list_marker_.ns = ns;
2101 
2102  if (id == 0)
2103  { // Provide a new id every call to this function
2104  line_list_marker_.id++;
2105  }
2106  else
2107  { // allow marker to be overwritten
2108  line_list_marker_.id = id;
2109  }
2110 
2111  std_msgs::ColorRGBA this_color = getColor(color);
2112  line_list_marker_.scale = getScale(XXSMALL);
2113  line_list_marker_.color = this_color;
2114  line_list_marker_.points.clear();
2115  line_list_marker_.colors.clear();
2116 
2117  // Add each point pair to the line message
2118  line_list_marker_.points.push_back(convertPoint(p1));
2119  line_list_marker_.points.push_back(convertPoint(p2));
2120  line_list_marker_.colors.push_back(this_color);
2121  line_list_marker_.colors.push_back(this_color);
2122 
2123  line_list_marker_.points.push_back(convertPoint(p1));
2124  line_list_marker_.points.push_back(convertPoint(p4));
2125  line_list_marker_.colors.push_back(this_color);
2126  line_list_marker_.colors.push_back(this_color);
2127 
2128  line_list_marker_.points.push_back(convertPoint(p1));
2129  line_list_marker_.points.push_back(convertPoint(p5));
2130  line_list_marker_.colors.push_back(this_color);
2131  line_list_marker_.colors.push_back(this_color);
2132 
2133  line_list_marker_.points.push_back(convertPoint(p5));
2134  line_list_marker_.points.push_back(convertPoint(p6));
2135  line_list_marker_.colors.push_back(this_color);
2136  line_list_marker_.colors.push_back(this_color);
2137 
2138  line_list_marker_.points.push_back(convertPoint(p5));
2139  line_list_marker_.points.push_back(convertPoint(p8));
2140  line_list_marker_.colors.push_back(this_color);
2141  line_list_marker_.colors.push_back(this_color);
2142 
2143  line_list_marker_.points.push_back(convertPoint(p2));
2144  line_list_marker_.points.push_back(convertPoint(p6));
2145  line_list_marker_.colors.push_back(this_color);
2146  line_list_marker_.colors.push_back(this_color);
2147 
2148  line_list_marker_.points.push_back(convertPoint(p6));
2149  line_list_marker_.points.push_back(convertPoint(p7));
2150  line_list_marker_.colors.push_back(this_color);
2151  line_list_marker_.colors.push_back(this_color);
2152 
2153  line_list_marker_.points.push_back(convertPoint(p7));
2154  line_list_marker_.points.push_back(convertPoint(p8));
2155  line_list_marker_.colors.push_back(this_color);
2156  line_list_marker_.colors.push_back(this_color);
2157 
2158  line_list_marker_.points.push_back(convertPoint(p2));
2159  line_list_marker_.points.push_back(convertPoint(p3));
2160  line_list_marker_.colors.push_back(this_color);
2161  line_list_marker_.colors.push_back(this_color);
2162 
2163  line_list_marker_.points.push_back(convertPoint(p4));
2164  line_list_marker_.points.push_back(convertPoint(p8));
2165  line_list_marker_.colors.push_back(this_color);
2166  line_list_marker_.colors.push_back(this_color);
2167 
2168  line_list_marker_.points.push_back(convertPoint(p3));
2169  line_list_marker_.points.push_back(convertPoint(p4));
2170  line_list_marker_.colors.push_back(this_color);
2171  line_list_marker_.colors.push_back(this_color);
2172 
2173  line_list_marker_.points.push_back(convertPoint(p3));
2174  line_list_marker_.points.push_back(convertPoint(p7));
2175  line_list_marker_.colors.push_back(this_color);
2176  line_list_marker_.colors.push_back(this_color);
2177 
2178  // Helper for publishing rviz markers
2179  return publishMarker(line_list_marker_);
2180 }
2181 
2182 bool RvizVisualTools::publishWireframeRectangle(const Eigen::Affine3d& pose, double height, double width, colors color,
2183  scales scale, std::size_t id)
2184 {
2185  if (id == 0)
2186  { // Provide a new id every call to this function
2187  line_list_marker_.id++;
2188  }
2189  else
2190  { // allow marker to be overwritten
2191  line_list_marker_.id = id;
2192  }
2193 
2194  // Extract 8 cuboid vertices
2195  Eigen::Vector3d p1(-width / 2.0, -height / 2.0, 0.0);
2196  Eigen::Vector3d p2(-width / 2.0, height / 2.0, 0.0);
2197  Eigen::Vector3d p3(width / 2.0, height / 2.0, 0.0);
2198  Eigen::Vector3d p4(width / 2.0, -height / 2.0, 0.0);
2199 
2200  p1 = pose * p1;
2201  p2 = pose * p2;
2202  p3 = pose * p3;
2203  p4 = pose * p4;
2204 
2205  // Setup marker
2206  line_list_marker_.header.stamp = ros::Time();
2207  line_list_marker_.ns = "Wireframe Rectangle";
2208 
2209  std_msgs::ColorRGBA this_color = getColor(color);
2210  line_list_marker_.scale = getScale(scale, 0.25);
2211  line_list_marker_.color = this_color;
2212  line_list_marker_.points.clear();
2213  line_list_marker_.colors.clear();
2214 
2215  // Add each point pair to the line message
2216  line_list_marker_.points.push_back(convertPoint(p1));
2217  line_list_marker_.points.push_back(convertPoint(p2));
2218  line_list_marker_.colors.push_back(this_color);
2219  line_list_marker_.colors.push_back(this_color);
2220 
2221  line_list_marker_.points.push_back(convertPoint(p2));
2222  line_list_marker_.points.push_back(convertPoint(p3));
2223  line_list_marker_.colors.push_back(this_color);
2224  line_list_marker_.colors.push_back(this_color);
2225 
2226  line_list_marker_.points.push_back(convertPoint(p3));
2227  line_list_marker_.points.push_back(convertPoint(p4));
2228  line_list_marker_.colors.push_back(this_color);
2229  line_list_marker_.colors.push_back(this_color);
2230 
2231  line_list_marker_.points.push_back(convertPoint(p4));
2232  line_list_marker_.points.push_back(convertPoint(p1));
2233  line_list_marker_.colors.push_back(this_color);
2234  line_list_marker_.colors.push_back(this_color);
2235 
2236  // Helper for publishing rviz markers
2237  return publishMarker(line_list_marker_);
2238 }
2239 
2240 bool RvizVisualTools::publishWireframeRectangle(const Eigen::Affine3d& pose, const Eigen::Vector3d& p1_in,
2241  const Eigen::Vector3d& p2_in, const Eigen::Vector3d& p3_in,
2242  const Eigen::Vector3d& p4_in, colors color, scales scale)
2243 {
2244  Eigen::Vector3d p1;
2245  Eigen::Vector3d p2;
2246  Eigen::Vector3d p3;
2247  Eigen::Vector3d p4;
2248 
2249  // Transform to pose
2250  p1 = pose * p1_in;
2251  p2 = pose * p2_in;
2252  p3 = pose * p3_in;
2253  p4 = pose * p4_in;
2254 
2255  // Setup marker
2256  line_list_marker_.header.stamp = ros::Time();
2257  line_list_marker_.ns = "Wireframe Rectangle";
2258 
2259  // Provide a new id every call to this function
2260  line_list_marker_.id++;
2261 
2262  std_msgs::ColorRGBA this_color = getColor(color);
2263  line_list_marker_.scale = getScale(scale, 0.25);
2264  line_list_marker_.color = this_color;
2265  line_list_marker_.points.clear();
2266  line_list_marker_.colors.clear();
2267 
2268  // Add each point pair to the line message
2269  line_list_marker_.points.push_back(convertPoint(p1));
2270  line_list_marker_.points.push_back(convertPoint(p2));
2271  line_list_marker_.colors.push_back(this_color);
2272  line_list_marker_.colors.push_back(this_color);
2273 
2274  line_list_marker_.points.push_back(convertPoint(p2));
2275  line_list_marker_.points.push_back(convertPoint(p3));
2276  line_list_marker_.colors.push_back(this_color);
2277  line_list_marker_.colors.push_back(this_color);
2278 
2279  line_list_marker_.points.push_back(convertPoint(p3));
2280  line_list_marker_.points.push_back(convertPoint(p4));
2281  line_list_marker_.colors.push_back(this_color);
2282  line_list_marker_.colors.push_back(this_color);
2283 
2284  line_list_marker_.points.push_back(convertPoint(p4));
2285  line_list_marker_.points.push_back(convertPoint(p1));
2286  line_list_marker_.colors.push_back(this_color);
2287  line_list_marker_.colors.push_back(this_color);
2288 
2289  // Helper for publishing rviz markers
2290  return publishMarker(line_list_marker_);
2291 }
2292 
2293 bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, colors color, scales scale,
2294  const std::string& ns)
2295 {
2296  std::vector<geometry_msgs::Point> points_msg;
2297 
2298  for (const auto& point : points)
2299  {
2300  points_msg.push_back(convertPoint(point));
2301  }
2302 
2303  return publishSpheres(points_msg, color, scale, ns);
2304 }
2305 
2306 bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, colors color, double scale,
2307  const std::string& ns)
2308 {
2309  std::vector<geometry_msgs::Point> points_msg;
2310 
2311  for (const auto& point : points)
2312  {
2313  points_msg.push_back(convertPoint(point));
2314  }
2315 
2316  return publishSpheres(points_msg, color, scale, ns);
2317 }
2318 
2319 bool RvizVisualTools::publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color, double scale,
2320  const std::string& ns)
2321 {
2322  geometry_msgs::Vector3 scale_vector;
2323  scale_vector.x = scale * global_scale_;
2324  scale_vector.y = scale * global_scale_;
2325  scale_vector.z = scale * global_scale_;
2326  return publishSpheres(points, color, scale_vector, ns);
2327 }
2328 
2329 bool RvizVisualTools::publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color, scales scale,
2330  const std::string& ns)
2331 {
2332  return publishSpheres(points, color, getScale(scale), ns);
2333 }
2334 
2335 bool RvizVisualTools::publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color,
2336  const geometry_msgs::Vector3& scale, const std::string& ns)
2337 {
2338  spheres_marker_.header.stamp = ros::Time();
2339  spheres_marker_.ns = ns;
2340 
2341  // Provide a new id every call to this function
2342  spheres_marker_.id++;
2343 
2344  std_msgs::ColorRGBA this_color = getColor(color);
2345  spheres_marker_.scale = scale;
2346  spheres_marker_.color = this_color;
2347  spheres_marker_.colors.clear();
2348 
2349  spheres_marker_.points = points; // straight copy
2350 
2351  // Convert path coordinates
2352  for (std::size_t i = 0; i < points.size(); ++i)
2353  {
2354  spheres_marker_.colors.push_back(this_color);
2355  }
2356 
2357  // Helper for publishing rviz markers
2358  return publishMarker(spheres_marker_);
2359 }
2360 
2361 bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, const std::vector<colors>& colors,
2362  scales scale, const std::string& ns)
2363 {
2364  BOOST_ASSERT_MSG(points.size() == colors.size(), "Mismatching vector sizes: points and colors");
2365 
2366  std::vector<geometry_msgs::Point> points_msg;
2367  std::vector<std_msgs::ColorRGBA> colors_msg;
2368 
2369  for (std::size_t i = 0; i < points.size(); ++i)
2370  {
2371  // Convert Eigen to ROS Msg
2372  points_msg.push_back(convertPoint(points[i]));
2373  // Convert color to ROS Msg
2374  colors_msg.push_back(getColor(colors[i]));
2375  }
2376 
2377  return publishSpheres(points_msg, colors_msg, getScale(scale), ns);
2378 }
2379 
2380 bool RvizVisualTools::publishSpheres(const std::vector<geometry_msgs::Point>& points,
2381  const std::vector<std_msgs::ColorRGBA>& colors,
2382  const geometry_msgs::Vector3& scale, const std::string& ns)
2383 {
2384  spheres_marker_.header.stamp = ros::Time();
2385  spheres_marker_.ns = ns;
2386 
2387  // Provide a new id every call to this function
2388  spheres_marker_.id++;
2389 
2390  spheres_marker_.scale = scale;
2391  spheres_marker_.points = points; // straight copy
2392  spheres_marker_.colors = colors; // straight copy
2393 
2394  // Helper for publishing rviz markers
2395  return publishMarker(spheres_marker_);
2396 }
2397 
2398 bool RvizVisualTools::publishText(const Eigen::Affine3d& pose, const std::string& text, colors color, scales scale,
2399  bool static_id)
2400 {
2401  return publishText(convertPose(pose), text, color, getScale(scale), static_id);
2402 }
2403 
2404 bool RvizVisualTools::publishText(const Eigen::Affine3d& pose, const std::string& text, colors color,
2405  const geometry_msgs::Vector3 scale, bool static_id)
2406 {
2407  return publishText(convertPose(pose), text, color, scale, static_id);
2408 }
2409 
2410 bool RvizVisualTools::publishText(const geometry_msgs::Pose& pose, const std::string& text, colors color, scales scale,
2411  bool static_id)
2412 {
2413  return publishText(pose, text, color, getScale(scale), static_id);
2414 }
2415 
2416 bool RvizVisualTools::publishText(const geometry_msgs::Pose& pose, const std::string& text, colors color,
2417  const geometry_msgs::Vector3 scale, bool static_id)
2418 {
2419  // Save the ID if this is a static ID or keep incrementing ID if not static
2420  double temp_id = text_marker_.id;
2421  if (static_id)
2422  {
2423  text_marker_.id = 0;
2424  }
2425  else
2426  {
2427  text_marker_.id++;
2428  }
2429 
2430  text_marker_.header.stamp = ros::Time::now();
2431  text_marker_.header.frame_id = base_frame_;
2432  text_marker_.text = text;
2433  text_marker_.pose = pose;
2434  text_marker_.color = getColor(color);
2435  text_marker_.scale = scale;
2436 
2437  // Helper for publishing rviz markers
2438  publishMarker(text_marker_);
2439 
2440  // Restore the ID count if needed
2441  if (static_id)
2442  {
2443  text_marker_.id = temp_id;
2444  }
2445 
2446  return true;
2447 }
2448 
2449 geometry_msgs::Pose RvizVisualTools::convertPose(const Eigen::Affine3d& pose)
2450 {
2451  geometry_msgs::Pose pose_msg;
2452  tf::poseEigenToMsg(pose, pose_msg);
2453  return pose_msg;
2454 }
2455 
2456 void RvizVisualTools::convertPoseSafe(const Eigen::Affine3d& pose, geometry_msgs::Pose& pose_msg)
2457 {
2458  tf::poseEigenToMsg(pose, pose_msg);
2459 }
2460 
2461 Eigen::Affine3d RvizVisualTools::convertPose(const geometry_msgs::Pose& pose)
2462 {
2463  Eigen::Affine3d shared_pose_eigen_;
2464  tf::poseMsgToEigen(pose, shared_pose_eigen_);
2465  return shared_pose_eigen_;
2466 }
2467 
2468 void RvizVisualTools::convertPoseSafe(const geometry_msgs::Pose& pose_msg, Eigen::Affine3d& pose)
2469 {
2470  tf::poseMsgToEigen(pose_msg, pose);
2471 }
2472 
2473 Eigen::Affine3d RvizVisualTools::convertPoint32ToPose(const geometry_msgs::Point32& point)
2474 {
2475  Eigen::Affine3d shared_pose_eigen_;
2476  shared_pose_eigen_ = Eigen::Affine3d::Identity();
2477  shared_pose_eigen_.translation().x() = point.x;
2478  shared_pose_eigen_.translation().y() = point.y;
2479  shared_pose_eigen_.translation().z() = point.z;
2480  return shared_pose_eigen_;
2481 }
2482 
2483 geometry_msgs::Pose RvizVisualTools::convertPointToPose(const geometry_msgs::Point& point)
2484 {
2485  geometry_msgs::Pose pose_msg;
2486  pose_msg.orientation.x = 0.0;
2487  pose_msg.orientation.y = 0.0;
2488  pose_msg.orientation.z = 0.0;
2489  pose_msg.orientation.w = 1.0;
2490  pose_msg.position = point;
2491  return pose_msg;
2492 }
2493 
2494 Eigen::Affine3d RvizVisualTools::convertPointToPose(const Eigen::Vector3d& point)
2495 {
2496  Eigen::Affine3d shared_pose_eigen_;
2497  shared_pose_eigen_ = Eigen::Affine3d::Identity();
2498  shared_pose_eigen_.translation() = point;
2499  return shared_pose_eigen_;
2500 }
2501 
2502 geometry_msgs::Point RvizVisualTools::convertPoseToPoint(const Eigen::Affine3d& pose)
2503 {
2504  geometry_msgs::Pose pose_msg;
2505  tf::poseEigenToMsg(pose, pose_msg);
2506  return pose_msg.position;
2507 }
2508 
2509 Eigen::Vector3d RvizVisualTools::convertPoint(const geometry_msgs::Point& point)
2510 {
2511  Eigen::Vector3d point_eigen;
2512  point_eigen[0] = point.x;
2513  point_eigen[1] = point.y;
2514  point_eigen[2] = point.z;
2515  return point_eigen;
2516 }
2517 
2518 Eigen::Vector3d RvizVisualTools::convertPoint32(const geometry_msgs::Point32& point)
2519 {
2520  Eigen::Vector3d point_eigen;
2521  point_eigen[0] = point.x;
2522  point_eigen[1] = point.y;
2523  point_eigen[2] = point.z;
2524  return point_eigen;
2525 }
2526 
2527 geometry_msgs::Point32 RvizVisualTools::convertPoint32(const Eigen::Vector3d& point)
2528 {
2529  geometry_msgs::Point32 point32_msg;
2530  point32_msg.x = point[0];
2531  point32_msg.y = point[1];
2532  point32_msg.z = point[2];
2533  return point32_msg;
2534 }
2535 
2536 geometry_msgs::Point RvizVisualTools::convertPoint(const geometry_msgs::Vector3& point)
2537 {
2538  geometry_msgs::Point point_msg;
2539  point_msg.x = point.x;
2540  point_msg.y = point.y;
2541  point_msg.z = point.z;
2542  return point_msg;
2543 }
2544 
2545 geometry_msgs::Point RvizVisualTools::convertPoint(const Eigen::Vector3d& point)
2546 {
2547  geometry_msgs::Point point_msg;
2548  point_msg.x = point.x();
2549  point_msg.y = point.y();
2550  point_msg.z = point.z();
2551  return point_msg;
2552 }
2553 
2554 Eigen::Affine3d RvizVisualTools::convertFromXYZRPY(const std::vector<double>& transform6)
2555 {
2556  return convertFromXYZRPY(std::move(transform6), XYZ);
2557 }
2558 
2559 Eigen::Affine3d RvizVisualTools::convertFromXYZRPY(double x, double y, double z, double roll, double pitch, double yaw)
2560 {
2561  // R-P-Y / X-Y-Z / 0-1-2 Euler Angle Standard
2562  Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
2563  Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
2564  Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
2565  Eigen::Quaternion<double> quaternion = roll_angle * pitch_angle * yaw_angle;
2566 
2567  return Eigen::Translation3d(x, y, z) * quaternion;
2568 }
2569 
2570 Eigen::Affine3d RvizVisualTools::convertFromXYZRPY(double tx, double ty, double tz, double rx, double ry, double rz,
2571  EulerConvention convention)
2572 {
2573  Eigen::Affine3d result;
2574 
2575  switch (convention)
2576  {
2577  case XYZ:
2578  result = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) *
2579  Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
2580  break;
2581 
2582  case ZYX:
2583  result = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) *
2584  Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX());
2585  break;
2586 
2587  case ZXZ:
2588  result = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) *
2589  Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
2590  break;
2591 
2592  default:
2593  ROS_ERROR_STREAM("Invalid euler convention entry " << convention);
2594  break;
2595  }
2596 
2597  return result;
2598 }
2599 
2600 Eigen::Affine3d RvizVisualTools::convertFromXYZRPY(const std::vector<double>& transform6, EulerConvention convention)
2601 {
2602  if (transform6.size() != 6)
2603  {
2604  ROS_ERROR_STREAM_NAMED(name_, "Incorrect number of variables passed for 6-size transform");
2605  throw;
2606  }
2607 
2608  return convertFromXYZRPY(transform6[0], transform6[1], transform6[2], transform6[3], transform6[4], transform6[5],
2609  convention);
2610 }
2611 
2612 void RvizVisualTools::convertToXYZRPY(const Eigen::Affine3d& pose, std::vector<double>& xyzrpy)
2613 {
2614  xyzrpy.resize(6);
2615  convertToXYZRPY(pose, xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4], xyzrpy[5]);
2616 }
2617 
2618 void RvizVisualTools::convertToXYZRPY(const Eigen::Affine3d& pose, double& x, double& y, double& z, double& roll,
2619  double& pitch, double& yaw)
2620 {
2621  x = pose(0, 3);
2622  y = pose(1, 3);
2623  z = pose(2, 3);
2624 
2625  // R-P-Y / X-Y-Z / 0-1-2 Euler Angle Standard
2626  Eigen::Vector3d vec = pose.rotation().eulerAngles(0, 1, 2);
2627  roll = vec[0];
2628  pitch = vec[1];
2629  yaw = vec[2];
2630 }
2631 
2632 void RvizVisualTools::generateRandomPose(geometry_msgs::Pose& pose, RandomPoseBounds pose_bounds)
2633 {
2634  Eigen::Affine3d pose_eigen;
2635  generateRandomPose(pose_eigen, pose_bounds);
2636  pose = convertPose(pose_eigen);
2637 }
2638 
2639 void RvizVisualTools::generateRandomCuboid(geometry_msgs::Pose& cuboid_pose, double& depth, double& width,
2640  double& height, RandomPoseBounds pose_bounds,
2641  RandomCuboidBounds cuboid_bounds)
2642 {
2643  // Size
2644  depth = fRand(cuboid_bounds.cuboid_size_min_, cuboid_bounds.cuboid_size_max_);
2645  width = fRand(cuboid_bounds.cuboid_size_min_, cuboid_bounds.cuboid_size_max_);
2646  height = fRand(cuboid_bounds.cuboid_size_min_, cuboid_bounds.cuboid_size_max_);
2647 
2648  // Orientation
2649  generateRandomPose(cuboid_pose, pose_bounds);
2650 }
2651 
2652 void RvizVisualTools::generateRandomPose(Eigen::Affine3d& pose, RandomPoseBounds pose_bounds)
2653 {
2654  // Error check elevation & azimuth angles
2655  // 0 <= elevation <= pi
2656  // 0 <= azimuth <= 2 * pi
2657  if (pose_bounds.elevation_min_ < 0)
2658  {
2659  ROS_WARN_STREAM_NAMED(name_, "min elevation bound < 0, setting equal to 0");
2660  pose_bounds.elevation_min_ = 0;
2661  }
2662 
2663  if (pose_bounds.elevation_max_ > M_PI)
2664  {
2665  ROS_WARN_STREAM_NAMED(name_, "max elevation bound > pi, setting equal to pi ");
2666  pose_bounds.elevation_max_ = M_PI;
2667  }
2668 
2669  if (pose_bounds.azimuth_min_ < 0)
2670  {
2671  ROS_WARN_STREAM_NAMED(name_, "min azimuth bound < 0, setting equal to 0");
2672  pose_bounds.azimuth_min_ = 0;
2673  }
2674 
2675  if (pose_bounds.azimuth_max_ > 2 * M_PI)
2676  {
2677  ROS_WARN_STREAM_NAMED(name_, "max azimuth bound > 2 pi, setting equal to 2 pi ");
2678  pose_bounds.azimuth_max_ = 2 * M_PI;
2679  }
2680 
2681  // Position
2682  pose.translation().x() = dRand(pose_bounds.x_min_, pose_bounds.x_max_);
2683  pose.translation().y() = dRand(pose_bounds.y_min_, pose_bounds.y_max_);
2684  pose.translation().z() = dRand(pose_bounds.z_min_, pose_bounds.z_max_);
2685 
2686  // Random orientation (random rotation axis from unit sphere and random angle)
2687  double angle = dRand(pose_bounds.angle_min_, pose_bounds.angle_max_);
2688  double elevation = dRand(pose_bounds.elevation_min_, pose_bounds.elevation_max_);
2689  double azimuth = dRand(pose_bounds.azimuth_min_, pose_bounds.azimuth_max_);
2690 
2691  Eigen::Vector3d axis;
2692  axis[0] = sin(elevation) * cos(azimuth);
2693  axis[1] = sin(elevation) * sin(azimuth);
2694  axis[2] = cos(elevation);
2695 
2696  Eigen::Quaterniond quaternion(Eigen::AngleAxis<double>(static_cast<double>(angle), axis));
2697  pose = Eigen::Translation3d(pose.translation().x(), pose.translation().y(), pose.translation().z()) * quaternion;
2698 }
2699 
2700 void RvizVisualTools::generateEmptyPose(geometry_msgs::Pose& pose)
2701 {
2702  // Position
2703  pose.position.x = 0;
2704  pose.position.y = 0;
2705  pose.position.z = 0;
2706 
2707  // Orientation on place
2708  pose.orientation.x = 0;
2709  pose.orientation.y = 0;
2710  pose.orientation.z = 0;
2711  pose.orientation.w = 1;
2712 }
2713 
2714 bool RvizVisualTools::posesEqual(const Eigen::Affine3d& pose1, const Eigen::Affine3d& pose2, double threshold)
2715 {
2716  static const std::size_t NUM_VARS = 16; // size of eigen matrix
2717 
2718  for (std::size_t i = 0; i < NUM_VARS; ++i)
2719  {
2720  if (fabs(pose1.data()[i] - pose2.data()[i]) > threshold)
2721  {
2722  return false;
2723  }
2724  }
2725 
2726  return true;
2727 }
2728 
2729 double RvizVisualTools::dRand(double min, double max)
2730 {
2731  double d = static_cast<double>(rand()) / RAND_MAX;
2732  return min + d * (max - min);
2733 }
2734 
2735 float RvizVisualTools::fRand(float min, float max)
2736 {
2737  float d = static_cast<float>(rand()) / RAND_MAX;
2738  return min + d * (max - min);
2739 }
2740 
2741 int RvizVisualTools::iRand(int min, int max)
2742 {
2743  int n = max - min + 1;
2744  int remainder = RAND_MAX % n;
2745  int x;
2746  do
2747  {
2748  x = rand();
2749  } while (x >= RAND_MAX - remainder);
2750  return min + x % n;
2751 }
2752 
2753 void RvizVisualTools::printTranslation(const Eigen::Vector3d& translation)
2754 {
2755  std::cout << "T.xyz = [" << translation.x() << ", " << translation.y() << ", " << translation.z() << "]" << std::endl;
2756 }
2757 
2758 void RvizVisualTools::printTransform(const Eigen::Affine3d& transform)
2759 {
2760  Eigen::Quaterniond q(transform.rotation());
2761  std::cout << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
2762  << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", "
2763  << q.w() << "]" << std::endl;
2764 }
2765 
2766 void RvizVisualTools::printTransformRPY(const Eigen::Affine3d& transform)
2767 {
2768  // R-P-Y / X-Y-Z / 0-1-2 Euler Angle Standard
2769  Eigen::Vector3d vec = transform.rotation().eulerAngles(0, 1, 2);
2770  std::cout << "transform: [" << transform.translation().x() << ", " << transform.translation().y() << ", "
2771  << transform.translation().z() << ", " << vec[0] << ", " << vec[1] << ", " << vec[2] << "]\n";
2772 }
2773 
2774 void RvizVisualTools::printTransformFull(const Eigen::Affine3d& transform)
2775 {
2776  std::cout << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
2777  << transform.translation().z() << "], R = \n"
2778  << transform.rotation() << "\n";
2779 }
2780 
2781 void RvizVisualTools::prompt(const std::string& msg)
2782 {
2783  getRemoteControl()->waitForNextStep(msg);
2784 }
2785 
2786 RemoteControlPtr& RvizVisualTools::getRemoteControl()
2787 {
2788  if (!remote_control_)
2789  {
2790  loadRemoteControl();
2791  }
2792  return remote_control_;
2793 }
2794 
2795 void RvizVisualTools::loadRemoteControl()
2796 {
2797  // Load remote
2798  if (!remote_control_)
2799  {
2800  remote_control_ = std::make_shared<RemoteControl>(nh_);
2801  ros::spinOnce();
2802  }
2803 }
2804 
2805 } // namespace rviz_visual_tools
rviz_visual_tools::RandomPoseBounds::angle_max_
double angle_max_
Definition: rviz_visual_tools.h:137
d
d
rviz_visual_tools::XXXSMALL
Definition: rviz_visual_tools.h:107
rviz_visual_tools::RvizVisualTools::block_marker_
visualization_msgs::Marker block_marker_
Definition: rviz_visual_tools.h:1108
rviz_visual_tools::scales
scales
Definition: rviz_visual_tools.h:104
rviz_visual_tools::RandomPoseBounds::x_max_
double x_max_
Definition: rviz_visual_tools.h:132
rviz_visual_tools::RvizVisualTools::printTransform
static void printTransform(const Eigen::Affine3d &transform)
Display in the console a transform in quaternions.
Definition: rviz_visual_tools.cpp:2758
rviz_visual_tools::SMALL
Definition: rviz_visual_tools.h:110
rviz_visual_tools::RvizVisualTools::sphere_marker_
visualization_msgs::Marker sphere_marker_
Definition: rviz_visual_tools.h:1107
rviz_visual_tools::RvizVisualTools::remote_control_
RemoteControlPtr remote_control_
Definition: rviz_visual_tools.h:1082
rviz_visual_tools::RvizVisualTools::publishMarker
bool publishMarker(visualization_msgs::Marker &marker)
Display a visualization_msgs Marker of a custom type. Allows reuse of the ros publisher.
Definition: rviz_visual_tools.cpp:830
rviz_visual_tools::RvizVisualTools::prompt
void prompt(const std::string &msg)
Wait for user feedback i.e. through a button or joystick.
Definition: rviz_visual_tools.cpp:2781
rviz_visual_tools::RvizVisualTools::publishAxisLabeled
bool publishAxisLabeled(const Eigen::Affine3d &pose, const std::string &label, scales scale=MEDIUM, colors color=WHITE)
Display a marker of a coordinate frame axis with a text label describing it.
Definition: rviz_visual_tools.cpp:1411
rviz_visual_tools::RandomPoseBounds::z_min_
double z_min_
Definition: rviz_visual_tools.h:134
ros::NodeHandle
rviz_visual_tools::RvizVisualTools::getRandColor
static colors getRandColor()
Get a random color from the list of hardcoded enum color types.
Definition: rviz_visual_tools.cpp:361
rviz_visual_tools::RandomPoseBounds::y_min_
double y_min_
Definition: rviz_visual_tools.h:133
rviz_visual_tools::RvizVisualTools::enableBatchPublishing
void enableBatchPublishing(bool enable=true)
Enable batch publishing - useful for when many markers need to be published at once and the ROS topic...
Definition: rviz_visual_tools.cpp:845
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
rviz_visual_tools::RvizVisualTools::loadRemoteControl
void loadRemoteControl()
Pre-load remote control.
Definition: rviz_visual_tools.cpp:2795
rviz_visual_tools::RvizVisualTools::waitForMarkerPub
void waitForMarkerPub()
Optional blocking function to call after calling loadMarkerPub(). Allows you to do some intermediate ...
Definition: rviz_visual_tools.cpp:284
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rviz_visual_tools::ZYX
Definition: rviz_visual_tools.h:123
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
EigenSTL::vector_Affine3d
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
rviz_visual_tools::RvizVisualTools::publishCuboid
bool publishCuboid(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, colors color=BLUE)
Display a rectangular cuboid.
Definition: rviz_visual_tools.cpp:1650
rviz_visual_tools::RvizVisualTools::alpha_
double alpha_
Definition: rviz_visual_tools.h:1099
rviz_visual_tools::RvizVisualTools::markers_
visualization_msgs::MarkerArray markers_
Definition: rviz_visual_tools.h:1103
rviz_visual_tools::WHITE
Definition: rviz_visual_tools.h:94
rviz_visual_tools::RvizVisualTools::publishMesh
bool publishMesh(const Eigen::Affine3d &pose, const std::string &file_name, colors color=CLEAR, double scale=1, const std::string &ns="mesh", std::size_t id=0)
Display a mesh from file.
Definition: rviz_visual_tools.cpp:1574
rviz_visual_tools::RvizVisualTools::publishLines
bool publishLines(const EigenSTL::vector_Vector3d &aPoints, const EigenSTL::vector_Vector3d &bPoints, const std::vector< colors > &colors, scales scale=MEDIUM)
Display a marker of lines.
Definition: rviz_visual_tools.cpp:1817
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
rviz_visual_tools::RvizVisualTools::waitForSubscriber
bool waitForSubscriber(const ros::Publisher &pub, double wait_time=0.5, bool blocking=false)
Wait until at least one subscriber connects to a publisher.
Definition: rviz_visual_tools.cpp:296
rviz_visual_tools::RandomCuboidBounds::cuboid_size_min_
double cuboid_size_min_
Definition: rviz_visual_tools.h:164
rviz_visual_tools::RvizVisualTools::pub_rviz_markers_connected_
bool pub_rviz_markers_connected_
Definition: rviz_visual_tools.h:1086
rviz_visual_tools::RvizVisualTools::publishXZPlane
bool publishXZPlane(const Eigen::Affine3d &pose, colors color=TRANSLUCENT, double scale=1.0)
Display the XY plane of a given pose.
Definition: rviz_visual_tools.cpp:1027
tf::poseEigenToMsg
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
tf::poseMsgToEigen
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
rviz_visual_tools::RAND
Definition: rviz_visual_tools.h:99
rviz_visual_tools::RvizVisualTools::marker_topic_
std::string marker_topic_
Definition: rviz_visual_tools.h:1090
rviz_visual_tools::RvizVisualTools::convertToXYZRPY
static void convertToXYZRPY(const Eigen::Affine3d &pose, std::vector< double > &xyzrpy)
Convert an affine3d to xyz rpy components R-P-Y / X-Y-Z / 0-1-2 Euler Angle Standard.
Definition: rviz_visual_tools.cpp:2612
rviz_visual_tools::RvizVisualTools::pub_rviz_markers_waited_
bool pub_rviz_markers_waited_
Definition: rviz_visual_tools.h:1087
rviz_visual_tools::RvizVisualTools::getColor
std_msgs::ColorRGBA getColor(colors color) const
Get the RGB value of standard colors.
Definition: rviz_visual_tools.cpp:367
rviz_visual_tools::RvizVisualTools::publishLine
bool publishLine(const Eigen::Affine3d &point1, const Eigen::Affine3d &point2, colors color=BLUE, scales scale=MEDIUM)
Display a marker of line.
Definition: rviz_visual_tools.cpp:1750
rviz_visual_tools::RvizVisualTools::setLifetime
void setLifetime(double lifetime)
Set the lifetime of markers published to rviz.
Definition: rviz_visual_tools.cpp:346
rviz_visual_tools::RvizVisualTools::slerp
static double slerp(double start, double end, double range, double value)
Interpolate from [start, end] with value of size steps with current value count.
Definition: rviz_visual_tools.cpp:604
rviz_visual_tools::ORANGE
Definition: rviz_visual_tools.h:90
rviz_visual_tools::RvizVisualTools::arrow_marker_
visualization_msgs::Marker arrow_marker_
Definition: rviz_visual_tools.h:1106
rviz_visual_tools::RvizVisualTools::text_marker_
visualization_msgs::Marker text_marker_
Definition: rviz_visual_tools.h:1111
rviz_visual_tools::RandomPoseBounds::angle_min_
double angle_min_
Definition: rviz_visual_tools.h:137
rviz_visual_tools::RemoteControlPtr
std::shared_ptr< RemoteControl > RemoteControlPtr
Definition: remote_control.h:136
rviz_visual_tools::TRANSLUCENT
Definition: rviz_visual_tools.h:96
rviz_visual_tools::RvizVisualTools::publishXYPlane
bool publishXYPlane(const Eigen::Affine3d &pose, colors color=TRANSLUCENT, double scale=1.0)
Display the XY plane of a given pose.
Definition: rviz_visual_tools.cpp:981
rviz_visual_tools::REGULAR
Definition: rviz_visual_tools.h:117
rviz_visual_tools::RvizVisualTools::enableFrameLocking
void enableFrameLocking(bool enable=true)
Enable frame locking - useful for when the markers is attached to a moving TF, the marker will be re-...
Definition: rviz_visual_tools.cpp:850
rviz_visual_tools::RvizVisualTools::printTransformFull
static void printTransformFull(const Eigen::Affine3d &transform)
Display in the console a transform with full 3x3 rotation matrix.
Definition: rviz_visual_tools.cpp:2774
rviz_visual_tools::RvizVisualTools::initialize
void initialize()
Shared function for initilization by constructors.
Definition: rviz_visual_tools.cpp:66
rviz_visual_tools::RvizVisualTools::dRand
static double dRand(double min, double max)
Get random between min and max.
Definition: rviz_visual_tools.cpp:2729
rviz_visual_tools::RvizVisualTools::publishLineStrip
bool publishLineStrip(const std::vector< geometry_msgs::Point > &path, colors color=RED, scales scale=MEDIUM, const std::string &ns="Path")
Display a series of connected lines using the LINE_STRIP method - deprecated because visual bugs...
Definition: rviz_visual_tools.cpp:1874
rviz_visual_tools::RvizVisualTools::publishYArrow
bool publishYArrow(const Eigen::Affine3d &pose, colors color=GREEN, scales scale=MEDIUM, double length=0.0)
Display an arrow along the y-axis of a pose.
Definition: rviz_visual_tools.cpp:1252
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
rviz_visual_tools::RvizVisualTools::publishSpheres
bool publishSpheres(const EigenSTL::vector_Vector3d &points, colors color=BLUE, scales scale=MEDIUM, const std::string &ns="Spheres")
Display a marker of a series of spheres.
Definition: rviz_visual_tools.cpp:2293
tf::vectorEigenToTF
void vectorEigenToTF(const Eigen::Vector3d &e, tf::Vector3 &t)
rviz_visual_tools::TRANSLUCENT_LIGHT
Definition: rviz_visual_tools.h:97
rviz_visual_tools::RvizVisualTools::resetMarkerCounts
void resetMarkerCounts()
Reset the id&#39;s of all published markers so that they overwrite themselves in the future NOTE you may ...
Definition: rviz_visual_tools.cpp:80
rviz_visual_tools::GREEN
Definition: rviz_visual_tools.h:87
rviz_visual_tools::XYZ
Definition: rviz_visual_tools.h:122
rviz_visual_tools::RvizVisualTools::all_rand_colors_
static const std::array< colors, 14 > all_rand_colors_
Definition: rviz_visual_tools.h:1123
rviz_visual_tools::GREY
Definition: rviz_visual_tools.h:85
rviz_visual_tools::RvizVisualTools::pub_rviz_markers_
ros::Publisher pub_rviz_markers_
Definition: rviz_visual_tools.h:1085
rviz_visual_tools::RvizVisualTools::getRemoteControl
RemoteControlPtr & getRemoteControl()
Ability to load remote control on the fly.
Definition: rviz_visual_tools.cpp:2786
rviz_visual_tools::XLARGE
Definition: rviz_visual_tools.h:113
rviz_visual_tools::RvizVisualTools::publishXArrow
bool publishXArrow(const Eigen::Affine3d &pose, colors color=RED, scales scale=MEDIUM, double length=0.0)
Display an arrow along the x-axis of a pose.
Definition: rviz_visual_tools.cpp:1237
rviz_visual_tools::RvizVisualTools::publishGraph
bool publishGraph(const graph_msgs::GeometryGraph &graph, colors color, double radius)
Display a graph.
Definition: rviz_visual_tools.cpp:1618
rviz_visual_tools::YELLOW
Definition: rviz_visual_tools.h:95
rviz_visual_tools::RvizVisualTools::triggerEvery
bool triggerEvery(std::size_t queueSize)
Trigger the publish function to send out all collected markers IF there are at leats queueSize number...
Definition: rviz_visual_tools.cpp:855
rviz_visual_tools::RvizVisualTools::generateEmptyPose
static void generateEmptyPose(geometry_msgs::Pose &pose)
Create a pose of position (0,0,0) and quaternion (0,0,0,1)
Definition: rviz_visual_tools.cpp:2700
rviz_visual_tools::RvizVisualTools::posesEqual
static bool posesEqual(const Eigen::Affine3d &pose1, const Eigen::Affine3d &pose2, double threshold=0.000001)
Test if two Eigen poses are close enough.
Definition: rviz_visual_tools.cpp:2714
rviz_visual_tools::XXXXSMALL
Definition: rviz_visual_tools.h:106
rviz_visual_tools::RvizVisualTools::convertPoseToPoint
static geometry_msgs::Point convertPoseToPoint(const Eigen::Affine3d &pose)
Convert an Eigen pose to a geometry_msg point Note: Not thread safe but very convenient.
Definition: rviz_visual_tools.cpp:2502
rviz_visual_tools::BLACK
Definition: rviz_visual_tools.h:81
rviz_visual_tools::XXLARGE
Definition: rviz_visual_tools.h:114
rviz_visual_tools::RvizVisualTools::publishAxis
bool publishAxis(const geometry_msgs::Pose &pose, scales scale=MEDIUM, const std::string &ns="Axis")
Display a red/green/blue coordinate frame axis.
Definition: rviz_visual_tools.cpp:1431
rviz_visual_tools::RED
Definition: rviz_visual_tools.h:92
rviz_visual_tools::RvizVisualTools::generateRandomPose
static void generateRandomPose(geometry_msgs::Pose &pose, RandomPoseBounds pose_bounds=RandomPoseBounds())
Create a random pose within bounds of random_pose_bounds_.
Definition: rviz_visual_tools.cpp:2632
rviz_visual_tools::RandomPoseBounds::elevation_max_
double elevation_max_
Definition: rviz_visual_tools.h:135
ROS_WARN_STREAM_ONCE_NAMED
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
rviz_visual_tools::PINK
Definition: rviz_visual_tools.h:93
rviz_visual_tools::RandomPoseBounds::azimuth_max_
double azimuth_max_
Definition: rviz_visual_tools.h:136
rviz_visual_tools::RvizVisualTools::publishAxisInternal
bool publishAxisInternal(const Eigen::Affine3d &pose, double length=0.1, double radius=0.01, const std::string &ns="Axis")
Display a red/green/blue coordinate axis - the &#39;internal&#39; version does not do a batch publish...
Definition: rviz_visual_tools.cpp:1457
rviz_visual_tools::RvizVisualTools::createRandColor
std_msgs::ColorRGBA createRandColor() const
Create a random color that is not too light.
Definition: rviz_visual_tools.cpp:575
rviz_visual_tools::SMALL_SCALE
static const double SMALL_SCALE
Definition: rviz_visual_tools.h:75
rviz_visual_tools::BLUE
Definition: rviz_visual_tools.h:83
rviz_visual_tools::RvizVisualTools::cylinder_marker_
visualization_msgs::Marker cylinder_marker_
Definition: rviz_visual_tools.h:1109
rviz_visual_tools::RvizVisualTools::triggerAndDisable
RVIZ_VISUAL_TOOLS_DEPRECATED bool triggerAndDisable()
Trigger the publish function to send out all collected markers. Also then turns off the batch mode...
Definition: rviz_visual_tools.cpp:882
rviz_visual_tools::RvizVisualTools::line_list_marker_
visualization_msgs::Marker line_list_marker_
Definition: rviz_visual_tools.h:1114
rviz_visual_tools.h
rviz_visual_tools::RandomPoseBounds
Bounds for generateRandomPose()
Definition: rviz_visual_tools.h:130
ros::ok
ROSCPP_DECL bool ok()
rviz_visual_tools::XXXXLARGE
Definition: rviz_visual_tools.h:116
rviz_visual_tools::ZXZ
Definition: rviz_visual_tools.h:124
rviz_visual_tools::RvizVisualTools::scaleToString
static std::string scaleToString(scales scale)
Convert an enum to its string name equivalent.
Definition: rviz_visual_tools.cpp:553
rviz_visual_tools::RvizVisualTools::publishWireframeCuboid
bool publishWireframeCuboid(const Eigen::Affine3d &pose, double depth, double width, double height, colors color=BLUE, const std::string &ns="Wireframe Cuboid", std::size_t id=0)
Publish transformed wireframe cuboid. Useful eg to show an oriented bounding box. ...
Definition: rviz_visual_tools.cpp:2066
rviz_visual_tools::RvizVisualTools::line_strip_marker_
visualization_msgs::Marker line_strip_marker_
Definition: rviz_visual_tools.h:1113
rviz_visual_tools::RvizVisualTools::convertPoint32ToPose
static Eigen::Affine3d convertPoint32ToPose(const geometry_msgs::Point32 &point)
Convert a geometry_msg point (32bit) to an Eigen pose Note: Not thread safe but very convenient...
Definition: rviz_visual_tools.cpp:2473
rviz_visual_tools::RvizVisualTools::convertFromXYZRPY
static RVIZ_VISUAL_TOOLS_DEPRECATED Eigen::Affine3d convertFromXYZRPY(double x, double y, double z, double roll, double pitch, double yaw)
Convert a 6-vector of x,y,z, roll,pitch,yall to an Affine3d with quaternion using Euler R-P-Y / X-Y-Z...
Definition: rviz_visual_tools.cpp:2559
rviz_visual_tools::TRANSLUCENT_DARK
Definition: rviz_visual_tools.h:98
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
rviz_visual_tools::RvizVisualTools::publishPath
bool publishPath(const std::vector< geometry_msgs::Pose > &path, colors color=RED, scales scale=MEDIUM, const std::string &ns="Path")
Display a marker of a series of connected cylinders.
Definition: rviz_visual_tools.cpp:1908
rviz_visual_tools::RvizVisualTools::getColorScale
std_msgs::ColorRGBA getColorScale(double value) const
Convert a value from [0,1] to a color Green->Red.
Definition: rviz_visual_tools.cpp:609
rviz_visual_tools::DEFAULT
Definition: rviz_visual_tools.h:101
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
rviz_visual_tools::RvizVisualTools::publishCone
bool publishCone(const Eigen::Affine3d &pose, double angle, colors color=TRANSLUCENT, double scale=1.0)
Display a cone of a given angle along the x-axis.
Definition: rviz_visual_tools.cpp:935
rviz_visual_tools::CYAN
Definition: rviz_visual_tools.h:84
rviz_visual_tools::CLEAR
Definition: rviz_visual_tools.h:100
rviz_visual_tools::RvizVisualTools::mesh_marker_
visualization_msgs::Marker mesh_marker_
Definition: rviz_visual_tools.h:1110
rviz_visual_tools::RandomCuboidBounds
Bounds for generateRandomCuboid()
Definition: rviz_visual_tools.h:162
rviz_visual_tools::LIME_GREEN
Definition: rviz_visual_tools.h:88
rviz_visual_tools::RvizVisualTools::publishPolygon
bool publishPolygon(const geometry_msgs::Polygon &polygon, colors color=RED, scales scale=MEDIUM, const std::string &ns="Polygon")
Display a marker of a polygon.
Definition: rviz_visual_tools.cpp:2042
rviz_visual_tools::RvizVisualTools::loadRvizMarkers
bool loadRvizMarkers()
Pre-load rviz markers for better efficiency.
Definition: rviz_visual_tools.cpp:95
rviz_visual_tools::RvizVisualTools::iRand
static int iRand(int min, int max)
Definition: rviz_visual_tools.cpp:2741
rviz_visual_tools::RvizVisualTools::spheres_marker_
visualization_msgs::Marker spheres_marker_
Definition: rviz_visual_tools.h:1115
tf::pointEigenToMsg
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
ros::Rate::sleep
bool sleep()
rviz_visual_tools::RvizVisualTools::convertPose
static geometry_msgs::Pose convertPose(const Eigen::Affine3d &pose)
Convert an Eigen pose to a geometry_msg pose.
Definition: rviz_visual_tools.cpp:2449
rviz_visual_tools::LARGE
Definition: rviz_visual_tools.h:112
rviz_visual_tools
Definition: imarker_simple.h:56
tf::quaternionTFToEigen
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e)
rviz_visual_tools::RvizVisualTools::marker_lifetime_
ros::Duration marker_lifetime_
Definition: rviz_visual_tools.h:1094
rviz_visual_tools::XSMALL
Definition: rviz_visual_tools.h:109
rviz_visual_tools::RvizVisualTools::publishZArrow
bool publishZArrow(const Eigen::Affine3d &pose, colors color=BLUE, scales scale=MEDIUM, double length=0.0, std::size_t id=0)
Display an arrow along the z-axis of a pose.
Definition: rviz_visual_tools.cpp:1272
rviz_visual_tools::colors
colors
Definition: rviz_visual_tools.h:79
rviz_visual_tools::RvizVisualTools::generateRandomCuboid
static void generateRandomCuboid(geometry_msgs::Pose &cuboid_pose, double &depth, double &width, double &height, RandomPoseBounds pose_bounds=RandomPoseBounds(), RandomCuboidBounds cuboid_bounds=RandomCuboidBounds())
Create a random rectangular cuboid of some shape.
Definition: rviz_visual_tools.cpp:2639
rviz_visual_tools::RvizVisualTools::getCenterPoint
Eigen::Vector3d getCenterPoint(const Eigen::Vector3d &a, const Eigen::Vector3d &b) const
Find the center between to points.
Definition: rviz_visual_tools.cpp:714
rviz_visual_tools::RvizVisualTools::publishCylinder
bool publishCylinder(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, colors color=BLUE, scales scale=MEDIUM, const std::string &ns="Cylinder")
Display a marker of a cylinder.
Definition: rviz_visual_tools.cpp:1504
rviz_visual_tools::MEDIUM
Definition: rviz_visual_tools.h:111
rviz_visual_tools::RvizVisualTools::getVectorBetweenPoints
Eigen::Affine3d getVectorBetweenPoints(const Eigen::Vector3d &a, const Eigen::Vector3d &b) const
Create a vector that points from point a to point b.
Definition: rviz_visual_tools.cpp:723
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rviz_visual_tools::RvizVisualTools::psychedelic_mode_
bool psychedelic_mode_
Definition: rviz_visual_tools.h:1120
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
rviz_visual_tools::PURPLE
Definition: rviz_visual_tools.h:91
rviz_visual_tools::RandomPoseBounds::z_max_
double z_max_
Definition: rviz_visual_tools.h:134
tf_eigen.h
rviz_visual_tools::RvizVisualTools::convertPoint32
static Eigen::Vector3d convertPoint32(const geometry_msgs::Point32 &point)
Convert a geometry_msg point to an Eigen point Note: Not thread safe but very convenient.
Definition: rviz_visual_tools.cpp:2518
rviz_visual_tools::XXSMALL
Definition: rviz_visual_tools.h:108
rviz_visual_tools::RandomPoseBounds::azimuth_min_
double azimuth_min_
Definition: rviz_visual_tools.h:136
rviz_visual_tools::RvizVisualTools::convertPoseSafe
static void convertPoseSafe(const Eigen::Affine3d &pose, geometry_msgs::Pose &pose_msg)
Convert an Eigen pose to a geometry_msg pose - thread safe.
Definition: rviz_visual_tools.cpp:2456
rviz_visual_tools::RvizVisualTools::convertPoint
static Eigen::Vector3d convertPoint(const geometry_msgs::Point &point)
Convert a geometry_msg point to an Eigen point Note: Not thread safe but very convenient.
Definition: rviz_visual_tools.cpp:2509
rviz_visual_tools::RvizVisualTools::publishText
bool publishText(const Eigen::Affine3d &pose, const std::string &text, colors color=WHITE, scales scale=MEDIUM, bool static_id=true)
Display a marker of a text.
Definition: rviz_visual_tools.cpp:2398
rviz_visual_tools::RandomPoseBounds::elevation_min_
double elevation_min_
Definition: rviz_visual_tools.h:135
rviz_visual_tools::RvizVisualTools::name_
static const std::string name_
Definition: rviz_visual_tools.h:1079
rviz_visual_tools::RvizVisualTools::getScale
geometry_msgs::Vector3 getScale(scales scale, double marker_scale=1.0) const
Get the rviz marker scale of standard sizes.
Definition: rviz_visual_tools.cpp:656
rviz_visual_tools::RvizVisualTools::intToRvizColor
static colors intToRvizColor(std::size_t color)
Used by interfaces that do not directly depend on Rviz Visual Tools, such as OMPL.
Definition: rviz_visual_tools.cpp:500
rviz_visual_tools::RvizVisualTools::publishArrow
bool publishArrow(const Eigen::Affine3d &pose, colors color=BLUE, scales scale=MEDIUM, double length=0.0, std::size_t id=0)
Display an arrow along the x-axis of a pose.
Definition: rviz_visual_tools.cpp:1302
ros::Time::now
static Time now()
rviz_visual_tools::RvizVisualTools::reset_marker_
visualization_msgs::Marker reset_marker_
Definition: rviz_visual_tools.h:1116
rviz_visual_tools::RvizVisualTools::deleteAllMarkers
bool deleteAllMarkers()
Tell Rviz to clear all markers on a particular display.
Definition: rviz_visual_tools.cpp:74
rviz_visual_tools::RvizVisualTools::frame_locking_enabled_
bool frame_locking_enabled_
Definition: rviz_visual_tools.h:1098
rviz_visual_tools::RandomPoseBounds::y_max_
double y_max_
Definition: rviz_visual_tools.h:133
rviz_visual_tools::RvizVisualTools::triangle_marker_
visualization_msgs::Marker triangle_marker_
Definition: rviz_visual_tools.h:1117
rviz_visual_tools::RvizVisualTools::intToRvizScale
static rviz_visual_tools::scales intToRvizScale(std::size_t scale)
Used by interfaces that do not directly depend on Rviz Visual Tools, such as OMPL.
Definition: rviz_visual_tools.cpp:531
rviz_visual_tools::RvizVisualTools::base_frame_
std::string base_frame_
Definition: rviz_visual_tools.h:1091
rviz_visual_tools::RvizVisualTools::publishSphere
bool publishSphere(const Eigen::Affine3d &pose, colors color=BLUE, scales scale=MEDIUM, const std::string &ns="Sphere", std::size_t id=0)
Display a marker of a sphere.
Definition: rviz_visual_tools.cpp:1119
ros::Publisher::getTopic
std::string getTopic() const
rviz_visual_tools::RvizVisualTools::convertPointToPose
static geometry_msgs::Pose convertPointToPose(const geometry_msgs::Point &point)
Add an identity rotation matrix to make a point have a full pose.
Definition: rviz_visual_tools.cpp:2483
rviz_visual_tools::RvizVisualTools::nh_
ros::NodeHandle nh_
Definition: rviz_visual_tools.h:1076
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
rviz_visual_tools::RvizVisualTools::cuboid_marker_
visualization_msgs::Marker cuboid_marker_
Definition: rviz_visual_tools.h:1112
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
rviz_visual_tools::RvizVisualTools::printTranslation
static void printTranslation(const Eigen::Vector3d &translation)
Display in the console the x,y,z values of a point.
Definition: rviz_visual_tools.cpp:2753
rviz_visual_tools::BROWN
Definition: rviz_visual_tools.h:82
rviz_visual_tools::DARK_GREY
Definition: rviz_visual_tools.h:86
rviz_visual_tools::RvizVisualTools::publishMarkers
bool publishMarkers(visualization_msgs::MarkerArray &markers)
Display an array of markers, allows reuse of the ROS publisher.
Definition: rviz_visual_tools.cpp:889
ros::spinOnce
ROSCPP_DECL void spinOnce()
rviz_visual_tools::RvizVisualTools::publishAxisPath
bool publishAxisPath(const EigenSTL::vector_Affine3d &path, scales scale=MEDIUM, const std::string &ns="Axis Path")
Display a series of red/green/blue coordinate axis along a path.
Definition: rviz_visual_tools.cpp:1480
rviz_visual_tools::RvizVisualTools::fRand
static float fRand(float min, float max)
Definition: rviz_visual_tools.cpp:2735
rviz_visual_tools::RvizVisualTools::RvizVisualTools
RvizVisualTools(std::string base_frame, std::string marker_topic=RVIZ_MARKER_TOPIC, ros::NodeHandle nh=ros::NodeHandle("~"))
Constructor.
Definition: rviz_visual_tools.cpp:60
rviz_visual_tools::RvizVisualTools::batch_publishing_enabled_
bool batch_publishing_enabled_
Definition: rviz_visual_tools.h:1097
rviz_visual_tools::RvizVisualTools::global_scale_
double global_scale_
Definition: rviz_visual_tools.h:1100
rviz_visual_tools::RandomCuboidBounds::cuboid_size_max_
double cuboid_size_max_
Definition: rviz_visual_tools.h:164
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
rviz_visual_tools::EulerConvention
EulerConvention
Definition: rviz_visual_tools.h:120
rviz_visual_tools::RvizVisualTools::loadMarkerPub
void loadMarkerPub(bool wait_for_subscriber=false, bool latched=false)
Load publishers as needed.
Definition: rviz_visual_tools.cpp:267
rviz_visual_tools::RvizVisualTools::printTransformRPY
static void printTransformRPY(const Eigen::Affine3d &transform)
Display in the console a transform in roll pitch yaw.
Definition: rviz_visual_tools.cpp:2766
rviz_visual_tools::XXXLARGE
Definition: rviz_visual_tools.h:115
rviz_visual_tools::RandomPoseBounds::x_min_
double x_min_
Definition: rviz_visual_tools.h:132
rviz_visual_tools::RvizVisualTools::trigger
bool trigger()
Definition: rviz_visual_tools.cpp:864
rviz_visual_tools::MAGENTA
Definition: rviz_visual_tools.h:89
eigen_msg.h
rviz_visual_tools::RvizVisualTools::publishYZPlane
bool publishYZPlane(const Eigen::Affine3d &pose, colors color=TRANSLUCENT, double scale=1.0)
Display the XY plane of a given pose.
Definition: rviz_visual_tools.cpp:1073
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
rviz_visual_tools::RvizVisualTools::publishWireframeRectangle
bool publishWireframeRectangle(const Eigen::Affine3d &pose, double height, double width, colors color=BLUE, scales scale=MEDIUM, std::size_t id=0)
Publish outline of a rectangle.
Definition: rviz_visual_tools.cpp:2182

rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Mon Feb 25 2019 03:54:12