rviz_visual_tools.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, University of Colorado, Boulder
 *  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@dav.ee>, Andy McEvoy
   Desc:   Helper functions for displaying basic shape markers in Rviz
*/

#ifndef RVIZ_VISUAL_TOOLS_RVIZ_VISUAL_TOOLS_H
#define RVIZ_VISUAL_TOOLS_RVIZ_VISUAL_TOOLS_H

#include <ros/ros.h>

// C++
#include <string>
#include <vector>

// Eigen
#include <Eigen/Geometry>

// Rviz
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// Boost
#include <boost/shared_ptr.hpp>

// Messages
#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>

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
{
  XXSMALL,
  XSMALL,
  SMALL,
  REGULAR,
  LARGE,
  xLARGE,
  xxLARGE,
  xxxLARGE,
  XLARGE,
  XXLARGE
};

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(const std::string &base_frame, const std::string &marker_topic = RVIZ_MARKER_TOPIC);
  ~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);

  bool waitForSubscriber(const ros::Publisher &pub, const double &wait_time = 0.5);

  void setFloorToBaseHeight(double floor_to_base_height);

  void setAlpha(double alpha)
  {
    alpha_ = alpha;
  }
  void setLifetime(double lifetime);

  colors getRandColor();

  std_msgs::ColorRGBA getColor(const colors &color);

  std_msgs::ColorRGBA createRandColor();

  double slerp(double start, double end, double range, double value);

  std_msgs::ColorRGBA getColorScale(double value);

  geometry_msgs::Vector3 getScale(const scales &scale, bool arrow_scale = false, double marker_scale = 1.0);

  Eigen::Affine3d getVectorBetweenPoints(Eigen::Vector3d a, Eigen::Vector3d b);

  Eigen::Vector3d getCenterPoint(Eigen::Vector3d a, Eigen::Vector3d b);

  const std::string getBaseFrame()
  {
    return base_frame_;
  }
  void setBaseFrame(const std::string &base_frame)
  {
    base_frame_ = base_frame;
    loadRvizMarkers();
  }

  double getGlobalScale()
  {
    return global_scale_;
  }
  void setGlobalScale(double global_scale)
  {
    global_scale_ = global_scale;
  }
  bool publishMarker(visualization_msgs::Marker &marker);

  void enableBatchPublishing(bool enable = true);

  bool triggerBatchPublish();

  bool triggerBatchPublishAndDisable();

  bool publishMarkers(visualization_msgs::MarkerArray &markers);

  bool publishCone(const Eigen::Affine3d &pose, double angle, const rviz_visual_tools::colors &color = TRANSLUCENT,
                   double scale = 1.0);
  bool publishCone(const geometry_msgs::Pose &pose, double angle, const rviz_visual_tools::colors &color = TRANSLUCENT,
                   double scale = 1.0);

  bool publishXYPlane(const Eigen::Affine3d &pose, const rviz_visual_tools::colors &color = TRANSLUCENT,
                      double scale = 1.0);
  bool publishXYPlane(const geometry_msgs::Pose &pose, const rviz_visual_tools::colors &color = TRANSLUCENT,
                      double scale = 1.0);

  bool publishXZPlane(const Eigen::Affine3d &pose, const rviz_visual_tools::colors &color = TRANSLUCENT,
                      double scale = 1.0);
  bool publishXZPlane(const geometry_msgs::Pose &pose, const rviz_visual_tools::colors &color = TRANSLUCENT,
                      double scale = 1.0);

  bool publishYZPlane(const Eigen::Affine3d &pose, const rviz_visual_tools::colors &color = TRANSLUCENT,
                      double scale = 1.0);
  bool publishYZPlane(const geometry_msgs::Pose &pose, const rviz_visual_tools::colors &color = TRANSLUCENT,
                      double scale = 1.0);

  bool publishSphere(const Eigen::Affine3d &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                     const std::string &ns = "Sphere", const std::size_t &id = 0);
  bool publishSphere(const Eigen::Vector3d &point, const colors &color = BLUE, const scales &scale = REGULAR,
                     const std::string &ns = "Sphere", const std::size_t &id = 0);
  bool publishSphere(const Eigen::Vector3d &point, const colors &color, const double scale,
                     const std::string &ns = "Sphere", const std::size_t &id = 0);
  bool publishSphere(const geometry_msgs::Point &point, const colors &color = BLUE, const scales &scale = REGULAR,
                     const std::string &ns = "Sphere", const std::size_t &id = 0);
  bool publishSphere(const geometry_msgs::Pose &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                     const std::string &ns = "Sphere", const std::size_t &id = 0);
  bool publishSphere(const geometry_msgs::Pose &pose, const colors &color, const double scale,
                     const std::string &ns = "Sphere", const std::size_t &id = 0);
  bool publishSphere(const geometry_msgs::Pose &pose, const colors &color, const geometry_msgs::Vector3 scale,
                     const std::string &ns = "Sphere", const 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", const std::size_t &id = 0);
  bool publishSphere(const Eigen::Affine3d &pose, const std_msgs::ColorRGBA &color,
                     const geometry_msgs::Vector3 scale, const std::string &ns = "Sphere", const std::size_t &id = 0);
  bool publishSphere(const geometry_msgs::PoseStamped &pose, const colors &color, const geometry_msgs::Vector3 scale,
                     const std::string &ns = "Sphere", const std::size_t &id = 0);

  bool publishSpheres(const std::vector<Eigen::Vector3d> &points, const colors &color = BLUE, const double scale = 0.1,
                      const std::string &ns = "Spheres");
  bool publishSpheres(const std::vector<geometry_msgs::Point> &points, const colors &color = BLUE,
                      const double scale = 0.1, const std::string &ns = "Spheres");
  bool publishSpheres(const std::vector<geometry_msgs::Point> &points, const colors &color = BLUE,
                      const scales &scale = REGULAR, const std::string &ns = "Spheres");
  bool publishSpheres(const std::vector<geometry_msgs::Point> &points, const colors &color,
                      const geometry_msgs::Vector3 &scale, const std::string &ns = "Spheres");

  bool publishXArrow(const Eigen::Affine3d &pose, const colors &color = RED, const scales &scale = REGULAR,
                     double length = 0.1);
  bool publishXArrow(const geometry_msgs::Pose &pose, const colors &color = RED, const scales &scale = REGULAR,
                     double length = 0.1);
  bool publishXArrow(const geometry_msgs::PoseStamped &pose, const colors &color = RED, const scales &scale = REGULAR,
                     double length = 0.1);

  bool publishYArrow(const Eigen::Affine3d &pose, const colors &color = GREEN, const scales &scale = REGULAR,
                     double length = 0.1);
  bool publishYArrow(const geometry_msgs::Pose &pose, const colors &color = GREEN, const scales &scale = REGULAR,
                     double length = 0.1);
  bool publishYArrow(const geometry_msgs::PoseStamped &pose, const colors &color = GREEN, const scales &scale = REGULAR,
                     double length = 0.1);

  bool publishZArrow(const Eigen::Affine3d &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                     double length = 0.1, const std::size_t &id = 0);
  bool publishZArrow(const geometry_msgs::Pose &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                     double length = 0.1);
  bool publishZArrow(const geometry_msgs::PoseStamped &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                     double length = 0.1);
  bool publishZArrow(const geometry_msgs::PoseStamped &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                     double length = 0.1, const std::size_t &id = 0);

  bool publishArrow(const Eigen::Affine3d &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                    double length = 0.1, const std::size_t &id = 0);
  bool publishArrow(const geometry_msgs::Pose &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                    double length = 0.1, const std::size_t &id = 0);
  bool publishArrow(const geometry_msgs::PoseStamped &pose, const colors &color = BLUE, const scales &scale = REGULAR,
                    double length = 0.1, const std::size_t &id = 0);

  bool publishCuboid(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const colors &color = BLUE);
  bool publishCuboid(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
                     const colors &color = BLUE, const std::string &ns = "Cuboid", const std::size_t &id = 0);

  bool publishCuboid(const geometry_msgs::Pose &pose, const double depth, const double width, const double height,
                     const colors &color = BLUE);
  bool publishCuboid(const Eigen::Affine3d &pose, const double depth, const double width, const double height,
                     const colors &color = BLUE);

  bool publishLine(const Eigen::Affine3d &point1, const Eigen::Affine3d &point2, const colors &color = BLUE,
                   const scales &scale = REGULAR);
  bool publishLine(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const colors &color = BLUE,
                   const scales &scale = REGULAR);
  bool publishLine(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2,
                   const std_msgs::ColorRGBA &color, const scales &scale = REGULAR);
  bool publishLine(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2,
                   const std_msgs::ColorRGBA &color, const double &radius);
  bool publishLine(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2, const colors &color = BLUE,
                   const scales &scale = REGULAR);
  bool publishLine(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
                   const std_msgs::ColorRGBA &color, const scales &scale = REGULAR);
  bool publishLine(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
                   const std_msgs::ColorRGBA &color, const geometry_msgs::Vector3 &scale);

  bool publishPath(const std::vector<geometry_msgs::Point> &path, const colors &color = RED,
                   const scales &scale = REGULAR, const std::string &ns = "Path");

  bool publishPath(const std::vector<Eigen::Vector3d> &path, const colors &color = RED,
                   const double radius = 0.01, const std::string &ns = "Path");

  bool publishPolygon(const geometry_msgs::Polygon &polygon, const colors &color = RED, const scales &scale = REGULAR,
                      const std::string &ns = "Polygon");

  bool publishBlock(const geometry_msgs::Pose &pose, const colors &color = BLUE, const double &block_size = 0.1);
  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishBlock(const Eigen::Affine3d &pose, const colors &color = BLUE, const double &block_size = 0.1);

  bool publishWireframeCuboid(const Eigen::Affine3d &pose, double depth, double width, double height,
                              const rviz_visual_tools::colors &color = BLUE, const std::string &ns = "Wireframe Cuboid",
                              const std::size_t &id = 0);

  bool publishWireframeCuboid(const Eigen::Affine3d &pose, const Eigen::Vector3d &min_point,
                              const Eigen::Vector3d &max_point, const rviz_visual_tools::colors &color = BLUE,
                              const std::string &ns = "Wireframe Cuboid", const std::size_t &id = 0);

  bool publishWireframeRectangle(const Eigen::Affine3d &pose, const double &height, const double &width,
                                 const colors &color = BLUE, const scales &scale = REGULAR);
  bool publishWireframeRectangle(const Eigen::Affine3d &pose, const Eigen::Vector3d &p1, const Eigen::Vector3d &p2,
                                 const Eigen::Vector3d &p3, const Eigen::Vector3d &p4, const colors &color,
                                 const scales &scale);
  bool publishAxisLabeled(const Eigen::Affine3d &pose, const std::string &label, const scales &scale = SMALL,
                          const colors &color = WHITE);
  bool publishAxisLabeled(const geometry_msgs::Pose &pose, const std::string &label, const scales &scale = SMALL,
                          const colors &color = WHITE);

  bool publishAxis(const geometry_msgs::Pose &pose, double length = 0.1, double radius = 0.01,
                   const std::string &ns = "Axis");
  bool publishAxis(const Eigen::Affine3d &pose, double length = 0.1, double radius = 0.01,
                   const std::string &ns = "Axis");

  bool publishCylinder(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const colors &color = BLUE,
                       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::Affine3d &pose, const colors &color = BLUE, double height = 0.1,
                       double radius = 0.01, const std::string &ns = "Cylinder");
  bool publishCylinder(const geometry_msgs::Pose &pose, const 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::Affine3d &pose, const std::string &file_name, const colors &color = CLEAR,
                   double scale = 1, const std::string &ns = "mesh", const std::size_t &id = 0);
  bool publishMesh(const geometry_msgs::Pose &pose, const std::string &file_name, const colors &color = CLEAR,
                   double scale = 1, const std::string &ns = "mesh", const std::size_t &id = 0);

  bool publishGraph(const graph_msgs::GeometryGraph &graph, const colors &color, double radius);

  bool publishText(const Eigen::Affine3d &pose, const std::string &text, const colors &color = WHITE,
                   const scales &scale = REGULAR, bool static_id = true);
  bool publishText(const Eigen::Affine3d &pose, const std::string &text, const colors &color,
                   const geometry_msgs::Vector3 scale, bool static_id = true);
  bool publishText(const geometry_msgs::Pose &pose, const std::string &text, const colors &color = WHITE,
                   const scales &scale = REGULAR, bool static_id = true);
  bool publishText(const geometry_msgs::Pose &pose, const std::string &text, const colors &color,
                   const geometry_msgs::Vector3 scale, bool static_id = true);

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishTests();

  geometry_msgs::Pose convertPose(const Eigen::Affine3d &pose);

  static void convertPoseSafe(const Eigen::Affine3d &pose, geometry_msgs::Pose &pose_msg);

  Eigen::Affine3d convertPose(const geometry_msgs::Pose &pose);

  Eigen::Affine3d convertPoint32ToPose(const geometry_msgs::Point32 &point);

  geometry_msgs::Pose convertPointToPose(const geometry_msgs::Point &point);
  Eigen::Affine3d convertPointToPose(const Eigen::Vector3d &point);

  geometry_msgs::Point convertPoseToPoint(const Eigen::Affine3d &pose);

  Eigen::Vector3d convertPoint(const geometry_msgs::Point &point);

  Eigen::Vector3d convertPoint32(const geometry_msgs::Point32 &point);

  geometry_msgs::Point32 convertPoint32(const Eigen::Vector3d &point);

  geometry_msgs::Point convertPoint(const geometry_msgs::Vector3 &point);

  geometry_msgs::Point convertPoint(const Eigen::Vector3d &point);

  static Eigen::Affine3d convertFromXYZRPY(const double &x, const double &y, const double &z, const double &roll,
                                           const double &pitch, const double &yaw);
  static Eigen::Affine3d convertFromXYZRPY(std::vector<double> transform6);

  static void convertToXYZRPY(const Eigen::Affine3d &pose, std::vector<double> &xyzrpy);
  static void convertToXYZRPY(const Eigen::Affine3d &pose, double &x, double &y, double &z, double &roll, double &pitch,
                              double &yaw);
  void generateRandomPose(geometry_msgs::Pose &pose, RandomPoseBounds pose_bounds = RandomPoseBounds());
  void generateRandomPose(Eigen::Affine3d &pose, RandomPoseBounds pose_bounds = RandomPoseBounds());

  void generateRandomCuboid(geometry_msgs::Pose &cuboid_pose, double &depth, double &width, double &height,
                            RandomPoseBounds pose_bounds = RandomPoseBounds(),
                            RandomCuboidBounds cuboid_bounds = RandomCuboidBounds());

  void generateEmptyPose(geometry_msgs::Pose &pose);

  bool posesEqual(const Eigen::Affine3d &pose1, const Eigen::Affine3d &pose2, const 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 printTransform(const Eigen::Affine3d &transform);

  static void printTransformRPY(const Eigen::Affine3d &transform);

  const bool& getPsychedelicMode() const
  {
    return psychedelic_mode_;
  }

  void setPsychedelicMode(const bool& psychedelic_mode = true)
  {
    psychedelic_mode_ = psychedelic_mode;
  }

protected:
  void enableInternalBatchPublishing(bool enable);

  bool triggerInternalBatchPublishAndDisable();

  // A shared node handle
  ros::NodeHandle nh_;

  // Short name for this class
  std::string name_;

  // ROS publishers
  ros::Publisher pub_rviz_markers_;  // for rviz visualization markers
  bool pub_rviz_markers_connected_;
  bool pub_rviz_markers_waited_;

  // 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_;
  bool internal_batch_publishing_enabled_;  // this allows certain marker functions to batch publish
                                            // without breaking external functinality
  double alpha_;                            // opacity of all markers
  double global_scale_;                     // 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_;

  // Cached geometry variables used for conversion
  geometry_msgs::Pose shared_pose_msg_;
  geometry_msgs::Point shared_point_msg_;
  geometry_msgs::Point32 shared_point32_msg_;
  Eigen::Affine3d shared_pose_eigen_;
  Eigen::Vector3d shared_point_eigen_;

  // Just for fun.
  bool psychedelic_mode_;
};  // class

typedef boost::shared_ptr<RvizVisualTools> RvizVisualToolsPtr;
typedef boost::shared_ptr<const RvizVisualTools> RvizVisualToolsConstPtr;

}  // namespace rviz_visual_tools

#endif  // RVIZ_VISUAL_TOOLS_RVIZ_VISUAL_TOOLS_H