diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
index 21847141df8240216e13c3b24f7230bafe686545..78c9dc67a479bd5a6775464837a54e40565ef6e6 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -288,6 +288,91 @@ namespace armarx
}
}
+ void DebugDrawerTopic::drawLine(
+ const VisuID& id, const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ topic->setLineVisu(layer(id), id.name, scaled(scale, from), scaled(scale, to),
+ width, color);
+ }
+ }
+
+ void DebugDrawerTopic::drawLineSet(
+ const VisuID& id, const DebugDrawerLineSet& lineSet, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ if (1.f <= scale && scale <= 1.f)
+ {
+ // Can use lineSet directly.
+ topic->setLineSetVisu(layer(id), id.name, lineSet);
+ }
+ else
+ {
+ // Need to scale line set, hence, reconstruct it.
+ DebugDrawerLineSet scaledLineSet = lineSet;
+ for (auto& point : scaledLineSet.points)
+ {
+ scaleXYZ(scale, point);
+ }
+ topic->setLineSetVisu(layer(id), id.name, scaledLineSet);
+ }
+ }
+ }
+
+ void DebugDrawerTopic::drawLineSet(
+ const VisuID& id, const std::vector<Eigen::Vector3f>& points,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ DebugDrawerLineSet lineSet;
+ for (const auto& point : points)
+ {
+ lineSet.points.push_back(scaledT<DebugDrawerPointCloudElement>(scale, point));
+ }
+
+ lineSet.lineWidth = width;
+ lineSet.colorNoIntensity = lineSet.colorFullIntensity = color;
+ lineSet.intensities.assign(points.size() / 2, 0.);
+
+ topic->setLineSetVisu(layer(id), id.name, lineSet);
+ }
+ }
+
+
+ void DebugDrawerTopic::drawLineSet(
+ const VisuID& id, const std::vector<Eigen::Vector3f>& points, float width,
+ const DrawColor& colorA, const DrawColor& colorB, const std::vector<float>& intensitiesB,
+ bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ DebugDrawerLineSet lineSet;
+ for (const auto& point : points)
+ {
+ lineSet.points.push_back(scaledT<DebugDrawerPointCloudElement>(scale, point));
+ }
+
+ lineSet.lineWidth = width;
+ lineSet.colorNoIntensity = colorA;
+ lineSet.colorFullIntensity = colorB;
+ lineSet.intensities = intensitiesB;
+
+ topic->setLineSetVisu(layer(id), id.name, lineSet);
+ }
+ }
+
void DebugDrawerTopic::drawPose(
const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreLengthScale)
@@ -538,37 +623,37 @@ namespace armarx
faceColorsInner.at(i), colorEdge, lineWidth);
}
}
-
+
void DebugDrawerTopic::drawPointCloud(
- const DebugDrawerTopic::VisuID& id,
- const DebugDrawerPointCloud& pointCloud)
+ const DebugDrawerTopic::VisuID& id,
+ const DebugDrawerPointCloud& pointCloud)
{
if (enabled())
{
topic->setPointCloudVisu(id.layer, id.name, pointCloud);
}
}
-
+
void DebugDrawerTopic::drawPointCloud(
- const DebugDrawerTopic::VisuID& id,
- const DebugDrawerColoredPointCloud& pointCloud)
+ const DebugDrawerTopic::VisuID& id,
+ const DebugDrawerColoredPointCloud& pointCloud)
{
if (enabled())
{
topic->setColoredPointCloudVisu(id.layer, id.name, pointCloud);
}
}
-
+
void DebugDrawerTopic::drawPointCloud(
- const DebugDrawerTopic::VisuID& id,
- const DebugDrawer24BitColoredPointCloud& pointCloud)
+ const DebugDrawerTopic::VisuID& id,
+ const DebugDrawer24BitColoredPointCloud& pointCloud)
{
if (enabled())
{
topic->set24BitColoredPointCloudVisu(id.layer, id.name, pointCloud);
}
}
-
+
void DebugDrawerTopic::clearColoredPointCloud(const DebugDrawerTopic::VisuID& id)
{
// Draw an empty point cloud.
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
index 323450196e6387e0888f35e96c2baf5ba1084ac9..2b992379c979ea574a34eed938256502b03164a8 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
@@ -187,6 +187,7 @@ namespace armarx
DrawColor colorArrow { 1, .5, 0, 1 };
DrawColor colorBox { 1, 0, 0, 1 };
DrawColor colorCylinder { 0, 1, 0, 1 };
+ DrawColor colorLine { .5, 0, 0, 1 };
DrawColor colorSphere { 0, 0, 1, 1 };
DrawColor colorPolygonFace { 0, 1, 1, 1 };
@@ -371,6 +372,43 @@ namespace armarx
bool ignoreLengthScale = false);
+ /// Draw a line from start to end.
+ void drawLine(
+ const VisuID& id,
+ const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+ float width, const DrawColor& color = DEFAULTS.colorLine,
+ bool ignoreLengthScale = false);
+
+
+ /// Draw a line set.
+ void drawLineSet(
+ const VisuID& id,
+ const DebugDrawerLineSet& lineSet,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a set of lines with constant color.
+ * @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
+ */
+ void drawLineSet(
+ const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width, const DrawColor& color = DEFAULTS.colorLine,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a set of lines with constant color.
+ * @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
+ * @param colors List of line colors [ c1, c2, ..., cn ].
+ */
+ void drawLineSet(
+ const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width, const DrawColor& colorA, const DrawColor& colorB,
+ const std::vector<float>& intensitiesB,
+ bool ignoreLengthScale = false);
+
+
// POSE
/// Draw a pose (with the preset scale).
@@ -620,6 +658,17 @@ namespace armarx
/// Scale the translation of the given pose and return it as Pose pointer.
static PoseBasePtr scaled(float scale, const Eigen::Matrix4f& pose);
+ /// Scale the given vector and return it as `ScaledT`.
+ /// The constructor of `ScaledT` must take the x, y and z coordinates.
+ template <class ScaledT>
+ static ScaledT scaledT(float scale, const Eigen::Vector3f& vector);
+
+ /// Scale a value with .x, .y and .z attributes in-place.
+ template <class XYZT>
+ static void scaleXYZ(float scale, XYZT& xyz);
+
+
+ private:
/// The name of the debug drawer topic.
static const std::string TOPIC_NAME;
@@ -730,4 +779,20 @@ namespace armarx
drawPointCloud(id, dd);
}
+
+ template <class ScaledT>
+ ScaledT DebugDrawerTopic::scaledT(float scale, const Eigen::Vector3f& vector)
+ {
+ auto scaled = vector * scale;
+ return { scaled.x(), scaled.y(), scaled.z() };
+ }
+
+ template <class XYZT>
+ void DebugDrawerTopic::scaleXYZ(float scale, XYZT& xyz)
+ {
+ xyz.x *= scale;
+ xyz.y *= scale;
+ xyz.z *= scale;
+ }
+
}