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

rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Mon Feb 28 2022 23:43:06