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

rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Wed Mar 2 2022 01:03:26