rviz_visual_tools.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, PickNik Consulting
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman <dave@dav.ee>, Andy McEvoy
36  Desc: Helper functions for displaying basic shape markers in Rviz
37 */
38 
39 #ifndef RVIZ_VISUAL_TOOLS_RVIZ_VISUAL_TOOLS_H
40 #define RVIZ_VISUAL_TOOLS_RVIZ_VISUAL_TOOLS_H
41 
42 #include <ros/ros.h>
43 
44 // C++
45 #include <string>
46 #include <vector>
47 
48 // Eigen
49 #include <Eigen/Geometry>
51 
52 // Rviz
53 #include <visualization_msgs/Marker.h>
54 #include <visualization_msgs/MarkerArray.h>
55 
56 // Boost
57 #include <boost/shared_ptr.hpp>
58 
59 // Messages
60 #include <std_msgs/ColorRGBA.h>
61 #include <graph_msgs/GeometryGraph.h>
62 #include <geometry_msgs/PoseArray.h>
63 #include <geometry_msgs/PoseStamped.h>
64 #include <geometry_msgs/Polygon.h>
65 #include <trajectory_msgs/JointTrajectory.h>
66 
67 // rviz_visual_tools
70 
71 namespace rviz_visual_tools
72 {
73 // Default constants
74 static const std::string RVIZ_MARKER_TOPIC = "/rviz_visual_tools";
75 static const double SMALL_SCALE = 0.001;
76 static const double LARGE_SCALE = 100;
77 
78 // Note: when adding new colors to colors, also add them to getRandColor() function
79 enum colors
80 {
81  BLACK = 0,
82  BROWN = 1,
83  BLUE = 2,
84  CYAN = 3,
85  GREY = 4,
86  DARK_GREY = 5,
87  GREEN = 6,
89  MAGENTA = 8,
90  ORANGE = 9,
91  PURPLE = 10,
92  RED = 11,
93  PINK = 12,
94  WHITE = 13,
95  YELLOW = 14,
99  RAND = 18,
100  CLEAR = 19,
101  DEFAULT = 20 // i.e. 'do not change default color'
102 };
103 
104 enum scales
105 {
107  XXXSMALL = 2,
108  XXSMALL = 3,
109  XSMALL = 4,
110  SMALL = 5,
111  MEDIUM = 6, // same as REGULAR
112  LARGE = 7,
113  XLARGE = 8,
114  XXLARGE = 9,
115  XXXLARGE = 10,
116  XXXXLARGE = 11,
117  REGULAR = 12 // deprecated as of ROS-KINETIC (remove in ROS-L)
118 };
119 
121 {
122  XYZ = 0,
123  ZYX, // This is the ROS standard: http://www.ros.org/reps/rep-0103.html
125 };
126 
131 {
132  double x_min_, x_max_;
133  double y_min_, y_max_;
134  double z_min_, z_max_;
138 
139  RandomPoseBounds(double x_min = 0.0, double x_max = 1.0, double y_min = 0.0, double y_max = 1.0, double z_min = 0.0,
140  double z_max = 1.0, double elevation_min = 0.0, double elevation_max = M_PI,
141  double azimuth_min = 0.0, double azimuth_max = 2 * M_PI, double angle_min = 0.0,
142  double angle_max = 2 * M_PI)
143  {
144  x_min_ = x_min;
145  x_max_ = x_max;
146  y_min_ = y_min;
147  y_max_ = y_max;
148  z_min_ = z_min;
149  z_max_ = z_max;
150  elevation_min_ = elevation_min;
151  elevation_max_ = elevation_max;
152  azimuth_min_ = azimuth_min;
153  azimuth_max_ = azimuth_max;
154  angle_min_ = angle_min;
155  angle_max_ = angle_max;
156  }
157 };
158 
163 {
164  double cuboid_size_min_, cuboid_size_max_;
165 
166  explicit RandomCuboidBounds(double cuboid_size_min = 0.02, double cuboid_size_max = 0.15)
167  {
168  cuboid_size_min_ = cuboid_size_min;
169  cuboid_size_max_ = cuboid_size_max;
170  }
171 };
172 
174 {
175 private:
179  void initialize();
180 
181 public:
188  explicit RvizVisualTools(std::string base_frame, std::string marker_topic = RVIZ_MARKER_TOPIC, ros::NodeHandle nh = ros::NodeHandle("~"));
193  {
194  }
195 
199  bool deleteAllMarkers();
200 
205  void resetMarkerCounts();
206 
211  bool loadRvizMarkers();
212 
214  void setMarkerTopic(const std::string& topic)
215  {
216  marker_topic_ = topic;
217  }
218 
224  void loadMarkerPub(bool wait_for_subscriber = false, bool latched = false);
225 
229  void waitForMarkerPub();
230  void waitForMarkerPub(double wait_time);
231 
239  bool waitForSubscriber(const ros::Publisher& pub, double wait_time = 0.5, bool blocking = false);
240 
245  void setAlpha(double alpha)
246  {
247  alpha_ = alpha;
248  }
253  void setLifetime(double lifetime);
254 
259  static colors getRandColor();
260 
266  std_msgs::ColorRGBA getColor(colors color) const;
267 
269  static colors intToRvizColor(std::size_t color);
270 
272  static rviz_visual_tools::scales intToRvizScale(std::size_t scale);
273 
275  static std::string scaleToString(scales scale);
276 
281  std_msgs::ColorRGBA createRandColor() const;
282 
287  static double slerp(double start, double end, double range, double value);
288 
293  std_msgs::ColorRGBA getColorScale(double value) const;
294 
301  geometry_msgs::Vector3 getScale(scales scale, double marker_scale = 1.0) const;
302 
309  Eigen::Affine3d getVectorBetweenPoints(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const;
310 
317  Eigen::Vector3d getCenterPoint(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const;
318 
323  const std::string getBaseFrame() const
324  {
325  return base_frame_;
326  }
332  void setBaseFrame(const std::string& base_frame)
333  {
334  base_frame_ = base_frame;
335  loadRvizMarkers();
336  }
337 
341  double getGlobalScale() const
342  {
343  return global_scale_;
344  }
348  void setGlobalScale(double global_scale)
349  {
350  global_scale_ = global_scale;
351  }
357  bool publishMarker(visualization_msgs::Marker& marker);
358 
364  void enableBatchPublishing(bool enable = true);
365 
370  void enableFrameLocking(bool enable = true);
371 
379  bool triggerEvery(std::size_t queueSize);
380 
387  {
388  return trigger();
389  }
390 
391  bool trigger();
392 
400  bool triggerAndDisable();
401 
407  bool publishMarkers(visualization_msgs::MarkerArray& markers);
408 
416  bool publishCone(const Eigen::Affine3d& pose, double angle, colors color = TRANSLUCENT, double scale = 1.0);
417  bool publishCone(const geometry_msgs::Pose& pose, double angle, colors color = TRANSLUCENT, double scale = 1.0);
418 
426  bool publishXYPlane(const Eigen::Affine3d& pose, colors color = TRANSLUCENT, double scale = 1.0);
427  bool publishXYPlane(const geometry_msgs::Pose& pose, colors color = TRANSLUCENT, double scale = 1.0);
428 
436  bool publishXZPlane(const Eigen::Affine3d& pose, colors color = TRANSLUCENT, double scale = 1.0);
437  bool publishXZPlane(const geometry_msgs::Pose& pose, colors color = TRANSLUCENT, double scale = 1.0);
438 
446  bool publishYZPlane(const Eigen::Affine3d& pose, colors color = TRANSLUCENT, double scale = 1.0);
447  bool publishYZPlane(const geometry_msgs::Pose& pose, colors color = TRANSLUCENT, double scale = 1.0);
448 
459  bool publishSphere(const Eigen::Affine3d& pose, colors color = BLUE, scales scale = MEDIUM,
460  const std::string& ns = "Sphere", std::size_t id = 0);
461  bool publishSphere(const Eigen::Vector3d& point, colors color = BLUE, scales scale = MEDIUM,
462  const std::string& ns = "Sphere", std::size_t id = 0);
463  bool publishSphere(const Eigen::Vector3d& point, colors color, double scale, const std::string& ns = "Sphere",
464  std::size_t id = 0);
465  bool publishSphere(const geometry_msgs::Point& point, colors color = BLUE, scales scale = MEDIUM,
466  const std::string& ns = "Sphere", std::size_t id = 0);
467  bool publishSphere(const geometry_msgs::Pose& pose, colors color = BLUE, scales scale = MEDIUM,
468  const std::string& ns = "Sphere", std::size_t id = 0);
469  bool publishSphere(const geometry_msgs::Pose& pose, colors color, double scale, const std::string& ns = "Sphere",
470  std::size_t id = 0);
471  bool publishSphere(const geometry_msgs::Pose& pose, colors color, const geometry_msgs::Vector3 scale,
472  const std::string& ns = "Sphere", std::size_t id = 0);
473  bool publishSphere(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color,
474  const geometry_msgs::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0);
475  bool publishSphere(const Eigen::Affine3d& pose, const std_msgs::ColorRGBA& color, const geometry_msgs::Vector3 scale,
476  const std::string& ns = "Sphere", std::size_t id = 0);
477  bool publishSphere(const Eigen::Vector3d& point, const std_msgs::ColorRGBA& color, const geometry_msgs::Vector3 scale,
478  const std::string& ns = "Sphere", std::size_t id = 0);
479  bool publishSphere(const geometry_msgs::PoseStamped& pose, colors color, const geometry_msgs::Vector3 scale,
480  const std::string& ns = "Sphere", std::size_t id = 0);
481 
490  bool publishSpheres(const EigenSTL::vector_Vector3d& points, colors color = BLUE, scales scale = MEDIUM,
491  const std::string& ns = "Spheres");
492  bool publishSpheres(const EigenSTL::vector_Vector3d& points, colors color, double scale = 0.1,
493  const std::string& ns = "Spheres");
494  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color = BLUE, scales scale = MEDIUM,
495  const std::string& ns = "Spheres");
496  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color = BLUE, double scale = 0.1,
497  const std::string& ns = "Spheres");
498  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color,
499  const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres");
500 
509  bool publishSpheres(const EigenSTL::vector_Vector3d& points, const std::vector<colors>& colors, scales scale = MEDIUM,
510  const std::string& ns = "Spheres");
511  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, const std::vector<std_msgs::ColorRGBA>& colors,
512  const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres");
513 
522  bool publishXArrow(const Eigen::Affine3d& pose, colors color = RED, scales scale = MEDIUM, double length = 0.0);
523  bool publishXArrow(const geometry_msgs::Pose& pose, colors color = RED, scales scale = MEDIUM, double length = 0.0);
524  bool publishXArrow(const geometry_msgs::PoseStamped& pose, colors color = RED, scales scale = MEDIUM,
525  double length = 0.0);
526 
535  bool publishYArrow(const Eigen::Affine3d& pose, colors color = GREEN, scales scale = MEDIUM, double length = 0.0);
536  bool publishYArrow(const geometry_msgs::Pose& pose, colors color = GREEN, scales scale = MEDIUM, double length = 0.0);
537  bool publishYArrow(const geometry_msgs::PoseStamped& pose, colors color = GREEN, scales scale = MEDIUM,
538  double length = 0.0);
539 
548  bool publishZArrow(const Eigen::Affine3d& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0,
549  std::size_t id = 0);
550  bool publishZArrow(const geometry_msgs::Pose& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0);
551  bool publishZArrow(const geometry_msgs::PoseStamped& pose, colors color = BLUE, scales scale = MEDIUM,
552  double length = 0.0);
553  bool publishZArrow(const geometry_msgs::PoseStamped& pose, colors color = BLUE, scales scale = MEDIUM,
554  double length = 0.0, std::size_t id = 0);
555 
566  bool publishArrow(const Eigen::Affine3d& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0,
567  std::size_t id = 0);
568  bool publishArrow(const geometry_msgs::Pose& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0,
569  std::size_t id = 0);
570  bool publishArrow(const geometry_msgs::PoseStamped& pose, colors color = BLUE, scales scale = MEDIUM,
571  double length = 0.0, std::size_t id = 0);
572  bool publishArrow(const geometry_msgs::Point& start, const geometry_msgs::Point& end, colors color = BLUE,
573  scales scale = MEDIUM, std::size_t id = 0);
574 
582  bool publishCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color = BLUE);
583  bool publishCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2, colors color = BLUE,
584  const std::string& ns = "Cuboid", std::size_t id = 0);
585 
595  bool publishCuboid(const geometry_msgs::Pose& pose, double depth, double width, double height, colors color = BLUE);
596  bool publishCuboid(const Eigen::Affine3d& pose, double depth, double width, double height, colors color = BLUE);
597 
606  bool publishLine(const Eigen::Affine3d& point1, const Eigen::Affine3d& point2, colors color = BLUE,
607  scales scale = MEDIUM);
608  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color = BLUE,
609  scales scale = MEDIUM);
610  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color, double radius);
611  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::ColorRGBA& color,
612  scales scale = MEDIUM);
613  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::ColorRGBA& color,
614  double radius);
615  bool publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2, colors color = BLUE,
616  scales scale = MEDIUM);
617  bool publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
618  const std_msgs::ColorRGBA& color, scales scale = MEDIUM);
619  bool publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
620  const std_msgs::ColorRGBA& color, const geometry_msgs::Vector3& scale);
621 
630  bool publishLines(const EigenSTL::vector_Vector3d& aPoints, const EigenSTL::vector_Vector3d& bPoints,
631  const std::vector<colors>& colors, scales scale = MEDIUM);
632  bool publishLines(const std::vector<geometry_msgs::Point>& aPoints, const std::vector<geometry_msgs::Point>& bPoints,
633  const std::vector<std_msgs::ColorRGBA>& colors, const geometry_msgs::Vector3& scale);
634 
643  bool publishLineStrip(const std::vector<geometry_msgs::Point>& path, colors color = RED, scales scale = MEDIUM,
644  const std::string& ns = "Path");
645 
654  bool publishPath(const std::vector<geometry_msgs::Pose>& path, colors color = RED, scales scale = MEDIUM,
655  const std::string& ns = "Path");
656  bool publishPath(const std::vector<geometry_msgs::Point>& path, colors color = RED, scales scale = MEDIUM,
657  const std::string& ns = "Path");
658  bool publishPath(const EigenSTL::vector_Affine3d& path, colors color = RED, scales scale = MEDIUM,
659  const std::string& ns = "Path");
660  bool publishPath(const EigenSTL::vector_Vector3d& path, colors color = RED, scales scale = MEDIUM,
661  const std::string& ns = "Path");
662  bool publishPath(const std::vector<geometry_msgs::Point>& path, colors color = RED, double radius = 0.01,
663  const std::string& ns = "Path");
664  bool publishPath(const EigenSTL::vector_Vector3d& path, colors color = RED, double radius = 0.01,
665  const std::string& ns = "Path");
666  bool publishPath(const EigenSTL::vector_Affine3d& path, colors color = RED, double radius = 0.01,
667  const std::string& ns = "Path");
668 
678  bool publishPath(const EigenSTL::vector_Vector3d& path, const std::vector<colors>& colors, double radius = 0.01,
679  const std::string& ns = "Path");
680 
681  bool publishPath(const EigenSTL::vector_Vector3d& path, const std::vector<std_msgs::ColorRGBA>& colors, double radius,
682  const std::string& ns = "Path");
683 
692  bool publishPolygon(const geometry_msgs::Polygon& polygon, colors color = RED, scales scale = MEDIUM,
693  const std::string& ns = "Polygon");
694 
707  bool publishWireframeCuboid(const Eigen::Affine3d& pose, double depth, double width, double height,
708  colors color = BLUE, const std::string& ns = "Wireframe Cuboid", std::size_t id = 0);
709 
720  bool publishWireframeCuboid(const Eigen::Affine3d& pose, const Eigen::Vector3d& min_point,
721  const Eigen::Vector3d& max_point, colors color = BLUE,
722  const std::string& ns = "Wireframe Cuboid", std::size_t id = 0);
723 
733  bool publishWireframeRectangle(const Eigen::Affine3d& pose, double height, double width, colors color = BLUE,
734  scales scale = MEDIUM, std::size_t id = 0);
735  bool publishWireframeRectangle(const Eigen::Affine3d& pose, const Eigen::Vector3d& p1_in,
736  const Eigen::Vector3d& p2_in, const Eigen::Vector3d& p3_in,
737  const Eigen::Vector3d& p4_in, colors color, scales scale);
746  bool publishAxisLabeled(const Eigen::Affine3d& pose, const std::string& label, scales scale = MEDIUM,
747  colors color = WHITE);
748  bool publishAxisLabeled(const geometry_msgs::Pose& pose, const std::string& label, scales scale = MEDIUM,
749  colors color = WHITE);
750 
760  bool publishAxis(const geometry_msgs::Pose& pose, scales scale = MEDIUM, const std::string& ns = "Axis");
761  bool publishAxis(const Eigen::Affine3d& pose, scales scale = MEDIUM, const std::string& ns = "Axis");
762  bool publishAxis(const geometry_msgs::Pose& pose, double length, double radius = 0.01,
763  const std::string& ns = "Axis");
764  bool publishAxis(const Eigen::Affine3d& pose, double length, double radius = 0.01, const std::string& ns = "Axis");
765 
766 private:
775  bool publishAxisInternal(const Eigen::Affine3d& pose, double length = 0.1, double radius = 0.01,
776  const std::string& ns = "Axis");
777 
778 public:
787  bool publishAxisPath(const EigenSTL::vector_Affine3d& path, scales scale = MEDIUM,
788  const std::string& ns = "Axis Path");
789  bool publishAxisPath(const EigenSTL::vector_Affine3d& path, double length = 0.1, double radius = 0.01,
790  const std::string& ns = "Axis Path");
791 
800  bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color = BLUE,
801  scales scale = MEDIUM, const std::string& ns = "Cylinder");
802  bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color, double radius = 0.01,
803  const std::string& ns = "Cylinder");
804  bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::ColorRGBA& color,
805  double radius = 0.01, const std::string& ns = "Cylinder");
806 
815  bool publishCylinder(const Eigen::Affine3d& pose, colors color = BLUE, double height = 0.1, double radius = 0.01,
816  const std::string& ns = "Cylinder");
817  bool publishCylinder(const geometry_msgs::Pose& pose, colors color = BLUE, double height = 0.1, double radius = 0.01,
818  const std::string& ns = "Cylinder");
819  bool publishCylinder(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color, double height = 0.1,
820  double radius = 0.01, const std::string& ns = "Cylinder");
821 
833  bool publishMesh(const Eigen::Affine3d& pose, const std::string& file_name, colors color = CLEAR, double scale = 1,
834  const std::string& ns = "mesh", std::size_t id = 0);
835  bool publishMesh(const geometry_msgs::Pose& pose, const std::string& file_name, colors color = CLEAR,
836  double scale = 1, const std::string& ns = "mesh", std::size_t id = 0);
837 
845  bool publishGraph(const graph_msgs::GeometryGraph& graph, colors color, double radius);
846 
856  bool publishText(const Eigen::Affine3d& pose, const std::string& text, colors color = WHITE, scales scale = MEDIUM,
857  bool static_id = true);
858  bool publishText(const Eigen::Affine3d& pose, const std::string& text, colors color,
859  const geometry_msgs::Vector3 scale, bool static_id = true);
860  bool publishText(const geometry_msgs::Pose& pose, const std::string& text, colors color = WHITE,
861  scales scale = MEDIUM, bool static_id = true);
862  bool publishText(const geometry_msgs::Pose& pose, const std::string& text, colors color,
863  const geometry_msgs::Vector3 scale, bool static_id = true);
864 
870  static geometry_msgs::Pose convertPose(const Eigen::Affine3d& pose);
871 
877  static void convertPoseSafe(const Eigen::Affine3d& pose, geometry_msgs::Pose& pose_msg);
878 
885  static Eigen::Affine3d convertPose(const geometry_msgs::Pose& pose);
886 
892  static void convertPoseSafe(const geometry_msgs::Pose& pose_msg, Eigen::Affine3d& pose);
893 
900  static Eigen::Affine3d convertPoint32ToPose(const geometry_msgs::Point32& point);
901 
905  static geometry_msgs::Pose convertPointToPose(const geometry_msgs::Point& point);
906  static Eigen::Affine3d convertPointToPose(const Eigen::Vector3d& point);
907 
914  static geometry_msgs::Point convertPoseToPoint(const Eigen::Affine3d& pose);
915 
922  static Eigen::Vector3d convertPoint(const geometry_msgs::Point& point);
923 
930  static Eigen::Vector3d convertPoint32(const geometry_msgs::Point32& point);
931 
938  static geometry_msgs::Point32 convertPoint32(const Eigen::Vector3d& point);
939 
946  static geometry_msgs::Point convertPoint(const geometry_msgs::Vector3& point);
947 
954  static geometry_msgs::Point convertPoint(const Eigen::Vector3d& point);
955 
965  static Eigen::Affine3d convertFromXYZRPY(double x, double y, double z, double roll, double pitch, double yaw);
967  static Eigen::Affine3d convertFromXYZRPY(const std::vector<double>& transform6); // TODO(davetcoleman): add new
968  // version of this function
969 
980  static Eigen::Affine3d convertFromXYZRPY(double tx, double ty, double tz, double rx, double ry, double rz,
981  EulerConvention convention); // ZYX is ROS standard
982  static Eigen::Affine3d convertFromXYZRPY(const std::vector<double>& transform6,
983  EulerConvention convention); // ZYX is ROS standard
984 
985  // TODO(davetcoleman): add opposite conversion that uses Eigen::Vector3d rpy = pose.rotation().eulerAngles(0, 1, 2);
986 
993  static void convertToXYZRPY(const Eigen::Affine3d& pose, std::vector<double>& xyzrpy);
994  static void convertToXYZRPY(const Eigen::Affine3d& pose, double& x, double& y, double& z, double& roll, double& pitch,
995  double& yaw);
1001  static void generateRandomPose(geometry_msgs::Pose& pose, RandomPoseBounds pose_bounds = RandomPoseBounds());
1002  static void generateRandomPose(Eigen::Affine3d& pose, RandomPoseBounds pose_bounds = RandomPoseBounds());
1003 
1007  static void generateRandomCuboid(geometry_msgs::Pose& cuboid_pose, double& depth, double& width, double& height,
1008  RandomPoseBounds pose_bounds = RandomPoseBounds(),
1009  RandomCuboidBounds cuboid_bounds = RandomCuboidBounds());
1010 
1015  static void generateEmptyPose(geometry_msgs::Pose& pose);
1016 
1024  static bool posesEqual(const Eigen::Affine3d& pose1, const Eigen::Affine3d& pose2, double threshold = 0.000001);
1025 
1029  static double dRand(double min, double max);
1030  static float fRand(float min, float max);
1031  static int iRand(int min, int max);
1032 
1036  static void printTranslation(const Eigen::Vector3d& translation);
1037 
1041  static void printTransform(const Eigen::Affine3d& transform);
1042 
1046  static void printTransformRPY(const Eigen::Affine3d& transform);
1047 
1051  static void printTransformFull(const Eigen::Affine3d& transform);
1052 
1054  bool getPsychedelicMode() const
1055  {
1056  return psychedelic_mode_;
1057  }
1058 
1060  void setPsychedelicMode(bool psychedelic_mode = true)
1061  {
1062  psychedelic_mode_ = psychedelic_mode;
1063  }
1064 
1066  void prompt(const std::string& msg);
1067 
1069  RemoteControlPtr& getRemoteControl();
1070 
1072  void loadRemoteControl();
1073 
1074 protected:
1075  // A shared node handle
1077 
1078  // Short name for this class
1079  static const std::string name_;
1080 
1081  // Optional remote control
1083 
1084  // ROS publishers
1085  ros::Publisher pub_rviz_markers_; // for rviz visualization markers
1086  bool pub_rviz_markers_connected_ = false;
1087  bool pub_rviz_markers_waited_ = false;
1088 
1089  // Strings
1090  std::string marker_topic_; // topic to publish to rviz
1091  std::string base_frame_; // name of base link
1092 
1093  // Duration to have Rviz markers persist, 0 for infinity
1095 
1096  // Settings
1097  bool batch_publishing_enabled_ = true;
1098  bool frame_locking_enabled_ = false;
1099  double alpha_ = 1.0; // opacity of all markers
1100  double global_scale_ = 1.0; // allow all markers to be increased by a constanct factor
1101 
1102  // Cached Rviz Marker Array
1103  visualization_msgs::MarkerArray markers_;
1104 
1105  // Cached Rviz markers
1106  visualization_msgs::Marker arrow_marker_;
1107  visualization_msgs::Marker sphere_marker_;
1108  visualization_msgs::Marker block_marker_;
1109  visualization_msgs::Marker cylinder_marker_;
1110  visualization_msgs::Marker mesh_marker_;
1111  visualization_msgs::Marker text_marker_;
1112  visualization_msgs::Marker cuboid_marker_;
1113  visualization_msgs::Marker line_strip_marker_;
1114  visualization_msgs::Marker line_list_marker_;
1115  visualization_msgs::Marker spheres_marker_;
1116  visualization_msgs::Marker reset_marker_;
1117  visualization_msgs::Marker triangle_marker_;
1118 
1119  // Just for fun.
1120  bool psychedelic_mode_ = false;
1121 
1122  // Chose random colors from this list
1123  static const std::array<colors, 14> all_rand_colors_;
1124 
1125 public:
1126  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
1127 }; // class
1128 
1129 typedef std::shared_ptr<RvizVisualTools> RvizVisualToolsPtr;
1130 typedef std::shared_ptr<const RvizVisualTools> RvizVisualToolsConstPtr;
1131 
1132 } // namespace rviz_visual_tools
1133 
1134 #endif // RVIZ_VISUAL_TOOLS_RVIZ_VISUAL_TOOLS_H
visualization_msgs::Marker block_marker_
bool getPsychedelicMode() const
Getter for PsychedelicMode.
visualization_msgs::Marker sphere_marker_
RVIZ_VISUAL_TOOLS_DEPRECATED bool triggerBatchPublish()
Trigger the publish function to send out all collected markers.
#define RVIZ_VISUAL_TOOLS_DEPRECATED
Definition: deprecation.h:48
double getGlobalScale() const
Getter for the global scale used for changing size of all markers.
RandomCuboidBounds(double cuboid_size_min=0.02, double cuboid_size_max=0.15)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void setBaseFrame(const std::string &base_frame)
Change the global base frame Note: this might reset all your current markers.
visualization_msgs::MarkerArray markers_
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
ROSCONSOLE_DECL void initialize()
visualization_msgs::Marker arrow_marker_
visualization_msgs::Marker text_marker_
void publishMesh(int id, float x, float y, float r, float g, float b, float a, bool use_embedded_materials, bool mesh)
std::shared_ptr< RemoteControl > RemoteControlPtr
void setMarkerTopic(const std::string &topic)
Set marker array topic.
static const std::array< colors, 14 > all_rand_colors_
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::shared_ptr< const RvizVisualTools > RvizVisualToolsConstPtr
std::shared_ptr< RvizVisualTools > RvizVisualToolsPtr
static const double SMALL_SCALE
void setGlobalScale(double global_scale)
Setter for the global scale used for changing size of all markers.
visualization_msgs::Marker cylinder_marker_
visualization_msgs::Marker line_list_marker_
Bounds for generateRandomPose()
visualization_msgs::Marker line_strip_marker_
TFSIMD_FORCE_INLINE const tfScalar & x() const
double min(double a, double b)
void setPsychedelicMode(bool psychedelic_mode=true)
Setter for PsychedelicMode.
visualization_msgs::Marker mesh_marker_
static const double LARGE_SCALE
Bounds for generateRandomCuboid()
TFSIMD_FORCE_INLINE const tfScalar & z() const
visualization_msgs::Marker spheres_marker_
const std::string getBaseFrame() const
Get the base frame.
static const std::string RVIZ_MARKER_TOPIC
visualization_msgs::Marker reset_marker_
visualization_msgs::Marker triangle_marker_
visualization_msgs::Marker cuboid_marker_
RandomPoseBounds(double x_min=0.0, double x_max=1.0, double y_min=0.0, double y_max=1.0, double z_min=0.0, double z_max=1.0, double elevation_min=0.0, double elevation_max=M_PI, double azimuth_min=0.0, double azimuth_max=2 *M_PI, double angle_min=0.0, double angle_max=2 *M_PI)
double max(double a, double b)
void setAlpha(double alpha)
Change the transparency of all markers published.
void publishText(int id, float x, float y, const std::string &text)


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