Add drawColor() for unicolored point cloud.

source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h

@@ -441,10 +441,23 @@ namespace armarx
          */
 
         /**
-         * @brief Draw a point cloud.
+         * @brief Draw a unicolored point cloud.
          * 
-         * `pointCloud` must be iterable (provide `begin()` and `end()`),
-         * and its elements must have at least members `x, y, z`.
+         * `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,
+            float pointSize = 1.0f, 
+            bool ignoreLengthScale = false);
+        
+        /**
+         * @brief Draw a colored point cloud.
+         * 
+         * `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`.
@@ -454,7 +467,8 @@ namespace armarx
             const VisuID& id,
             const PointCloudT& pointCloud,
             const ColorFuncT& colorFunc,
-            float pointSize = 1.0f);
+            float pointSize = 1.0f, 
+            bool ignoreLengthScale = false);
 
         
         // CUSTOM
@@ -562,9 +576,9 @@ namespace armarx
 
         /// The duration for shortSleep().
         std::chrono::milliseconds shortSleepDuration { 100 };
-
+        
     };
-
+    
 
     template <typename DurationT>
     void DebugDrawerTopic::sleepFor(const DurationT& duration)
@@ -589,28 +603,44 @@ namespace armarx
     }
 
 
+    template <class PointCloudT>
+    void DebugDrawerTopic::drawPointCloud(
+            const DebugDrawerTopic::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, class ColorFuncT>
     void DebugDrawerTopic::drawPointCloud(
         const VisuID& id,
         const PointCloudT& pointCloud,
         const ColorFuncT& colorFn,
-        float pointSize)
+        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
             {
-                p.x, p.y, p.z, colorFn(p)
+                lf * p.x, lf * p.y, lf * p.z, colorFn(p)
             });
         }