diff --git a/source/RobotAPI/libraries/core/test/CMakeLists.txt b/source/RobotAPI/libraries/core/test/CMakeLists.txt
index 37fd064ee6553f8324c2fd2916c89dbb801f29e7..3f70ca0b8d1b201a90bc46e0d5bdf11d3241baf4 100644
--- a/source/RobotAPI/libraries/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/test/CMakeLists.txt
@@ -9,3 +9,5 @@ armarx_add_test(CartesianVelocityControllerTest CartesianVelocityControllerTest.
armarx_add_test(CartesianVelocityRampTest CartesianVelocityRampTest.cpp "${LIBS}")
armarx_add_test(CartesianVelocityControllerWithRampTest CartesianVelocityControllerWithRampTest.cpp "${LIBS}")
+
+armarx_add_test(DebugDrawerTopicTest DebugDrawerTopicTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6a2310df6ec38de4a5bb4f413636765212b5c0e
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
@@ -0,0 +1,143 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::DebugDrawerTopicTest::Test
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+
+#include <ArmarXCore/core/test/IceTestHelper.h>
+
+#include <RobotAPI/libraries/core/visualization/DebugDrawerTopic.h>
+
+
+using namespace armarx;
+
+
+// PCL-like dummy types.
+
+struct PointXYZ
+{
+ float x, y, z;
+};
+
+struct PointXYZRGBA : public PointXYZ
+{
+ uint8_t r, g, b, a;
+};
+
+struct PointXYZRGBL : public PointXYZRGBA
+{
+ uint32_t label;
+};
+
+template <class PointT>
+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,
+ * but check whether this code compiles.
+ */
+
+template <class PointT>
+struct Fixture
+{
+ Fixture()
+ {
+ }
+
+ const DebugDrawerTopic::VisuID id {"name", "layer"};
+ const int pointSize = 10;
+
+ DebugDrawerTopic drawer;
+
+ PointCloud<PointT> pointCloudMutable;
+ const PointCloud<PointT>& pointCloud = pointCloudMutable;
+};
+
+
+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,
+ [](const PointXYZ&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+}
+
+
+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,
+ [](const PointXYZRGBA&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+
+ drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
+}
+
+
+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 70d18d468b472fffca2b7a0d0a6538c4afe2b0ab..ff170b8d9087bf73d5f86968a80e9bd2c815f1c1 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -533,6 +533,42 @@ namespace armarx
faceColorsInner.at(i), colorEdge, lineWidth);
}
}
+
+ void DebugDrawerTopic::drawPointCloud(
+ 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)
+ {
+ if (enabled())
+ {
+ topic->setColoredPointCloudVisu(id.layer, id.name, pointCloud);
+ }
+ }
+
+ void DebugDrawerTopic::drawPointCloud(
+ 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.
+ drawPointCloud(id, DebugDrawerColoredPointCloud{});
+ }
void DebugDrawerTopic::drawFloor(
const VisuID& id,
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
index d7a1a9045986d476b1e895cb6e76cf9f51c62b82..e775735e2031e48acd1b4d704e3ce4ae4b676fae 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
@@ -1,6 +1,7 @@
#pragma once
#include <chrono>
+#include <functional>
#include <thread>
#include <Eigen/Geometry>
@@ -35,6 +36,7 @@ namespace armarx
* is enabled (i.e. a topic proxy is set), use `enabled()` or just convert
* `*this` to bool:
* @code
+ * DebugDrawerTopic debugDrawer;
* if (debugDrawer) // equivalent: if (debugDrawer.enabled())
* {
* // do stuff if visualization is enabled
@@ -42,11 +44,36 @@ namespace armarx
* @endcode
*
* The `DebugDrawerTopic` allows to set a layer on constructor or via
- * `setLayer()`. This layer will be used if none is passed to a drawing method.
+ * `setLayer()`. This layer will be used if none is passed to a drawing
+ * method. If no layer is passed or set, `DebugDrawerTopic::DEFAULT_LAYER`
+ * is used.
+ *
+ *
+ * @par Initialisation by Offering and Getting Topic
+ *
+ * A `DebugDrawerTopic` needs an underlying `DebugDrawerInterfacePrx` topic proxy.
+ * This proxy can be passed on construction or set via `setTopic()`.
+ * In a component (or any other `ManagedIceObject`), `DebugDrawerTopic`
+ * provides convenience functions to register and fetch the topics.
+ *
+ * In `onInitComponent()` (or equivalent method), call:
+ * @code
+ * debugDrawer.offeringTopic(*this);
+ * @endcode
+ * In `onConnectComponent()` (or equivalent), call:
+ * @code
+ * debugDrawer.getTopic(*this);
+ * @endcode
+ * where `*this` is a `ManagedIceObject`.
+ *
+ * This will call `this->offeringTopic("...")` and `this->getTopic("...")`
+ * with the correct topic name (`DebugDrawerTopic::TOPIC_NAME`) and
+ * enable the `DebugDrawerTopic`.
+ *
*
* @par Scaling
*
- * `DebugDrawerTopic` support length scaling and pose scaling.
+ * `DebugDrawerTopic` supports length scaling and pose scaling.
*
* If a length scale is set, all visualizations will be scaled up or down
* by this value. This scaling affects positions, sizes / extents, and
@@ -87,7 +114,7 @@ namespace armarx
* @endcode
*
* This will draw a pose on the preset layer (i.e. the layer passed to the
- * constructor or set via `setLayer()`).
+ * constructor or set via `setLayer()`, or "debug" by default).
* To specify both name and layer of a single visualization, pass both in
* an initializer list:
*
@@ -100,7 +127,7 @@ namespace armarx
*
*
* After the VisuID, usually the essential geometric parameters follow,
- * (e.g. position, size, lenght, point list, ...), depending on the type
+ * (e.g. position, size, length, point list, ...), depending on the type
* of visualization.
* Finally, decorative parameters like colors and width can be passed.
* Most of the time, they have sensible default values and can be omitted
@@ -108,7 +135,7 @@ namespace armarx
*
* (Added methods should adhere to this pattern.)
*
- * @see DebugDrawerTopic::VisuID
+ * @see `DebugDrawerTopic::VisuID`
*/
class DebugDrawerTopic
{
@@ -118,7 +145,7 @@ namespace armarx
{
/// Empty constructor.
VisuID();
-
+
/**
* @brief Construct a VisuID.
*
@@ -166,12 +193,19 @@ namespace armarx
DrawColor colorPolygonEdge { .75, .75, .75, 1 };
DrawColor colorFloor { .1f, .1f, .1f, 1 };
+
+ DrawColor colorPointCloud { .5, .5, .5, 1. };
+
+ // Default value of DebugDrawerColoredPointCloud etc.
+ float pointCloudPointSize = 3.0f;
};
static const Defaults DEFAULTS;
public:
+ // CONSTRUCTION & SETUP
+
/// Construct without topic, and optional layer.
DebugDrawerTopic(const std::string& layer = DEFAULT_LAYER);
/// Construct with given topic and optional layer.
@@ -188,9 +222,9 @@ namespace armarx
/// Get the topic by calling getTopic([topicName]) on the given component.
void getTopic(ManagedIceObject& component);
- /// Get the default layer (used if no layer is passed to the method).
+ /// Get the default layer (used if no layer is passed to a method).
const std::string& getLayer() const;
- /// Set the default layer (used if no layer is passed to the method).
+ /// Set the default layer (used if no layer is passed to a method).
void setLayer(const std::string& layer);
/// Get the scaling for positions, lengths and distances.
@@ -250,6 +284,7 @@ namespace armarx
int size = 10, const DrawColor color = DEFAULTS.colorText,
bool ignoreLengthScale = false);
+
/// Draw a box.
void drawBox(const VisuID& id, const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
const Eigen::Vector3f& extents, const DrawColor& color = DEFAULTS.colorBox,
@@ -271,6 +306,7 @@ namespace armarx
const DrawColor& color = DEFAULTS.colorBox,
bool ignoreLengthScale = false);
+
/**
* @brief Draw a cylinder with center and direction.
* @param length the full length (not half-length)
@@ -430,6 +466,71 @@ namespace armarx
bool ignoreLengthScale = false);
+ // POINT CLOUD
+ /* (By templating these functions, we can make them usable for PCL
+ * point clouds without a dependency on PCL.)
+ */
+
+ /**
+ * @brief Draw a unicolored point cloud.
+ *
+ * `pointCloud` must be iterable and its elements must provide members `x, y, z`.
+ */
+ template <class PointCloudT>
+ void drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const DrawColor& color = DEFAULTS.colorPointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a colored point cloud with RGBA information.
+ *
+ * `pointCloud` must be iterable and its elements must provide
+ * members `x, y, z, r, g, b, a`.
+ */
+ template <class PointCloudT>
+ void drawPointCloudRGBA(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a colored point cloud with custom colors.
+ *
+ * `pointCloud` must be iterable and its elements must provide
+ * members `x, y, z`.
+ * The color of a point is specified by `colorFunc`, which must be
+ * a callable taking an element of `pointCloud` and returning its
+ * color as `armarx::DrawColor`.
+ */
+ template <class PointCloudT, class ColorFuncT>
+ void drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const ColorFuncT& colorFunc,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
+
+
+ // Debug Drawer Point Cloud Types
+
+ /// Draw a non-colored point cloud.
+ void drawPointCloud(const VisuID& id, const DebugDrawerPointCloud& pointCloud);
+
+ /// Draw a colored point cloud.
+ void drawPointCloud(const VisuID& id, const DebugDrawerColoredPointCloud& pointCloud);
+
+ /// Draw a 24 bit colored point cloud.
+ void drawPointCloud(const VisuID& id, const DebugDrawer24BitColoredPointCloud& pointCloud);
+
+
+ /// Forces the "deletion" of a point cloud by drawing an empty one.
+ void clearColoredPointCloud(const VisuID& id);
+
+
// CUSTOM
/**
@@ -446,10 +547,15 @@ namespace armarx
bool ignoreLengthScale = false);
+ // STATUS
+
/// 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.
operator bool() const;
+
+
+ // OPERATORS
/// Conversion operator to DebugDrawerInterfacePrx.
operator DebugDrawerInterfacePrx& ();
@@ -460,7 +566,7 @@ namespace armarx
const DebugDrawerInterfacePrx& operator->() const;
- // STATIC
+ public: // STATIC
/**
* @brief Convert a RGB color to HSV.
@@ -480,9 +586,10 @@ namespace armarx
/**
* @brief Construct a DrawColor from the given color type.
*
- * The used color type must have members named "r", "g" and "b".
+ * The used color type must have members named `r`, `g` and `b`.
* Applicable types include:
* - pcl::RGB (byteToFloat = true)
+ * - armarx::DrawColor (useful to get a color with a different alpha)
*
* @param alpha the alpha (default: 1)
* @param byteToFloat If true, scale from range [0, 255] to [0, 1]
@@ -524,7 +631,7 @@ namespace armarx
DebugDrawerInterfacePrx topic = nullptr;
/// The default layer (used if none is passed to the method).
- std::string _layer = "debug";
+ std::string _layer = DEFAULT_LAYER;
/// Scaling for positions, lengths and distances.
float _lengthScale = 1;
@@ -541,7 +648,7 @@ namespace armarx
template <typename DurationT>
void DebugDrawerTopic::sleepFor(const DurationT& duration)
{
- if (topic)
+ if (enabled())
{
std::this_thread::sleep_for(duration);
}
@@ -556,8 +663,71 @@ namespace armarx
template <class ColorT>
DrawColor DebugDrawerTopic::toDrawColor(const ColorT& color, float alpha, bool byteToFloat)
{
- float scale = byteToFloat ? (1 / 255.f) : 1;
+ const float scale = byteToFloat ? (1 / 255.f) : 1;
return { color.r * scale, color.g * scale, color.b * scale, alpha };
}
+
+ template <class PointCloudT>
+ void DebugDrawerTopic::drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const DrawColor& color,
+ float pointSize,
+ bool ignoreLengthScale)
+ {
+ drawPointCloud(id, pointCloud,
+ [&color](const auto&)
+ {
+ return color;
+ },
+ pointSize, ignoreLengthScale);
+ }
+
+ template<class PointCloudT>
+ void DebugDrawerTopic::drawPointCloudRGBA(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize,
+ bool ignoreLengthScale)
+ {
+ drawPointCloud(id, pointCloud,
+ [](const auto & p)
+ {
+ return toDrawColor(p, p.a);
+ },
+ pointSize, ignoreLengthScale);
+ }
+
+ template <class PointCloudT, class ColorFuncT>
+ void DebugDrawerTopic::drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const ColorFuncT& colorFn,
+ float pointSize,
+ bool ignoreLengthScale)
+ {
+ if (!enabled())
+ {
+ return;
+ }
+
+ const float lf = ignoreLengthScale ? 1.0 : _lengthScale;
+
+ DebugDrawerColoredPointCloud dd;
+ dd.points.reserve(pointCloud.size());
+
+ dd.pointSize = pointSize;
+
+ for (const auto& p : pointCloud)
+ {
+ dd.points.push_back(DebugDrawerColoredPointCloudElement
+ {
+ lf * p.x, lf * p.y, lf * p.z, colorFn(p)
+ });
+ }
+
+ drawPointCloud(id, dd);
+ }
+
}