Program Listing for File rviz_visual_tools.hpp
↰ Return to documentation for file (/tmp/ws/src/rviz_visual_tools/include/rviz_visual_tools/rviz_visual_tools.hpp)
// Copyright 2021 PickNik Inc.
//
// 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 PickNik Inc. 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 HOLDER 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
// rviz_visual_tools
#include <rviz_visual_tools/remote_control.hpp>
#include <rclcpp/rclcpp.hpp>
// C++
#include <string>
#include <vector>
// Eigen
#include <Eigen/Geometry>
#include <eigen_stl_containers/eigen_stl_vector_container.h>
// Rviz
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
// Messages
#include <shape_msgs/msg/mesh.hpp>
#include <std_msgs/msg/color_rgba.hpp>
// #include <graph_msgs/msg/geometry_graph.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/polygon.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
// 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:
template <typename NodePtr>
explicit RvizVisualTools(const std::string& base_frame, const std::string& marker_topic,
NodePtr node, const RemoteControlPtr& remote_control = nullptr)
: RvizVisualTools(base_frame, marker_topic, node->get_node_base_interface(),
node->get_node_topics_interface(), node->get_node_graph_interface(),
node->get_node_clock_interface(), node->get_node_logging_interface(),
remote_control)
{
}
explicit RvizVisualTools(
const std::string& base_frame, const std::string& marker_topic,
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr& node_base_interface,
const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr& topics_interface,
const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr& graph_interface,
const rclcpp::node_interfaces::NodeClockInterface::SharedPtr& clock_interface,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr& logging_interface,
const RemoteControlPtr& remote_control = nullptr);
~RvizVisualTools()
{
}
bool deleteMarker(const std::string& ns, std::size_t id);
bool deleteAllMarkers();
bool deleteAllMarkers(const std::string& ns);
void resetMarkerCounts();
bool loadRvizMarkers();
void setMarkerTopic(const std::string& topic)
{
marker_topic_ = topic;
}
void loadMarkerPub(bool wait_for_subscriber = true);
bool waitForMarkerSub(double wait_time = 5);
[[deprecated("waitForMarkerPub deprecated. Use waitForMarkerSub instad")]] bool
waitForMarkerPub(double wait_time = 5)
{
return waitForMarkerSub(wait_time);
}
template <typename MessageT>
bool waitForSubscriber(std::shared_ptr<rclcpp::Publisher<MessageT> >& pub, double wait_time = 5.0)
{
// if the user does not want to wait return no connection
if (!wait_for_subscriber_)
{
return false;
}
// Will wait at most this amount of time
rclcpp::Time max_time(clock_interface_->get_clock()->now() +
rclcpp::Duration::from_seconds(wait_time));
// This is wrong. It returns only the number of subscribers that have already
// established their direct connections to this publisher
// How often to check for subscribers
rclcpp::Duration loop_duration = rclcpp::Duration::from_seconds(1.0 / 200.0);
if (!pub)
{
RCLCPP_ERROR(logger_,
"loadMarkerPub() has not been called yet, unable to wait for subscriber.");
}
std::string topic_name = pub->get_topic_name();
int num_existing_subscribers = graph_interface_->count_subscribers(topic_name);
if (wait_time > 0 && num_existing_subscribers == 0)
{
RCLCPP_INFO_STREAM(logger_, "Topic " << pub->get_topic_name() << " waiting " << wait_time
<< " seconds for subscriber.");
}
// Wait for subscriber
while (wait_time > 0 && num_existing_subscribers == 0 && rclcpp::ok())
{
if (clock_interface_->get_clock()->now() > max_time) // Check if timed out
{
RCLCPP_WARN_STREAM(
logger_,
"Topic " << pub->get_topic_name() << " unable to connect to any subscribers within "
<< wait_time
<< " sec. It is possible initially published visual messages will be lost.");
pub_rviz_markers_connected_ = false;
return pub_rviz_markers_connected_;
}
// Check again
num_existing_subscribers = graph_interface_->count_subscribers(topic_name);
// Sleep
rclcpp::sleep_for(std::chrono::nanoseconds(loop_duration.nanoseconds()));
}
if (!rclcpp::ok())
{
pub_rviz_markers_connected_ = false;
return false;
}
pub_rviz_markers_connected_ = (num_existing_subscribers != 0);
return pub_rviz_markers_connected_;
}
void setAlpha(double alpha)
{
alpha_ = alpha;
}
void setLifetime(double lifetime);
static Colors getRandColor();
std_msgs::msg::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::msg::ColorRGBA createRandColor() const;
static double slerp(double start, double end, double range, double value);
std_msgs::msg::ColorRGBA getColorScale(double value) const;
geometry_msgs::msg::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::msg::Marker& marker);
void enableBatchPublishing(bool enable = true);
void enableFrameLocking(bool enable = true);
bool triggerEvery(std::size_t queueSize);
bool trigger();
bool publishMarkers(visualization_msgs::msg::MarkerArray& markers);
bool publishCone(const Eigen::Isometry3d& pose, double angle, Colors color = TRANSLUCENT,
double scale = 1.0);
bool publishCone(const geometry_msgs::msg::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 publishNormalAndDistancePlane(const Eigen::Vector3d& normal, const double d,
const Colors color = TRANSLUCENT, const double x_width = 1.0,
const double y_width = 1.0);
bool publishXYPlane(const Eigen::Isometry3d& pose, Colors color = TRANSLUCENT,
double scale = 1.0);
bool publishXYPlane(const geometry_msgs::msg::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::msg::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::msg::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::msg::Point& point, Colors color = BLUE,
Scales scale = MEDIUM, const std::string& ns = "Sphere", std::size_t id = 0);
bool publishSphere(const geometry_msgs::msg::Pose& pose, Colors color = BLUE,
Scales scale = MEDIUM, const std::string& ns = "Sphere", std::size_t id = 0);
bool publishSphere(const geometry_msgs::msg::Pose& pose, Colors color, double scale,
const std::string& ns = "Sphere", std::size_t id = 0);
bool publishSphere(const geometry_msgs::msg::Pose& pose, Colors color,
const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere",
std::size_t id = 0);
bool publishSphere(const geometry_msgs::msg::Pose& pose, const std_msgs::msg::ColorRGBA& color,
const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere",
std::size_t id = 0);
bool publishSphere(const Eigen::Isometry3d& pose, const std_msgs::msg::ColorRGBA& color,
const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere",
std::size_t id = 0);
bool publishSphere(const Eigen::Vector3d& point, const std_msgs::msg::ColorRGBA& color,
const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere",
std::size_t id = 0);
bool publishSphere(const geometry_msgs::msg::PoseStamped& pose, Colors color,
const geometry_msgs::msg::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::msg::Point>& points, Colors color = BLUE,
Scales scale = MEDIUM, const std::string& ns = "Spheres");
bool publishSpheres(const std::vector<geometry_msgs::msg::Point>& points, Colors color = BLUE,
double scale = 0.1, const std::string& ns = "Spheres");
bool publishSpheres(const std::vector<geometry_msgs::msg::Point>& points, Colors color,
const geometry_msgs::msg::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::msg::Point>& points,
const std::vector<std_msgs::msg::ColorRGBA>& colors,
const geometry_msgs::msg::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::msg::Pose& pose, Colors color = RED,
Scales scale = MEDIUM, double length = 0.0);
bool publishXArrow(const geometry_msgs::msg::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::msg::Pose& pose, Colors color = GREEN,
Scales scale = MEDIUM, double length = 0.0);
bool publishYArrow(const geometry_msgs::msg::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::msg::Pose& pose, Colors color = BLUE,
Scales scale = MEDIUM, double length = 0.0);
bool publishZArrow(const geometry_msgs::msg::PoseStamped& pose, Colors color = BLUE,
Scales scale = MEDIUM, double length = 0.0);
bool publishZArrow(const geometry_msgs::msg::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::msg::Pose& pose, Colors color = BLUE,
Scales scale = MEDIUM, double length = 0.0, std::size_t id = 0);
bool publishArrow(const geometry_msgs::msg::PoseStamped& pose, Colors color = BLUE,
Scales scale = MEDIUM, double length = 0.0, std::size_t id = 0);
bool publishArrow(const geometry_msgs::msg::Point& start, const geometry_msgs::msg::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 Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
const std_msgs::msg::ColorRGBA& color);
bool publishCuboid(const geometry_msgs::msg::Point& point1,
const geometry_msgs::msg::Point& point2, Colors color = BLUE,
const std::string& ns = "Cuboid", std::size_t id = 0);
bool publishCuboid(const geometry_msgs::msg::Point& point1,
const geometry_msgs::msg::Point& point2, const std_msgs::msg::ColorRGBA& color,
const std::string& ns = "Cuboid", std::size_t id = 0);
bool publishCuboid(const Eigen::Isometry3d& pose, double depth, double width, double height,
const std_msgs::msg::ColorRGBA& color);
bool publishCuboid(const Eigen::Isometry3d& pose, double depth, double width, double height,
Colors color = BLUE);
bool publishCuboid(const geometry_msgs::msg::Pose& pose, double depth, double width,
double height, Colors color = BLUE);
bool publishCuboid(const geometry_msgs::msg::Pose& pose, double depth, double width,
double height, const std_msgs::msg::ColorRGBA& color);
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::msg::ColorRGBA& color, Scales scale = MEDIUM);
bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
const std_msgs::msg::ColorRGBA& color, double radius);
bool publishLine(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2,
Colors color = BLUE, Scales scale = MEDIUM);
bool publishLine(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2,
const std_msgs::msg::ColorRGBA& color, Scales scale = MEDIUM);
bool publishLine(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2,
const std_msgs::msg::ColorRGBA& color, const geometry_msgs::msg::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::msg::Point>& aPoints,
const std::vector<geometry_msgs::msg::Point>& bPoints,
const std::vector<std_msgs::msg::ColorRGBA>& colors,
const geometry_msgs::msg::Vector3& scale);
bool publishLineStrip(const std::vector<geometry_msgs::msg::Point>& path, Colors color = RED,
Scales scale = MEDIUM, const std::string& ns = "Path");
bool publishPath(const std::vector<geometry_msgs::msg::Pose>& path, Colors color = RED,
Scales scale = MEDIUM, const std::string& ns = "Path");
bool publishPath(const std::vector<geometry_msgs::msg::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::msg::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::msg::ColorRGBA>& colors, double radius,
const std::string& ns = "Path");
bool publishPolygon(const geometry_msgs::msg::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::msg::Pose& pose, const std::string& label,
Scales scale = MEDIUM, Colors color = WHITE);
bool publishAxis(const geometry_msgs::msg::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::msg::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::msg::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::msg::Pose& pose, Colors color = BLUE,
double height = 0.1, double radius = 0.01,
const std::string& ns = "Cylinder");
bool publishCylinder(const geometry_msgs::msg::Pose& pose, const std_msgs::msg::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::msg::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::msg::Mesh& mesh,
Colors color = CLEAR, double scale = 1, const std::string& ns = "mesh",
std::size_t id = 0);
bool publishMesh(const geometry_msgs::msg::Pose& pose, const shape_msgs::msg::Mesh& mesh,
Colors color = CLEAR, double scale = 1, const std::string& ns = "mesh",
std::size_t id = 0);
// TODO(mlautman): port graph_msgs
// bool publishGraph(const graph_msgs::msg::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::msg::Vector3 scale, bool static_id = true);
bool publishText(const geometry_msgs::msg::Pose& pose, const std::string& text,
Colors color = WHITE, Scales scale = MEDIUM, bool static_id = true);
bool publishText(const geometry_msgs::msg::Pose& pose, const std::string& text, Colors color,
const geometry_msgs::msg::Vector3 scale, bool static_id = true);
static geometry_msgs::msg::Pose convertPose(const Eigen::Isometry3d& pose);
static void convertPoseSafe(const Eigen::Isometry3d& pose, geometry_msgs::msg::Pose& pose_msg);
static Eigen::Isometry3d convertPose(const geometry_msgs::msg::Pose& pose);
static void convertPoseSafe(const geometry_msgs::msg::Pose& pose_msg, Eigen::Isometry3d& pose);
static Eigen::Isometry3d convertPoint32ToPose(const geometry_msgs::msg::Point32& point);
static geometry_msgs::msg::Pose convertPointToPose(const geometry_msgs::msg::Point& point);
static Eigen::Isometry3d convertPointToPose(const Eigen::Vector3d& point);
static geometry_msgs::msg::Point convertPoseToPoint(const Eigen::Isometry3d& pose);
static Eigen::Vector3d convertPoint(const geometry_msgs::msg::Point& point);
static Eigen::Vector3d convertPoint32(const geometry_msgs::msg::Point32& point);
static geometry_msgs::msg::Point32 convertPoint32(const Eigen::Vector3d& point);
static geometry_msgs::msg::Point convertPoint(const geometry_msgs::msg::Vector3& point);
static geometry_msgs::msg::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::msg::Pose& pose,
RandomPoseBounds pose_bounds = RandomPoseBounds());
static void generateRandomPose(Eigen::Isometry3d& pose,
RandomPoseBounds pose_bounds = RandomPoseBounds());
static void generateRandomCuboid(geometry_msgs::msg::Pose& cuboid_pose, double& depth,
double& width, double& height,
RandomPoseBounds pose_bounds = RandomPoseBounds(),
RandomCuboidBounds cuboid_bounds = RandomCuboidBounds());
static geometry_msgs::msg::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;
}
bool prompt(const std::string& msg);
RemoteControlPtr& getRemoteControl();
void setRemoteControl(const RemoteControlPtr& remote_control);
void loadRemoteControl();
int32_t getArrowId() const;
int32_t getSphereId() const;
int32_t getBlockId() const;
int32_t getCylinderId() const;
int32_t getMeshId() const;
int32_t getTextId() const;
int32_t getCuboidId() const;
int32_t getLineStripId() const;
int32_t getLineListId() const;
int32_t getSpheresId() const;
int32_t getTriangleId() const;
protected:
// Node Interfaces
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface_;
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr graph_interface_;
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_;
rclcpp::Logger logger_;
// Optional remote control
RemoteControlPtr remote_control_;
// ROS publishers
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
pub_rviz_markers_; // for rviz visualization
// markers
bool pub_rviz_markers_connected_ = false;
bool pub_rviz_markers_waited_ = false;
bool wait_for_subscriber_ = 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
builtin_interfaces::msg::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::msg::MarkerArray markers_;
// Cached Rviz markers
visualization_msgs::msg::Marker arrow_marker_;
visualization_msgs::msg::Marker sphere_marker_;
visualization_msgs::msg::Marker block_marker_;
visualization_msgs::msg::Marker cylinder_marker_;
visualization_msgs::msg::Marker mesh_marker_;
visualization_msgs::msg::Marker text_marker_;
visualization_msgs::msg::Marker cuboid_marker_;
visualization_msgs::msg::Marker line_strip_marker_;
visualization_msgs::msg::Marker line_list_marker_;
visualization_msgs::msg::Marker spheres_marker_;
visualization_msgs::msg::Marker reset_marker_;
visualization_msgs::msg::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