added point cloud and global enable in DebugDrawerHelper

source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp

@@ -30,6 +30,8 @@
 using namespace math;
 using namespace armarx;
 
+#define CHECK_ENABLE_VISU if (!enableVisu) return;
+
 DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot)
     : debugDrawerPrx(debugDrawerPrx), robot(robot), rn(robot->getRootNode())
 {
@@ -37,49 +39,86 @@ DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerP
 
 void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
 {
+    CHECK_ENABLE_VISU
     debugDrawerPrx->setPoseVisu(layerName, name, makeGlobal(pose));
 }
 
 void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale)
 {
+    CHECK_ENABLE_VISU
     debugDrawerPrx->setScaledPoseVisu(layerName, name, makeGlobal(pose), scale);
 }
 
 void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color)
 {
+    CHECK_ENABLE_VISU
     drawBox(name, Helpers::CreatePose(position, Eigen::Matrix3f::Identity()), Eigen::Vector3f(size, size, size), color);
 }
 
 void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Matrix4f& pose, float size, const DrawColor& color)
 {
+    CHECK_ENABLE_VISU
     drawBox(name, pose, Eigen::Vector3f(size, size, size), color);
 }
 
 void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color)
 {
+    CHECK_ENABLE_VISU
     debugDrawerPrx->setBoxVisu(layerName, name, makeGlobal(pose), new Vector3(size), color);
 }
 
 void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color)
 {
+    CHECK_ENABLE_VISU
     debugDrawerPrx->setLineVisu(layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
 }
 
 void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
 {
+    CHECK_ENABLE_VISU
     drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
 }
 
 void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size)
 {
+    CHECK_ENABLE_VISU
     debugDrawerPrx->setTextVisu(layerName, name, text, makeGlobal(p1), color, size);
 }
 
 void DebugDrawerHelper::drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width)
 {
+    CHECK_ENABLE_VISU
     debugDrawerPrx->setArrowVisu(layerName, name, makeGlobal(pos), makeGlobalDirection(direction), color, length, width);
 }
 
+void DebugDrawerHelper::drawPointCloud(const std::string& name, const std::vector<Eigen::Vector3f>& points, float pointSize, const DrawColor& color)
+{
+    CHECK_ENABLE_VISU
+    DebugDrawerColoredPointCloud pc;
+    pc.pointSize = pointSize;
+    for (const Eigen::Vector3f& p : points)
+    {
+        Eigen::Vector3f pg = rn->getGlobalPosition(p);
+        DebugDrawerColoredPointCloudElement e;
+        e.x = pg(0);
+        e.y = pg(1);
+        e.z = pg(2);
+        e.color = color; //DrawColor24Bit {(Ice::Byte)(color.r * 255), (Ice::Byte)(color.g * 255), (Ice::Byte)(color.b * 255)};
+        pc.points.push_back(e);
+    }
+    debugDrawerPrx->setColoredPointCloudVisu(layerName, name, pc);
+}
+
+void DebugDrawerHelper::clearLayer()
+{
+    debugDrawerPrx->clearLayer(layerName);
+}
+
+void DebugDrawerHelper::setVisuEnabled(bool enableVisu)
+{
+    this->enableVisu = enableVisu;
+}
+
 PosePtr DebugDrawerHelper::makeGlobal(const Eigen::Matrix4f& pose)
 {
     return new Pose(rn->getGlobalPose(pose));

source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h

@@ -61,6 +61,12 @@ namespace armarx
 
         void drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width);
 
+        void drawPointCloud(const std::string& name, const std::vector<Eigen::Vector3f>& points, float pointSize, const DrawColor& color);
+
+        void clearLayer();
+
+        void setVisuEnabled(bool enableVisu);
+
         PosePtr makeGlobal(const Eigen::Matrix4f& pose);
         Vector3Ptr makeGlobal(const Eigen::Vector3f& position);
         Vector3Ptr makeGlobalDirection(const Eigen::Vector3f& direction);
@@ -69,6 +75,7 @@ namespace armarx
         Defaults defaults;
 
     private:
+        bool enableVisu = true;
         DebugDrawerInterfacePrx debugDrawerPrx;
         std::string layerName;
         VirtualRobot::RobotPtr robot;