diff --git a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
index b6a2310df6ec38de4a5bb4f413636765212b5c0e..a18af95c22d8b53f97a5ab87121e5e815213a9d4 100644
--- a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
+++ b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
@@ -59,31 +59,31 @@ struct PointCloud
private:
/// The point container type.
using VectorT = std::vector<PointT>;
-
+
public:
-
+
PointCloud() {}
PointCloud(const VectorT& points) : points(points) {}
-
+
// Container methods.
std::size_t size() const { return points.size(); }
-
+
PointT& operator[](std::size_t i) { return points[i]; }
const PointT& operator[](std::size_t i) const { return points[i]; }
-
+
// Iterators.
typename VectorT::iterator begin() { return points.begin(); }
typename VectorT::const_iterator begin() const { return points.begin(); }
typename VectorT::iterator end() { return points.end(); }
typename VectorT::const_iterator end() const { return points.end(); }
-
-
+
+
/// The points.
VectorT points;
};
-/* These test do not actually check any behaviour,
+/* These test do not actually check any behaviour,
* but check whether this code compiles.
*/
@@ -93,12 +93,12 @@ struct Fixture
Fixture()
{
}
-
- const DebugDrawerTopic::VisuID id {"name", "layer"};
+
+ const DebugDrawerTopic::VisuID id {"layer", "name"};
const int pointSize = 10;
-
+
DebugDrawerTopic drawer;
-
+
PointCloud<PointT> pointCloudMutable;
const PointCloud<PointT>& pointCloud = pointCloudMutable;
};
@@ -107,11 +107,11 @@ struct Fixture
BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZ, Fixture<PointXYZ>)
{
pointCloudMutable.points = { {1, 2, 3}, {2, 3, 4}, {3, 4, 5} };
-
+
drawer.drawPointCloud(id, pointCloud);
drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
-
- drawer.drawPointCloud(id, pointCloud,
+
+ drawer.drawPointCloud(id, pointCloud,
[](const PointXYZ&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
}
@@ -120,10 +120,10 @@ BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBA, Fixture<PointXYZRGBA>)
{
drawer.drawPointCloud(id, pointCloud);
drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
-
- drawer.drawPointCloud(id, pointCloud,
+
+ drawer.drawPointCloud(id, pointCloud,
[](const PointXYZRGBA&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
-
+
drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
}
@@ -132,10 +132,10 @@ BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBL, Fixture<PointXYZRGBL>)
{
drawer.drawPointCloud(id, pointCloud);
drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
-
+
drawer.drawPointCloud(id, pointCloud,
[](const PointXYZRGBL&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
-
+
drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
}
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
index ef4c6d5a64a8a0dcc49ba960babc9c0c74848348..ca6e0b43978efa60f7e82dd5e5d17e05b027f2bb 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -16,13 +16,21 @@ namespace armarx
DebugDrawerTopic::VisuID::VisuID() : VisuID("", "")
{}
- DebugDrawerTopic::VisuID::VisuID(const std::string& name, const std::string& layer) :
- name(name), layer(layer)
+ DebugDrawerTopic::VisuID::VisuID(const std::string& name) : VisuID("", name)
{}
+ DebugDrawerTopic::VisuID::VisuID(const std::string& layer, const std::string& name) :
+ layer(layer), name(name)
+ {}
+
+ DebugDrawerTopic::VisuID DebugDrawerTopic::VisuID::withName(const std::string& newName) const
+ {
+ return VisuID(this->layer, newName);
+ }
+
std::ostream& operator<<(std::ostream& os, const DebugDrawerTopic::VisuID& rhs)
{
- os << "Visu '" << rhs.name << "' at layer '" << rhs.layer << "'";
+ os << "Visu '" << rhs.name << "' on layer '" << rhs.layer << "'";
return os;
}
@@ -50,6 +58,16 @@ namespace armarx
return topic;
}
+ void DebugDrawerTopic::setEnabled(bool enabled)
+ {
+ this->_enabled = enabled;
+ }
+
+ bool DebugDrawerTopic::enabled() const
+ {
+ return _enabled && topic;
+ }
+
void DebugDrawerTopic::offeringTopic(ManagedIceObject& component, const std::string& topicNameOverride) const
{
component.offeringTopic(topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride);
@@ -194,9 +212,135 @@ namespace armarx
boundingBox.getMax() - boundingBox.getMin(), color, ignoreLengthScale);
}
- void DebugDrawerTopic::drawCylinder(const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float radius, float length,
- const DrawColor& color, bool ignoreLengthScale)
+ void DebugDrawerTopic::removeBox(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removeBoxVisu(layer(id), id.name);
+ }
+ }
+
+ void DebugDrawerTopic::drawBoxEdges(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ drawBoxEdges(id, math::Helpers::Pose(position, orientation), extents, width, color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawBoxEdges(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (!enabled())
+ {
+ return;
+ }
+
+ std::vector<Eigen::Vector3f> points;
+
+ Eigen::Matrix<float, 3, 2> bb;
+ bb.col(0) = -extents / 2;
+ bb.col(1) = extents / 2;
+
+ auto addLine = [&](int x1, int y1, int z1, int x2, int y2, int z2)
+ {
+ Eigen::Vector3f start = { bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z() };
+ Eigen::Vector3f end = { bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z() };
+
+ start = math::Helpers::TransformPosition(pose, start);
+ end = math::Helpers::TransformPosition(pose, end);
+
+ points.push_back(start);
+ points.push_back(end);
+ };
+
+ /* 001 +-----+ 011
+ * /| 111/|
+ * 101 +-----+ |
+ * | +---|-+ 010
+ * |/000 |/
+ * 100 +-----+ 110
+ */
+
+ // 000 -> 100, 010, 001
+ addLine(0, 0, 0, 1, 0, 0);
+ addLine(0, 0, 0, 0, 1, 0);
+ addLine(0, 0, 0, 0, 0, 1);
+
+ // 111 -> 011, 101, 110
+ addLine(1, 1, 1, 0, 1, 1);
+ addLine(1, 1, 1, 1, 0, 1);
+ addLine(1, 1, 1, 1, 1, 0);
+
+ // 100 -> 101, 110
+ addLine(1, 0, 0, 1, 0, 1);
+ addLine(1, 0, 0, 1, 1, 0);
+
+ // 010 -> 110, 011
+ addLine(0, 1, 0, 1, 1, 0);
+ addLine(0, 1, 0, 0, 1, 1);
+
+ // 001 -> 101, 011
+ addLine(0, 0, 1, 1, 0, 1);
+ addLine(0, 0, 1, 0, 1, 1);
+
+ drawLineSet(id, points, width, color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawBoxEdges(
+ const DebugDrawerTopic::VisuID& id, const VirtualRobot::BoundingBox& boundingBox,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ drawBoxEdges(id, boundingBox, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawBoxEdges(
+ const DebugDrawerTopic::VisuID& id, const Eigen::Matrix32f& aabb,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ drawBoxEdges(id, aabb, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawBoxEdges(
+ const DebugDrawerTopic::VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
+
+ drawBoxEdges(id, math::Helpers::Pose(math::Helpers::TransformPosition(pose, center),
+ math::Helpers::Orientation(pose)),
+ boundingBox.getMax() - boundingBox.getMin(),
+ width, color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawBoxEdges(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Matrix32f& aabb, const Eigen::Matrix4f& pose,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ const Eigen::Vector3f center = 0.5 * (aabb.col(0) + aabb.col(1));
+
+ drawBoxEdges(id, math::Helpers::Pose(math::Helpers::TransformPosition(pose, center),
+ math::Helpers::Orientation(pose)),
+ aabb.col(1) - aabb.col(0),
+ width, color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::removeboxEdges(const DebugDrawerTopic::VisuID& id)
+ {
+ removeLineSet(id);
+ }
+
+
+ void DebugDrawerTopic::drawCylinder(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& center, const Eigen::Vector3f& direction,
+ float length, float radius,
+ const DrawColor& color, bool ignoreLengthScale)
{
if (enabled())
{
@@ -208,7 +352,8 @@ namespace armarx
void DebugDrawerTopic::drawCylinder(
const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float radius, float length,
+ const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation,
+ float length, float radius,
const DrawColor& color, bool ignoreLengthScale)
{
drawCylinder(id, center, orientation * Eigen::Vector3f::UnitY(), radius, length, color,
@@ -228,6 +373,16 @@ namespace armarx
}
}
+
+ void DebugDrawerTopic::removeCylinder(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removeCylinderVisu(layer(id), id.name);
+ }
+ }
+
+
void DebugDrawerTopic::drawSphere(
const DebugDrawerTopic::VisuID& id,
const Eigen::Vector3f& center, float radius,
@@ -242,6 +397,15 @@ namespace armarx
}
+ void DebugDrawerTopic::removeSphere(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removeSphereVisu(layer(id), id.name);
+ }
+ }
+
+
void DebugDrawerTopic::drawArrow(
const VisuID& id,
const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
@@ -255,6 +419,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::drawArrowFromTo(
const VisuID& id,
const Eigen::Vector3f& from, const Eigen::Vector3f& to,
@@ -268,6 +433,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::removeArrow(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
@@ -299,6 +465,16 @@ namespace armarx
}
}
+
+ void DebugDrawerTopic::removePolygon(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removePolygonVisu(layer(id), id.name);
+ }
+ }
+
+
void DebugDrawerTopic::drawLine(
const VisuID& id, const Eigen::Vector3f& from, const Eigen::Vector3f& to,
float width, const DrawColor& color, bool ignoreLengthScale)
@@ -312,6 +488,16 @@ namespace armarx
}
}
+
+ void DebugDrawerTopic::removeLine(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removeLineVisu(layer(id), id.name);
+ }
+ }
+
+
void DebugDrawerTopic::drawLineSet(
const VisuID& id, const DebugDrawerLineSet& lineSet, bool ignoreLengthScale)
{
@@ -337,6 +523,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::drawLineSet(
const VisuID& id, const std::vector<Eigen::Vector3f>& points,
float width, const DrawColor& color, bool ignoreLengthScale)
@@ -385,12 +572,22 @@ namespace armarx
}
+ void DebugDrawerTopic::removeLineSet(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removeLineSetVisu(layer(id), id.name);
+ }
+ }
+
+
void DebugDrawerTopic::drawPose(
const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreLengthScale)
{
drawPose(id, pose, _poseScale, ignoreLengthScale);
}
+
void DebugDrawerTopic::drawPose(
const VisuID& id,
const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
@@ -399,6 +596,7 @@ namespace armarx
drawPose(id, math::Helpers::Pose(pos, ori), _poseScale, ignoreLengthScale);
}
+
void DebugDrawerTopic::drawPose(
const VisuID& id, const Eigen::Matrix4f& pose, float scale,
bool ignoreLengthScale)
@@ -419,6 +617,7 @@ namespace armarx
}
+
void DebugDrawerTopic::drawPose(
const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
float scale, bool ignoreLengthScale)
@@ -426,6 +625,7 @@ namespace armarx
drawPose(id, math::Helpers::Pose(pos, ori), scale, ignoreLengthScale);
}
+
void DebugDrawerTopic::removePose(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
@@ -445,6 +645,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::updateRobotPose(
const DebugDrawerTopic::VisuID& id,
const Eigen::Matrix4f& pose, bool ignoreScale)
@@ -455,6 +656,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::updateRobotPose(
const DebugDrawerTopic::VisuID& id,
const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, bool ignoreScale)
@@ -462,6 +664,7 @@ namespace armarx
updateRobotPose(id, math::Helpers::Pose(pos, ori), ignoreScale);
}
+
void DebugDrawerTopic::updateRobotConfig(
const DebugDrawerTopic::VisuID& id, const std::map<std::string, float>& config)
{
@@ -471,6 +674,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::updateRobotColor(
const DebugDrawerTopic::VisuID& id, const DrawColor& color)
{
@@ -480,6 +684,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::updateRobotNodeColor(
const DebugDrawerTopic::VisuID& id,
const std::string& nodeName, const DrawColor& color)
@@ -490,6 +695,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::removeRobot(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
@@ -562,6 +768,7 @@ namespace armarx
topic->setTriMeshVisu(layer(id), id.name, dd);
}
+
void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
const VirtualRobot::TriMeshModel& trimesh,
const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
@@ -571,6 +778,7 @@ namespace armarx
colorFace, lineWidth, colorEdge, ignoreLengthScale);
}
+
void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
const VirtualRobot::TriMeshModel& trimesh, const Eigen::Matrix4f& pose,
const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
@@ -608,6 +816,7 @@ namespace armarx
}
}
+
void DebugDrawerTopic::drawTriMeshAsPolygons(
const VisuID& id,
const VirtualRobot::TriMeshModel& trimesh,
@@ -757,15 +966,10 @@ namespace armarx
}
}
- bool DebugDrawerTopic::enabled() const
- {
- return topic;
- }
-
DebugDrawerTopic::operator bool() const
{
- return topic;
+ return enabled();
}
armarx::DebugDrawerTopic::operator DebugDrawerInterfacePrx& ()
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
index b95de6d43258d5a34246a639f6735ea4ee1ee1b4..c707e5cb242660136f2b0e4f0fd42ad68f5d617d 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
@@ -8,6 +8,18 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+namespace Eigen
+{
+ /**
+ * @brief A 3x2 matrix.
+ *
+ * Useful to represent axis-aligned bounding boxes (AABBs). When used as a
+ * AABB, column 0 contains the minimal x, y, z values and column 1 the
+ * maximal x, y, z values.
+ * Accordingly, the rows each contain the limits in x, y, z direction.
+ */
+ using Matrix32f = Matrix<float, 3, 2>;
+}
namespace VirtualRobot
{
@@ -15,7 +27,6 @@ namespace VirtualRobot
class BoundingBox;
}
-
namespace armarx
{
// forward declaration
@@ -30,16 +41,17 @@ namespace armarx
* types, and take care of conversion to Ice variants or data structures.
* In addition, this class provides useful overloads for different use cases.
*
- * All methods check whether the internal topic proxy is set, and do
- * nothing if it is not set. To disable visualization by this classs
- * completely, just do not set the topic. To check whether visualization
- * is enabled (i.e. a topic proxy is set), use `enabled()` or just convert
- * `*this` to bool:
+ * Drawing is enabled if the internal topic proxy is set and an optional
+ * enabled flag is set (true by default). All methods check whether drawing
+ * is enabled and do nothing if drawing is disabled. To disable
+ * visualization by this class completely, use `setEnabled(true/false)` or
+ * just do not set the topic. To check whether visualization is enabled,
+ * use `enabled()` or just convert `*this` to bool:
* @code
* DebugDrawerTopic debugDrawer;
- * if (debugDrawer) // equivalent: if (debugDrawer.enabled())
+ * if (debugDrawer) // Equivalent: if (debugDrawer.enabled())
* {
- * // do stuff if visualization is enabled
+ * // Do stuff if visualization is enabled.
* }
* @endcode
*
@@ -122,7 +134,7 @@ namespace armarx
* Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
* std::string layer = "layer";
* std::string name = "pose";
- * debugDrawer.drawPose({name, layer}, pose);
+ * debugDrawer.drawPose({layer, name}, pose);
* @endcode
*
*
@@ -141,45 +153,57 @@ namespace armarx
{
public:
+
+ /**
+ * @brief A visualisation ID.
+ *
+ * This constructor can be called in the following ways
+ * (with `draw(const VisuID& id, ...)` being any drawing method):
+ *
+ * @code
+ * std::string name = "pose";
+ * std::string layer = "layer";
+ * draw(name, ...); // just the name, implicit call
+ * draw({name}, ...); // just the name, call with initializer list
+ * draw({layer, name}, ...); // layer and name, with initializer list
+ * @endcode
+ *
+ * (And of course by an explicit call if you want to be really verbose.)
+ * Not passing a layer will cause DebugDrawerTopic to use the
+ * preset layer.
+ */
struct VisuID
{
+ public:
+
/// Empty constructor.
VisuID();
- /**
- * @brief Construct a VisuID.
- *
- * This constructor can be called in the following ways
- * (with `draw(const VisuID& id, ...)` being any drawing method):
- *
- * @code
- * std::string name = "pose";
- * std::string layer = "layer";
- * draw(name, ...); // just the name, implicit call
- * draw({name}, ...); // just the name, call with initializer list
- * draw({name, layer}, ...); // name and layer, with initializer list
- * @endcode
- *
- * (And of course by an explicit call if you want to be really verbose.)
- * Not passing a layer will cause DebugDrawerTopic to use the
- * preset layer.
- *
- * @param name the name
- * @param layer the layer name
- */
- VisuID(const std::string& name, const std::string& layer = "");
+ /// Construct a VisuID with given name (for drawing to the preset layer).
+ VisuID(const std::string& name);
+ /// Construct a VisuID with given name and layer.
+ VisuID(const std::string& layer, const std::string& name);
/// Construct a VisuID from a non-std::string source (e.g. char[]).
template <typename Source>
VisuID(const Source& name) : VisuID(std::string(name))
{}
- std::string name = ""; ///< The visu name (empty by default).
- std::string layer = ""; ///< The layer name (empty by default).
+ /// Get a `VisuID` with the given name and same layer as `*this.
+ VisuID withName(const std::string& name) const;
+
+ /// Streams a short human-readable description of `rhs` to `os`.
friend std::ostream& operator<<(std::ostream& os, const VisuID& rhs);
+
+ public:
+
+ std::string layer = ""; ///< The layer name (empty by default).
+ std::string name = ""; ///< The visu name (empty by default).
};
+
+ /// Default values for drawing functions.
struct Defaults
{
DrawColor colorText { 0, 0, 0, 1 };
@@ -199,6 +223,11 @@ namespace armarx
// Default value of DebugDrawerColoredPointCloud etc.
float pointCloudPointSize = 3.0f;
+
+ float lineWidth = 2;
+
+ DrawColor boxEdgesColor { 0, 1, 1, 1 };
+ float boxEdgesWidth = 2;
};
static const Defaults DEFAULTS;
@@ -218,6 +247,14 @@ namespace armarx
/// Get the topic.
DebugDrawerInterfacePrx getTopic() const;
+ /**
+ * @brief Set whether drawing is enabled.
+ * Visualization is only truly enabled if the topic is set.
+ */
+ void setEnabled(bool enabled);
+ /// Indicate whether visualization is enabled, i.e. a topic is set and enabled flag is set.
+ bool enabled() const;
+
/**
* @brief Call offeringTopic([topicName]) on the given component.
* @param component The component (`*this` when called from a component).
@@ -321,24 +358,69 @@ namespace armarx
bool ignoreLengthScale = false);
+ /// Remove a box.
+ void removeBox(const VisuID& id);
+
+
+ /// Draw box edges (as a line set).
+ void drawBoxEdges(const VisuID& id,
+ const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents,
+ float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ bool ignoreLengthScale = false);
+
+ /// Draw box edges (as a line set).
+ void drawBoxEdges(const VisuID& id,
+ const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+ float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ bool ignoreLengthScale = false);
+
+ /// Draw edges of an axis aligned bounding box (as a line set).
+ void drawBoxEdges(const VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox,
+ float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ bool ignoreLengthScale = false);
+
+ /// Draw edges of an axis aligned bounding box (as a line set).
+ void drawBoxEdges(const VisuID& id,
+ const Eigen::Matrix32f& aabb,
+ float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ bool ignoreLengthScale = false);
+
+ /// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
+ void drawBoxEdges(const VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
+ float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ bool ignoreLengthScale = false);
+
+ /// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
+ void drawBoxEdges(const VisuID& id,
+ const Eigen::Matrix32f& aabb, const Eigen::Matrix4f& pose,
+ float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ bool ignoreLengthScale = false);
+
+ /// Remove box edges (as a line set).
+ void removeboxEdges(const VisuID& id);
+
+
/**
* @brief Draw a cylinder with center and direction.
- * @param length the full length (not half-length)
+ * @param length The full length (not half-length).
*/
void drawCylinder(
const VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float radius, float length,
+ const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float length, float radius,
const DrawColor& color = DEFAULTS.colorCylinder,
bool ignoreLengthScale = false);
/**
* @brief Draw a cylinder with center and orientation.
* An identity orientation represents a cylinder with an axis aligned to the Y-axis.
- * @param length the full length (not half-length)
+ * @param length The full length (not half-length).
*/
void drawCylinder(
const VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float radius, float length,
+ const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float length, float radius,
const DrawColor& color = DEFAULTS.colorCylinder,
bool ignoreLengthScale = false);
@@ -349,6 +431,9 @@ namespace armarx
const DrawColor& color = DEFAULTS.colorCylinder,
bool ignoreLengthScale = false);
+ /// Remove a cylinder.
+ void removeCylinder(const VisuID& id);
+
/// Draw a sphere.
void drawSphere(
@@ -357,6 +442,9 @@ namespace armarx
const DrawColor& color = DEFAULTS.colorSphere,
bool ignoreLengthScale = false);
+ /// Remove a sphere.
+ void removeSphere(const VisuID& id);
+
/// Draw an arrow with position (start) and direction.
void drawArrow(
@@ -384,6 +472,9 @@ namespace armarx
float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
bool ignoreLengthScale = false);
+ /// Remove a polygon.
+ void removePolygon(const VisuID& id);
+
/// Draw a line from start to end.
void drawLine(
@@ -392,6 +483,9 @@ namespace armarx
float width, const DrawColor& color = DEFAULTS.colorLine,
bool ignoreLengthScale = false);
+ /// Remove a line.
+ void removeLine(const VisuID& id);
+
/// Draw a line set.
void drawLineSet(
@@ -421,6 +515,9 @@ namespace armarx
const std::vector<float>& intensitiesB,
bool ignoreLengthScale = false);
+ /// Remove a line set.
+ void removeLineSet(const VisuID& id);
+
// POSE
@@ -439,7 +536,7 @@ namespace armarx
float scale,
bool ignoreLengthScale = false);
- /// Remove a pose visualization.
+ /// Remove a pose.
void removePose(const VisuID& id);
@@ -614,16 +711,12 @@ namespace armarx
bool ignoreLengthScale = false);
- // STATUS
+ // OPERATORS
- /// Indicate whether a topic is set, i.e. visualization is enabled.
- bool enabled() const;
- /// Indicate whether a topic is set, i.e. visualization is enabled.
+ /// Indicate whether visualization is enabled.
+ /// @see `enabled()`
operator bool() const;
-
- // OPERATORS
-
/// Conversion operator to DebugDrawerInterfacePrx.
operator DebugDrawerInterfacePrx& ();
operator const DebugDrawerInterfacePrx& () const;
@@ -711,6 +804,9 @@ namespace armarx
/// The debug drawer topic.
DebugDrawerInterfacePrx topic = nullptr;
+ /// Whether drawing is enabled. (In comination with `topic`.
+ bool _enabled = true;
+
/// The default layer (used if none is passed to the method).
std::string _layer = DEFAULT_LAYER;