rviz_visual_tools.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, PickNik Consulting
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Univ of CO, Boulder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy
   Desc:   Helper functions for displaying basic shape markers in Rviz
*/
 
#pragma once
 
#include <ros/ros.h>
 
// C++
#include <string>
#include <vector>
 
// Eigen
#include <Eigen/Geometry>
#include <eigen_stl_containers/eigen_stl_vector_container.h>
 
// Rviz
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
 
// Boost
#include <boost/shared_ptr.hpp>
 
// Messages
#include <shape_msgs/Mesh.h>
#include <std_msgs/ColorRGBA.h>
#include <graph_msgs/GeometryGraph.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Polygon.h>
#include <trajectory_msgs/JointTrajectory.h>
 
// rviz_visual_tools
#include <rviz_visual_tools/deprecation.h>
#include <rviz_visual_tools/remote_control.h>
 
// Import/export for windows dll's and visibility for gcc shared libraries.
 
#ifdef ROS_BUILD_SHARED_LIBS      // ros is being built around shared libraries
#ifdef rviz_visual_tools_EXPORTS  // we are building a shared lib/dll
#define RVIZ_VISUAL_TOOLS_DECL ROS_HELPER_EXPORT
#else  // we are using shared lib/dll
#define RVIZ_VISUAL_TOOLS_DECL ROS_HELPER_IMPORT
#endif
#else  // ros is being built around static libraries
#define RVIZ_VISUAL_TOOLS_DECL
#endif
 
namespace rviz_visual_tools
{
// Default constants
static const std::string RVIZ_MARKER_TOPIC = "/rviz_visual_tools";
static const double SMALL_SCALE = 0.001;
static const double LARGE_SCALE = 100;
 
// Note: when adding new colors to colors, also add them to getRandColor() function
enum colors
{
  BLACK = 0,
  BROWN = 1,
  BLUE = 2,
  CYAN = 3,
  GREY = 4,
  DARK_GREY = 5,
  GREEN = 6,
  LIME_GREEN = 7,
  MAGENTA = 8,
  ORANGE = 9,
  PURPLE = 10,
  RED = 11,
  PINK = 12,
  WHITE = 13,
  YELLOW = 14,
  TRANSLUCENT = 15,
  TRANSLUCENT_LIGHT = 16,
  TRANSLUCENT_DARK = 17,
  RAND = 18,
  CLEAR = 19,
  DEFAULT = 20  // i.e. 'do not change default color'
};
 
enum scales
{
  XXXXSMALL = 1,
  XXXSMALL = 2,
  XXSMALL = 3,
  XSMALL = 4,
  SMALL = 5,
  MEDIUM = 6,  // same as REGULAR
  LARGE = 7,
  XLARGE = 8,
  XXLARGE = 9,
  XXXLARGE = 10,
  XXXXLARGE = 11,
};
 
enum EulerConvention
{
  XYZ = 0,
  ZYX,  // This is the ROS standard: http://www.ros.org/reps/rep-0103.html
  ZXZ
};
 
struct RandomPoseBounds
{
  double x_min_, x_max_;
  double y_min_, y_max_;
  double z_min_, z_max_;
  double elevation_min_, elevation_max_;
  double azimuth_min_, azimuth_max_;
  double angle_min_, angle_max_;
 
  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)
  {
    x_min_ = x_min;
    x_max_ = x_max;
    y_min_ = y_min;
    y_max_ = y_max;
    z_min_ = z_min;
    z_max_ = z_max;
    elevation_min_ = elevation_min;
    elevation_max_ = elevation_max;
    azimuth_min_ = azimuth_min;
    azimuth_max_ = azimuth_max;
    angle_min_ = angle_min;
    angle_max_ = angle_max;
  }
};
 
struct RandomCuboidBounds
{
  double cuboid_size_min_, cuboid_size_max_;
 
  explicit RandomCuboidBounds(double cuboid_size_min = 0.02, double cuboid_size_max = 0.15)
  {
    cuboid_size_min_ = cuboid_size_min;
    cuboid_size_max_ = cuboid_size_max;
  }
};
 
class RvizVisualTools
{
private:
  void initialize();
 
public:
  explicit RvizVisualTools(std::string base_frame, std::string marker_topic = RVIZ_MARKER_TOPIC,
                           ros::NodeHandle nh = ros::NodeHandle("~"));
  ~RvizVisualTools()
  {
  }
 
  bool deleteAllMarkers();
 
  void resetMarkerCounts();
 
  bool loadRvizMarkers();
 
  void setMarkerTopic(const std::string& topic)
  {
    marker_topic_ = topic;
  }
 
  void loadMarkerPub(bool wait_for_subscriber = false, bool latched = false);
 
  void waitForMarkerPub();
  void waitForMarkerPub(double wait_time);
 
  bool waitForSubscriber(const ros::Publisher& pub, double wait_time = 0.5, bool blocking = false);
 
  void setAlpha(double alpha)
  {
    alpha_ = alpha;
  }
  void setLifetime(double lifetime);
 
  static colors getRandColor();
 
  std_msgs::ColorRGBA getColor(colors color) const;
 
  static colors intToRvizColor(std::size_t color);
 
  static rviz_visual_tools::scales intToRvizScale(std::size_t scale);
 
  static std::string scaleToString(scales scale);
 
  std_msgs::ColorRGBA createRandColor() const;
 
  static double slerp(double start, double end, double range, double value);
 
  std_msgs::ColorRGBA getColorScale(double value) const;
 
  geometry_msgs::Vector3 getScale(scales scale, double marker_scale = 1.0) const;
 
  Eigen::Isometry3d getVectorBetweenPoints(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const;
 
  Eigen::Vector3d getCenterPoint(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const;
 
  const std::string getBaseFrame() const
  {
    return base_frame_;
  }
  void setBaseFrame(const std::string& base_frame)
  {
    base_frame_ = base_frame;
    loadRvizMarkers();
  }
 
  double getGlobalScale() const
  {
    return global_scale_;
  }
  void setGlobalScale(double global_scale)
  {
    global_scale_ = global_scale;
  }
  bool publishMarker(visualization_msgs::Marker& marker);
 
  void enableBatchPublishing(bool enable = true);
 
  void enableFrameLocking(bool enable = true);
 
  bool triggerEvery(std::size_t queueSize);
 
  bool trigger();
 
  bool publishMarkers(visualization_msgs::MarkerArray& markers);
 
  bool publishCone(const Eigen::Isometry3d& pose, double angle, colors color = TRANSLUCENT, double scale = 1.0);
  bool publishCone(const geometry_msgs::Pose& pose, double angle, colors color = TRANSLUCENT, double scale = 1.0);
 
  bool publishABCDPlane(const double A, const double B, const double C, const double D, colors color = TRANSLUCENT,
                        double x_width = 1.0, double y_width = 1.0);
 
  bool publishXYPlane(const Eigen::Isometry3d& pose, colors color = TRANSLUCENT, double scale = 1.0);
  bool publishXYPlane(const geometry_msgs::Pose& pose, colors color = TRANSLUCENT, double scale = 1.0);
 
  bool publishXZPlane(const Eigen::Isometry3d& pose, colors color = TRANSLUCENT, double scale = 1.0);
  bool publishXZPlane(const geometry_msgs::Pose& pose, colors color = TRANSLUCENT, double scale = 1.0);
 
  bool publishYZPlane(const Eigen::Isometry3d& pose, colors color = TRANSLUCENT, double scale = 1.0);
  bool publishYZPlane(const geometry_msgs::Pose& pose, colors color = TRANSLUCENT, double scale = 1.0);
 
  bool publishSphere(const Eigen::Isometry3d& pose, colors color = BLUE, scales scale = MEDIUM,
                     const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const Eigen::Vector3d& point, colors color = BLUE, scales scale = MEDIUM,
                     const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const Eigen::Vector3d& point, colors color, double scale, const std::string& ns = "Sphere",
                     std::size_t id = 0);
  bool publishSphere(const geometry_msgs::Point& point, colors color = BLUE, scales scale = MEDIUM,
                     const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const geometry_msgs::Pose& pose, colors color = BLUE, scales scale = MEDIUM,
                     const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const geometry_msgs::Pose& pose, colors color, double scale, const std::string& ns = "Sphere",
                     std::size_t id = 0);
  bool publishSphere(const geometry_msgs::Pose& pose, colors color, const geometry_msgs::Vector3 scale,
                     const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color,
                     const geometry_msgs::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const Eigen::Isometry3d& pose, const std_msgs::ColorRGBA& color,
                     const geometry_msgs::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const Eigen::Vector3d& point, const std_msgs::ColorRGBA& color, const geometry_msgs::Vector3 scale,
                     const std::string& ns = "Sphere", std::size_t id = 0);
  bool publishSphere(const geometry_msgs::PoseStamped& pose, colors color, const geometry_msgs::Vector3 scale,
                     const std::string& ns = "Sphere", std::size_t id = 0);
 
  bool publishSpheres(const EigenSTL::vector_Vector3d& points, colors color = BLUE, scales scale = MEDIUM,
                      const std::string& ns = "Spheres");
  bool publishSpheres(const EigenSTL::vector_Vector3d& points, colors color, double scale = 0.1,
                      const std::string& ns = "Spheres");
  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color = BLUE, scales scale = MEDIUM,
                      const std::string& ns = "Spheres");
  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color = BLUE, double scale = 0.1,
                      const std::string& ns = "Spheres");
  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, colors color,
                      const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres");
 
  bool publishSpheres(const EigenSTL::vector_Vector3d& points, const std::vector<colors>& colors, scales scale = MEDIUM,
                      const std::string& ns = "Spheres");
  bool publishSpheres(const std::vector<geometry_msgs::Point>& points, const std::vector<std_msgs::ColorRGBA>& colors,
                      const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres");
 
  bool publishXArrow(const Eigen::Isometry3d& pose, colors color = RED, scales scale = MEDIUM, double length = 0.0);
  bool publishXArrow(const geometry_msgs::Pose& pose, colors color = RED, scales scale = MEDIUM, double length = 0.0);
  bool publishXArrow(const geometry_msgs::PoseStamped& pose, colors color = RED, scales scale = MEDIUM,
                     double length = 0.0);
 
  bool publishYArrow(const Eigen::Isometry3d& pose, colors color = GREEN, scales scale = MEDIUM, double length = 0.0);
  bool publishYArrow(const geometry_msgs::Pose& pose, colors color = GREEN, scales scale = MEDIUM, double length = 0.0);
  bool publishYArrow(const geometry_msgs::PoseStamped& pose, colors color = GREEN, scales scale = MEDIUM,
                     double length = 0.0);
 
  bool publishZArrow(const Eigen::Isometry3d& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0,
                     std::size_t id = 0);
  bool publishZArrow(const geometry_msgs::Pose& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0);
  bool publishZArrow(const geometry_msgs::PoseStamped& pose, colors color = BLUE, scales scale = MEDIUM,
                     double length = 0.0);
  bool publishZArrow(const geometry_msgs::PoseStamped& pose, colors color = BLUE, scales scale = MEDIUM,
                     double length = 0.0, std::size_t id = 0);
 
  bool publishArrow(const Eigen::Isometry3d& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0,
                    std::size_t id = 0);
  bool publishArrow(const geometry_msgs::Pose& pose, colors color = BLUE, scales scale = MEDIUM, double length = 0.0,
                    std::size_t id = 0);
  bool publishArrow(const geometry_msgs::PoseStamped& pose, colors color = BLUE, scales scale = MEDIUM,
                    double length = 0.0, std::size_t id = 0);
  bool publishArrow(const geometry_msgs::Point& start, const geometry_msgs::Point& end, colors color = BLUE,
                    scales scale = MEDIUM, std::size_t id = 0);
 
  bool publishCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color = BLUE);
  bool publishCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2, colors color = BLUE,
                     const std::string& ns = "Cuboid", std::size_t id = 0);
 
  bool publishCuboid(const geometry_msgs::Pose& pose, double depth, double width, double height, colors color = BLUE);
  bool publishCuboid(const Eigen::Isometry3d& pose, double depth, double width, double height, colors color = BLUE);
 
  bool publishCuboid(const Eigen::Isometry3d& pose, const Eigen::Vector3d& size, colors color = BLUE);
  bool publishCuboid(const geometry_msgs::Pose& pose, const geometry_msgs::Vector3& size, colors color = BLUE);
 
  bool publishLine(const Eigen::Isometry3d& point1, const Eigen::Isometry3d& point2, colors color = BLUE,
                   scales scale = MEDIUM);
  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color = BLUE,
                   scales scale = MEDIUM);
  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color, double radius);
  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::ColorRGBA& color,
                   scales scale = MEDIUM);
  bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::ColorRGBA& color,
                   double radius);
  bool publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2, colors color = BLUE,
                   scales scale = MEDIUM);
  bool publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
                   const std_msgs::ColorRGBA& color, scales scale = MEDIUM);
  bool publishLine(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
                   const std_msgs::ColorRGBA& color, const geometry_msgs::Vector3& scale);
 
  bool publishLines(const EigenSTL::vector_Vector3d& aPoints, const EigenSTL::vector_Vector3d& bPoints,
                    const std::vector<colors>& colors, scales scale = MEDIUM);
  bool publishLines(const std::vector<geometry_msgs::Point>& aPoints, const std::vector<geometry_msgs::Point>& bPoints,
                    const std::vector<std_msgs::ColorRGBA>& colors, const geometry_msgs::Vector3& scale);
 
  bool publishLineStrip(const std::vector<geometry_msgs::Point>& path, colors color = RED, scales scale = MEDIUM,
                        const std::string& ns = "Path");
 
  bool publishPath(const std::vector<geometry_msgs::Pose>& path, colors color = RED, scales scale = MEDIUM,
                   const std::string& ns = "Path");
  bool publishPath(const std::vector<geometry_msgs::Point>& path, colors color, scales scale,
                   const std::string& ns = "Path");
  bool publishPath(const EigenSTL::vector_Isometry3d& path, colors color, scales scale, const std::string& ns = "Path");
  bool publishPath(const EigenSTL::vector_Vector3d& path, colors color, scales scale, const std::string& ns = "Path");
  bool publishPath(const std::vector<geometry_msgs::Point>& path, colors color = RED, double radius = 0.01,
                   const std::string& ns = "Path");
  bool publishPath(const EigenSTL::vector_Vector3d& path, colors color = RED, double radius = 0.01,
                   const std::string& ns = "Path");
  bool publishPath(const EigenSTL::vector_Isometry3d& path, colors color = RED, double radius = 0.01,
                   const std::string& ns = "Path");
 
  bool publishPath(const EigenSTL::vector_Vector3d& path, const std::vector<colors>& colors, double radius = 0.01,
                   const std::string& ns = "Path");
 
  bool publishPath(const EigenSTL::vector_Vector3d& path, const std::vector<std_msgs::ColorRGBA>& colors, double radius,
                   const std::string& ns = "Path");
 
  bool publishPolygon(const geometry_msgs::Polygon& polygon, colors color = RED, scales scale = MEDIUM,
                      const std::string& ns = "Polygon");
 
  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);
 
  bool publishWireframeCuboid(const Eigen::Isometry3d& pose, const Eigen::Vector3d& min_point,
                              const Eigen::Vector3d& max_point, colors color = BLUE,
                              const std::string& ns = "Wireframe Cuboid", std::size_t id = 0);
 
  bool publishWireframeRectangle(const Eigen::Isometry3d& pose, double height, double width, colors color = BLUE,
                                 scales scale = MEDIUM, std::size_t id = 0);
  bool publishWireframeRectangle(const Eigen::Isometry3d& pose, const Eigen::Vector3d& p1_in,
                                 const Eigen::Vector3d& p2_in, const Eigen::Vector3d& p3_in,
                                 const Eigen::Vector3d& p4_in, colors color, scales scale);
  bool publishAxisLabeled(const Eigen::Isometry3d& pose, const std::string& label, scales scale = MEDIUM,
                          colors color = WHITE);
  bool publishAxisLabeled(const geometry_msgs::Pose& pose, const std::string& label, scales scale = MEDIUM,
                          colors color = WHITE);
 
  bool publishAxis(const geometry_msgs::Pose& pose, scales scale = MEDIUM, const std::string& ns = "Axis");
  bool publishAxis(const Eigen::Isometry3d& pose, scales scale = MEDIUM, const std::string& ns = "Axis");
  bool publishAxis(const geometry_msgs::Pose& pose, double length, double radius = 0.01, const std::string& ns = "Axis");
  bool publishAxis(const Eigen::Isometry3d& pose, double length, double radius = 0.01, const std::string& ns = "Axis");
 
private:
  bool publishAxisInternal(const Eigen::Isometry3d& pose, double length = 0.1, double radius = 0.01,
                           const std::string& ns = "Axis");
 
public:
  bool publishAxisPath(const EigenSTL::vector_Isometry3d& path, scales scale = MEDIUM,
                       const std::string& ns = "Axis Path");
  bool publishAxisPath(const EigenSTL::vector_Isometry3d& path, double length = 0.1, double radius = 0.01,
                       const std::string& ns = "Axis Path");
 
  bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color = BLUE,
                       scales scale = MEDIUM, const std::string& ns = "Cylinder");
  bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color, double radius = 0.01,
                       const std::string& ns = "Cylinder");
  bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::ColorRGBA& color,
                       double radius = 0.01, const std::string& ns = "Cylinder");
 
  bool publishCylinder(const Eigen::Isometry3d& pose, colors color = BLUE, double height = 0.1, double radius = 0.01,
                       const std::string& ns = "Cylinder");
  bool publishCylinder(const geometry_msgs::Pose& pose, colors color = BLUE, double height = 0.1, double radius = 0.01,
                       const std::string& ns = "Cylinder");
  bool publishCylinder(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color, double height = 0.1,
                       double radius = 0.01, const std::string& ns = "Cylinder");
 
  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);
  bool publishMesh(const geometry_msgs::Pose& pose, const std::string& file_name, colors color = CLEAR,
                   double scale = 1, const std::string& ns = "mesh", std::size_t id = 0);
 
  bool publishMesh(const Eigen::Isometry3d& pose, const shape_msgs::Mesh& mesh, colors color = CLEAR, double scale = 1,
                   const std::string& ns = "mesh", std::size_t id = 0);
  bool publishMesh(const geometry_msgs::Pose& pose, const shape_msgs::Mesh& mesh, colors color = CLEAR,
                   double scale = 1, const std::string& ns = "mesh", std::size_t id = 0);
 
  bool publishGraph(const graph_msgs::GeometryGraph& graph, colors color, double radius);
 
  bool publishText(const Eigen::Isometry3d& pose, const std::string& text, colors color = WHITE, scales scale = MEDIUM,
                   bool static_id = true);
  bool publishText(const Eigen::Isometry3d& pose, const std::string& text, colors color,
                   const geometry_msgs::Vector3 scale, bool static_id = true);
  bool publishText(const geometry_msgs::Pose& pose, const std::string& text, colors color = WHITE,
                   scales scale = MEDIUM, bool static_id = true);
  bool publishText(const geometry_msgs::Pose& pose, const std::string& text, colors color,
                   const geometry_msgs::Vector3 scale, bool static_id = true);
 
  static geometry_msgs::Pose convertPose(const Eigen::Isometry3d& pose);
 
  static void convertPoseSafe(const Eigen::Isometry3d& pose, geometry_msgs::Pose& pose_msg);
 
  static Eigen::Isometry3d convertPose(const geometry_msgs::Pose& pose);
 
  static void convertPoseSafe(const geometry_msgs::Pose& pose_msg, Eigen::Isometry3d& pose);
 
  static Eigen::Isometry3d convertPoint32ToPose(const geometry_msgs::Point32& point);
 
  static geometry_msgs::Pose convertPointToPose(const geometry_msgs::Point& point);
  static Eigen::Isometry3d convertPointToPose(const Eigen::Vector3d& point);
 
  static geometry_msgs::Point convertPoseToPoint(const Eigen::Isometry3d& pose);
 
  static Eigen::Vector3d convertPoint(const geometry_msgs::Point& point);
 
  static Eigen::Vector3d convertPoint32(const geometry_msgs::Point32& point);
 
  static geometry_msgs::Point32 convertPoint32(const Eigen::Vector3d& point);
 
  static geometry_msgs::Point convertPoint(const geometry_msgs::Vector3& point);
 
  static geometry_msgs::Point convertPoint(const Eigen::Vector3d& point);
 
  static Eigen::Isometry3d convertFromXYZRPY(double tx, double ty, double tz, double rx, double ry, double rz,
                                             EulerConvention convention);  // ZYX is ROS standard
  static Eigen::Isometry3d convertFromXYZRPY(const std::vector<double>& transform6,
                                             EulerConvention convention);  // ZYX is ROS standard
 
  // TODO(davetcoleman): add opposite conversion that uses   Eigen::Vector3d rpy = pose.rotation().eulerAngles(0, 1, 2);
 
  static void convertToXYZRPY(const Eigen::Isometry3d& pose, std::vector<double>& xyzrpy);
  static void convertToXYZRPY(const Eigen::Isometry3d& pose, double& x, double& y, double& z, double& roll,
                              double& pitch, double& yaw);
  static void generateRandomPose(geometry_msgs::Pose& pose, RandomPoseBounds pose_bounds = RandomPoseBounds());
  static void generateRandomPose(Eigen::Isometry3d& pose, RandomPoseBounds pose_bounds = RandomPoseBounds());
 
  static void generateRandomCuboid(geometry_msgs::Pose& cuboid_pose, double& depth, double& width, double& height,
                                   RandomPoseBounds pose_bounds = RandomPoseBounds(),
                                   RandomCuboidBounds cuboid_bounds = RandomCuboidBounds());
 
  static geometry_msgs::Pose getIdentityPose();
 
  static bool posesEqual(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double threshold = 0.000001);
 
  static double dRand(double min, double max);
  static float fRand(float min, float max);
  static int iRand(int min, int max);
 
  static void printTranslation(const Eigen::Vector3d& translation);
 
  static void printTransform(const Eigen::Isometry3d& transform);
 
  static void printTransformRPY(const Eigen::Isometry3d& transform);
 
  static void printTransformFull(const Eigen::Isometry3d& transform);
 
  bool getPsychedelicMode() const
  {
    return psychedelic_mode_;
  }
 
  void setPsychedelicMode(bool psychedelic_mode = true)
  {
    psychedelic_mode_ = psychedelic_mode;
  }
 
  void prompt(const std::string& msg);
 
  RemoteControlPtr& getRemoteControl();
 
  void loadRemoteControl();
 
protected:
  // A shared node handle
  ros::NodeHandle nh_;
 
  // Short name for this class
  static RVIZ_VISUAL_TOOLS_DECL const std::string NAME;
 
  // Optional remote control
  RemoteControlPtr remote_control_;
 
  // ROS publishers
  ros::Publisher pub_rviz_markers_;  // for rviz visualization markers
  bool pub_rviz_markers_connected_ = false;
  bool pub_rviz_markers_waited_ = false;
 
  // Strings
  std::string marker_topic_;  // topic to publish to rviz
  std::string base_frame_;    // name of base link
 
  // Duration to have Rviz markers persist, 0 for infinity
  ros::Duration marker_lifetime_;
 
  // Settings
  bool batch_publishing_enabled_ = true;
  bool frame_locking_enabled_ = false;
  double alpha_ = 1.0;         // opacity of all markers
  double global_scale_ = 1.0;  // allow all markers to be increased by a constanct factor
 
  // Cached Rviz Marker Array
  visualization_msgs::MarkerArray markers_;
 
  // Cached Rviz markers
  visualization_msgs::Marker arrow_marker_;
  visualization_msgs::Marker sphere_marker_;
  visualization_msgs::Marker block_marker_;
  visualization_msgs::Marker cylinder_marker_;
  visualization_msgs::Marker mesh_marker_;
  visualization_msgs::Marker text_marker_;
  visualization_msgs::Marker cuboid_marker_;
  visualization_msgs::Marker line_strip_marker_;
  visualization_msgs::Marker line_list_marker_;
  visualization_msgs::Marker spheres_marker_;
  visualization_msgs::Marker reset_marker_;
  visualization_msgs::Marker triangle_marker_;
 
  // Just for fun.
  bool psychedelic_mode_ = false;
 
  // Chose random colors from this list
  static const std::array<colors, 14> ALL_RAND_COLORS;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
};                                 // class
 
typedef std::shared_ptr<RvizVisualTools> RvizVisualToolsPtr;
typedef std::shared_ptr<const RvizVisualTools> RvizVisualToolsConstPtr;
 
}  // namespace rviz_visual_tools