Class RvizVisualTools
Defined in File rviz_visual_tools.hpp
Class Documentation
-
class RvizVisualTools
Public Functions
-
template<typename NodePtr>
inline explicit RvizVisualTools(const std::string &base_frame, const std::string &marker_topic, NodePtr node, const RemoteControlPtr &remote_control = nullptr) Constructor.
- Parameters:
base_frame – - common base for all visualization markers, usually “/world” or “/odom”
marker_topic – - rostopic to publish markers to - your Rviz display should match
node – - a pointer to a ros node
-
inline ~RvizVisualTools()
Deconstructor.
-
bool deleteMarker(const std::string &ns, std::size_t id)
Tell Rviz to clear all markers on a particular display.
- Parameters:
ns – - the namespace of the marker to be deleted
id – - the id of the marker to be deleted
- Returns:
true if we published a marker message
-
bool deleteAllMarkers()
Tell Rviz to clear all markers on a particular display.
- Returns:
true if we published a marker message
-
bool deleteAllMarkers(const std::string &ns)
Tell Rviz to clear all markers on a particular display in a particular namespace.
- Returns:
true if we published a marker message
-
void resetMarkerCounts()
Reset the id’s of all published markers so that they overwrite themselves in the future NOTE you may prefer deleteAllMarkers()
-
bool loadRvizMarkers()
Pre-load rviz markers for better efficiency.
- Returns:
converted pose *
- Returns:
true on sucess
-
inline void setMarkerTopic(const std::string &topic)
Set marker array topic.
-
void loadMarkerPub(bool wait_for_subscriber = true)
Load publishers as needed.
- Parameters:
wait_for_subscriber – - whether a sleep for loop should be used to check for connectivity to an external subscriber before proceeding
-
bool waitForMarkerSub(double wait_time = 5)
Optional blocking function to call after calling loadMarkerPub(). Allows you to do some intermediate processing before wasting cycles waiting for the marker pub to find a subscriber.
- Parameters:
wait_time – - Wait some amount of time for a subscriber before returning. Method will early exit if a subscriber is found before wait_time. Set to 0 to disable waiting and simply return if a subscriber exists.
- Returns:
- true if a subscriber is found
-
inline bool waitForMarkerPub(double wait_time = 5)
Wait until at least one subscriber connects to a publisher.
- Parameters:
pub – - the publisher to check for subscribers
wait_time – - time to wait for subscriber to be available before throwing warning (sec)
- Returns:
true on successful connection
-
inline void setAlpha(double alpha)
Change the transparency of all markers published.
- Parameters:
alpha – - value 0 - 1 where 0 is invisible
-
void setLifetime(double lifetime)
Set the lifetime of markers published to rviz.
- Parameters:
lifetime – seconds of how long to show markers. 0 for inifinity
-
std_msgs::msg::ColorRGBA getColor(Colors color) const
Get the RGB value of standard colors.
- Parameters:
color – - an enum pre-defined name of a color
- Returns:
the RGB message equivalent
-
std_msgs::msg::ColorRGBA createRandColor() const
Create a random color that is not too light.
- Returns:
the RGB message of a random color
-
std_msgs::msg::ColorRGBA getColorScale(double value) const
Convert a value from [0,1] to a color Green->Red.
- Returns:
interpolated color
-
geometry_msgs::msg::Vector3 getScale(Scales scale, double marker_scale = 1.0) const
Get the rviz marker scale of standard sizes.
- Parameters:
scale – - an enum pre-defined name of a size
marker_scale – - amount to scale the scale for accounting for different types of markers
- Returns:
vector of 3 scales
-
Eigen::Isometry3d getVectorBetweenPoints(const Eigen::Vector3d &a, const Eigen::Vector3d &b) const
Create a vector that points from point a to point b.
- Parameters:
point – a - x,y,z in space of a point
point – b - x,y,z in space of a point
- Returns:
vector from a to b
-
Eigen::Vector3d getCenterPoint(const Eigen::Vector3d &a, const Eigen::Vector3d &b) const
Find the center between to points.
- Parameters:
point – a - x,y,z in space of a point
point – b - x,y,z in space of a point
- Returns:
center point
-
inline const std::string getBaseFrame() const
Get the base frame.
- Returns:
name of base frame
-
inline void setBaseFrame(const std::string &base_frame)
Change the global base frame Note: this might reset all your current markers.
- Parameters:
name – of frame
-
inline double getGlobalScale() const
Getter for the global scale used for changing size of all markers.
-
inline void setGlobalScale(double global_scale)
Setter for the global scale used for changing size of all markers.
-
bool publishMarker(visualization_msgs::msg::Marker &marker)
Display a visualization_msgs Marker of a custom type. Allows reuse of the ros publisher.
- Parameters:
marker – - a pre-made marker ready to be published
- Returns:
true on success
-
void enableBatchPublishing(bool enable = true)
Enable batch publishing - useful for when many markers need to be published at once and the ROS topic can get overloaded. This collects all published markers into array and only publishes them with trigger() is called.
-
void enableFrameLocking(bool enable = true)
Enable frame locking - useful for when the markers is attached to a moving TF, the marker will be re-transformed into its frame every time-step.
-
bool triggerEvery(std::size_t queueSize)
Trigger the publish function to send out all collected markers IF there are at least queueSize number of markers ready to be published. Warning: when using this in a loop be sure to call trigger() at end of loop in case there are any remainder markers in the queue.
- Returns:
true on success
-
bool trigger()
Trigger the publish function to send out all collected markers.
- Returns:
true on success
-
bool publishMarkers(visualization_msgs::msg::MarkerArray &markers)
Display an array of markers, allows reuse of the ROS publisher.
- Parameters:
markers –
- Returns:
true on success
-
bool publishCone(const Eigen::Isometry3d &pose, double angle, Colors color = TRANSLUCENT, double scale = 1.0)
Display a cone of a given angle along the x-axis.
- Parameters:
pose – - the location and orientation of the cone
color – - color of the cone
scale – - size of the cone
- Returns:
true on success
-
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)
Display a plane. Vector (A, B, C) gives the normal to the plane. |D|/|(A,B,C)| gives the distance to plane along that unit normal. The plane equation used is Ax+By+Cz+D=0.
- Parameters:
A – - coefficient from Ax+By+Cz+D=0
B – - coefficient from Ax+By+Cz+D=0
C – - coefficient from Ax+By+Cz+D=0
D – - coefficient from Ax+By+Cz+D=0
color – - the color of the plane
x_width – - X-size of the vizualized plane [meters]
y_width – - Y-size of the visualized plane [meters]
- Returns:
true on success
-
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)
Display a plane given a vector normal to the plane and the distance to the plane along that normal. The plane normal does not need to be given as a unit vector.
- Parameters:
normal – - a vector representing the normal of the plane
d – - the distance to the plane along the vector
color – - the color of the plane
x_width – - X-size of the vizualized plane [meters]
y_width – - Y-size of the visualized plane [meters]
- Returns:
true on success
-
bool publishXYPlane(const Eigen::Isometry3d &pose, Colors color = TRANSLUCENT, double scale = 1.0)
Display the XY plane of a given pose.
- Parameters:
pose – - the position of the plane
color – - the color of the plane
scale – - the size of the vizualized plane
- Returns:
true on success
-
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)
Display the XZ plane of a given pose.
- Parameters:
pose – - the position of the plane
color – - the color of the plane
scale – - the size of the vizualized plane
- Returns:
true on success
-
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)
Display the YZ plane of a given pose.
- Parameters:
pose – - the position of the plane
color – - the color of the plane
scale – - the size of the vizualized plane
- Returns:
true on success
-
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)
Display a marker of a sphere.
- Parameters:
pose – - the location to publish the sphere with respect to the base frame
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
id – - unique counter of mesh that allows you to overwrite a previous mesh. if 0, defaults to incremental counter
- Returns:
true on success
-
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")
Display a marker of a series of spheres.
- Parameters:
spheres – - where to publish them
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
- Returns:
true on success
-
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")
Display a marker of a series of spheres, with the possibility of different colors.
- Parameters:
spheres – - where to publish them
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
- Returns:
true on success
-
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)
Display an arrow along the x-axis of a pose.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
length – - the length of the arrow tail, if zero, will auto set with scale
- Returns:
true on success
-
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)
Display an arrow along the y-axis of a pose.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
length – - the length of the arrow tail, if zero, will auto set with scale
- Returns:
true on success
-
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)
Display an arrow along the z-axis of a pose.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
length – - the length of the arrow tail, if zero, will auto set with scale
- Returns:
true on success
-
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)
Display an arrow along the x-axis of a pose.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
length – - how long the arrow tail should be. if zero, will auto set with scale
start – - the starting point of the arrow
end – - the ending point of the arrow
- Returns:
true on success
-
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)
Display a rectangular cuboid.
- Parameters:
point1 – - x,y,z top corner location of box
point2 – - x,y,z bottom opposite corner location of box
color – - an enum pre-defined name of a color or a color message
- Returns:
true on success
-
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)
Display a rectangular cuboid.
- Parameters:
pose – - pose of the box
depth – - depth of the box
width – - width of the box
height – - height of the box
color – - an enum pre-defined name of a color or a color message
- Returns:
true on success
-
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)
Display a marker of line.
- Parameters:
point1 – - x,y,z of start of line
point2 – - x,y,z of end of line
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
- Returns:
true on success
-
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)
Display a marker of lines.
- Parameters:
aPoints – - x,y,z of start of line, as a vector
bPoints – - x,y,z of end of line, as a vector
colors – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
- Returns:
true on success
-
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")
Display a series of connected lines using the LINE_STRIP method - deprecated because visual bugs.
- Parameters:
path – - a series of points to connect with lines
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
- Returns:
true on success
-
bool publishPath(const std::vector<geometry_msgs::msg::Pose> &path, Colors color = RED, Scales scale = MEDIUM, const std::string &ns = "Path")
Display a marker of a series of connected cylinders.
- Parameters:
path – - a series of points to connect with cylinders
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
- Returns:
true on success
-
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")
Display a marker of a series of connected colored cylinders.
Note
path and colors vectors must be the same size
- Parameters:
path – - a series of points to connect with cylinders
colors – - a series of colors
radius – - the radius of the cylinders
ns – - namespace of marker
- Returns:
true on success
-
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")
Display a marker of a polygon.
- Parameters:
polygon – - a series of points to connect with lines
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
- Returns:
true on success
-
bool publishWireframeCuboid(const Eigen::Isometry3d &pose, double depth, double width, double height, Colors color = BLUE, const std::string &ns = "Wireframe Cuboid", std::size_t id = 0)
Publish transformed wireframe cuboid. Useful eg to show an oriented bounding box.
- Parameters:
pose – - cuboid vertices are transformed according to it
depth – - object depth
width – - object width
height – - object height
color – - an enum pre-defined name of a color
ns – - namespace
id – - unique counter of mesh that allows you to overwrite a previous mesh. if 0, defaults to incremental counter
- Returns:
true on success
-
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)
Publish transformed wireframe cuboid. Useful eg to show an oriented bounding box.
- Parameters:
pose – - cuboid vertices are transformed according to it
min_point – - minimum x, y, z coordinates
max_point – - maximum x, y, z coordinates
color – - an enum pre-defined name of a color
ns – - namespace
id – - unique counter that allows you to overwrite a previous marker. if 0, defaults to incremental counter
- Returns:
true on success
-
bool publishWireframeRectangle(const Eigen::Isometry3d &pose, double height, double width, Colors color = BLUE, Scales scale = MEDIUM, std::size_t id = 0)
Publish outline of a rectangle.
- Parameters:
pose – - cuboid vertices are transformed according to it
height –
width –
color – - an enum pre-defined name of a color
id – - unique counter that allows you to overwrite a previous marker. if 0, defaults to incremental counter
- Returns:
true on success
-
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)
Display a marker of a coordinate frame axis with a text label describing it.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
label – - name of axis/coordinate frame
scale – - size of axis
color – - an enum pre-defined name of a color
- Returns:
true on success
-
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")
Display a red/green/blue coordinate frame axis.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
scale – - size of axis
length – - geometry of cylinder
radius – - geometry of cylinder
ns – - namespace
- Returns:
true on success
-
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")
-
bool publishAxisPath(const EigenSTL::vector_Isometry3d &path, Scales scale = MEDIUM, const std::string &ns = "Axis Path")
Display a series of red/green/blue coordinate axis along a path.
- Parameters:
path – - the location to publish each marker with respect to the base frame
length – - geometry of cylinder
radius – - geometry of cylinder
ns – - namespace
- Returns:
true on success
-
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")
Display a marker of a cylinder.
- Parameters:
point1 – - starting side of cylinder
point2 – - end side of cylinder
color – - an enum pre-defined name of a color
radius – - geometry of cylinder
- Returns:
true on success
-
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")
Display a marker of a cylinder.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
color – - an enum pre-defined name of a color
height – - geometry of cylinder
radius – - geometry of cylinder
- Returns:
true on success
-
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)
Display a mesh from file.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
file – name of mesh, starting with “file://” e.g. “file:///home/user/mesh.stl”
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
id – - unique counter of mesh that allows you to overwrite a previous mesh. if 0, defaults to incremental counter
- Returns:
true on success
-
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)
Display a mesh from triangles and vertices.
- Parameters:
pose – - the location to publish the marker with respect to the base frame
mesh – - shape_msgs::msg::Mesh contains the triangles and vertices
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
ns – - namespace of marker
id – - unique counter of mesh that allows you to overwrite a previous mesh. if 0, defaults to incremental counter
- Returns:
true on success
-
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)
-
bool publishText(const Eigen::Isometry3d &pose, const std::string &text, Colors color = WHITE, Scales scale = MEDIUM, bool static_id = true)
Display a graph.
Display a marker of a text
- Parameters:
graph – of nodes and edges
color – - an enum pre-defined name of a color
radius – - width of cylinders
pose – - the location to publish the marker with respect to the base frame
text – - what message to display
color – - an enum pre-defined name of a color
scale – - an enum pre-defined name of a size
static_id – - if true, only one text can be published at a time
- Returns:
true on success
- Returns:
true on success
-
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)
-
inline bool getPsychedelicMode() const
Getter for PsychedelicMode.
-
inline void setPsychedelicMode(bool psychedelic_mode = true)
Setter for PsychedelicMode.
-
bool prompt(const std::string &msg)
Wait for user feedback i.e. through a button or joystick.
-
RemoteControlPtr &getRemoteControl()
Ability to load remote control on the fly.
-
void setRemoteControl(const RemoteControlPtr &remote_control)
Pre-load remote control.
-
void loadRemoteControl()
Ability to load remote control on the fly.
-
int32_t getArrowId() const
Get the latest ID of arrow_marker_.
-
int32_t getSphereId() const
Get the latest ID of sphere_marker_.
-
int32_t getBlockId() const
Get the latest ID of block_marker_.
-
int32_t getCylinderId() const
Get the latest ID of cylinder_marker_.
-
int32_t getMeshId() const
Get the latest ID of mesh_marker_.
-
int32_t getTextId() const
Get the latest ID of text_marker_.
-
int32_t getCuboidId() const
Get the latest ID of cuboid_marker_.
-
int32_t getLineStripId() const
Get the latest ID of line_strip_marker_.
-
int32_t getLineListId() const
Get the latest ID of line_list_marker_.
-
int32_t getSpheresId() const
Get the latest ID of spheres_marker_.
-
int32_t getTriangleId() const
Get the latest ID of triangle_marker_.
Public Static Functions
-
static Colors getRandColor()
Get a random color from the list of hardcoded enum color types.
- Returns:
Random color from colors
-
static Colors intToRvizColor(std::size_t color)
Used by interfaces that do not directly depend on Rviz Visual Tools, such as OMPL.
-
static rviz_visual_tools::Scales intToRvizScale(std::size_t scale)
Used by interfaces that do not directly depend on Rviz Visual Tools, such as OMPL.
-
static double slerp(double start, double end, double range, double value)
Interpolate from [start, end] with value of size steps with current value count.
- Returns:
interpolated value
-
static geometry_msgs::msg::Pose convertPose(const Eigen::Isometry3d &pose)
Convert an Eigen pose to a geometry_msg pose.
- Parameters:
pose –
- Returns:
converted pose
-
static void convertPoseSafe(const Eigen::Isometry3d &pose, geometry_msgs::msg::Pose &pose_msg)
Convert an Eigen pose to a geometry_msg pose - thread safe.
- Parameters:
Eigen – pose - input
ROS – msg pose - output
-
static Eigen::Isometry3d convertPose(const geometry_msgs::msg::Pose &pose)
Convert a geometry_msg pose to an Eigen pose Note: Not thread safe but very convenient.
- Parameters:
ROS – msg pose
- Returns:
converted pose
-
static void convertPoseSafe(const geometry_msgs::msg::Pose &pose_msg, Eigen::Isometry3d &pose)
Convert a geometry_msg pose to an Eigen pose - thread safe.
- Parameters:
ROS – msg pose - input
Eigen – pose - output
-
static Eigen::Isometry3d convertPoint32ToPose(const geometry_msgs::msg::Point32 &point)
Convert a geometry_msg point (32bit) to an Eigen pose Note: Not thread safe but very convenient.
- Parameters:
pose –
- Returns:
converted point with default rotation matrix
-
static geometry_msgs::msg::Pose convertPointToPose(const geometry_msgs::msg::Point &point)
Add an identity rotation matrix to make a point have a full pose.
-
static Eigen::Isometry3d convertPointToPose(const Eigen::Vector3d &point)
-
static geometry_msgs::msg::Point convertPoseToPoint(const Eigen::Isometry3d &pose)
Convert an Eigen pose to a geometry_msg point Note: Not thread safe but very convenient.
- Parameters:
pose –
- Returns:
converted point with orientation discarded
-
static Eigen::Vector3d convertPoint(const geometry_msgs::msg::Point &point)
Convert a geometry_msg point to an Eigen point Note: Not thread safe but very convenient.
- Parameters:
point –
- Returns:
converted pose
-
static Eigen::Vector3d convertPoint32(const geometry_msgs::msg::Point32 &point)
Convert a geometry_msg point to an Eigen point Note: Not thread safe but very convenient.
- Parameters:
point –
- Returns:
converted pose
-
static geometry_msgs::msg::Point32 convertPoint32(const Eigen::Vector3d &point)
Convert an Eigen point to a 32 bit geometry_msg point Note: Not thread safe but very convenient.
- Parameters:
point –
- Returns:
converted pose
-
static geometry_msgs::msg::Point convertPoint(const geometry_msgs::msg::Vector3 &point)
Convert a Vector3 to a geometry_msg Point Note: Not thread safe but very convenient.
- Parameters:
point –
- Returns:
converted point
-
static geometry_msgs::msg::Point convertPoint(const Eigen::Vector3d &point)
Convert a Eigen point to a geometry_msg Point Note: Not thread safe but very convenient.
- Parameters:
point –
- Returns:
converted point
-
static Eigen::Isometry3d convertFromXYZRPY(double tx, double ty, double tz, double rx, double ry, double rz, EulerConvention convention)
Converts scalar translations and rotations to an Eigen Frame. This is achieved by chaining a translation with individual euler rotations in ZYX order (this is equivalent to fixed rotatins XYZ) http://en.wikipedia.org/wiki/Euler_angles#Conversion_between_intrinsic_and_extrinsic_rotations Euler conversion code sourced from Descartes, Copyright (c) 2014, Southwest Research Institute.
- Parameters:
tx, ty, tz – - translations in x, y, z respectively
rx, ry, rz – - rotations about x, y, z, respectively
convention – - default is rviz_visual_tools::XYZ
-
static Eigen::Isometry3d convertFromXYZRPY(const std::vector<double> &transform6, EulerConvention convention)
-
static void convertToXYZRPY(const Eigen::Isometry3d &pose, std::vector<double> &xyzrpy)
Convert an affine3d to xyz rpy components R-P-Y / X-Y-Z / 0-1-2 Euler Angle Standard.
- Parameters:
input – Eigen pose
output – vector of size 6 in order xyz rpy
-
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())
Create a random pose within bounds of random_pose_bounds_.
- Parameters:
Pose – to fill in \parma options bounds on the pose to generate
-
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())
Create a random rectangular cuboid of some shape.
-
static geometry_msgs::msg::Pose getIdentityPose()
Create a pose of position (0,0,0) and quaternion (0,0,0,1)
- Parameters:
Pose – to fill in
-
static bool posesEqual(const Eigen::Isometry3d &pose1, const Eigen::Isometry3d &pose2, double threshold = 0.000001)
Test if two Eigen poses are close enough.
- Parameters:
pose1 –
pose2 –
threshold – - how close in value they must be in order to be considered the same
- Returns:
true if equal
-
static double dRand(double min, double max)
Get random between min and max.
-
static float fRand(float min, float max)
-
static int iRand(int min, int max)
-
static void printTranslation(const Eigen::Vector3d &translation)
Display in the console the x,y,z values of a point.
-
static void printTransform(const Eigen::Isometry3d &transform)
Display in the console a transform in quaternions.
-
static void printTransformRPY(const Eigen::Isometry3d &transform)
Display in the console a transform in roll pitch yaw.
-
static void printTransformFull(const Eigen::Isometry3d &transform)
Display in the console a transform with full 3x3 rotation matrix.
Protected Attributes
-
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_
-
RemoteControlPtr remote_control_
-
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_rviz_markers_
-
bool pub_rviz_markers_connected_ = false
-
bool pub_rviz_markers_waited_ = false
-
bool wait_for_subscriber_ = false
-
std::string marker_topic_
-
std::string base_frame_
-
builtin_interfaces::msg::Duration marker_lifetime_
-
bool batch_publishing_enabled_ = true
-
bool frame_locking_enabled_ = false
-
double alpha_ = 1.0
-
double global_scale_ = 1.0
-
visualization_msgs::msg::MarkerArray 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_
-
bool psychedelic_mode_ = false
-
template<typename NodePtr>