diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 7b109f0b52970e87ffa614756e9a5d9ab8d9c847..3a0de019a99a2db556f9d3057d3ef0ae886d8bf5 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -39,6 +39,12 @@
#include <Inventor/nodes/SoPointSet.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/nodes/SoTranslation.h>
+#include <Inventor/nodes/SoMaterialBinding.h>
+#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/SbVec3f.h>
+#include <Inventor/fields/SoMFVec3f.h>
+#include <Inventor/fields/SoMFColor.h>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -445,32 +451,40 @@ namespace armarx
return;
}
- SoSeparator* sep = new SoSeparator;
- int sz = d.pointCloud.size();
- auto coordinates = new float[sz][3];
+ const auto& pcl = d.pointCloud.points;
+
+ SoSeparator* pclSep = new SoSeparator;
+
+ SoMaterial* pclMat = new SoMaterial;
+ pclMat->diffuseColor.setValue(0.2, 0.2, 0.2);
+ pclMat->diffuseColor.setValue(0.2, 0.2, 0.2);
+ pclSep->addChild(pclMat);
+
+ SoMaterialBinding* pclMatBind = new SoMaterialBinding;
+ pclMatBind->value = SoMaterialBinding::OVERALL;
+ pclSep->addChild(pclMatBind);
- for (int i = 0; i < sz; i++)
+ SoCoordinate3* pclCoords = new SoCoordinate3;
+ std::vector<SbVec3f> coords;
+ coords.reserve(pcl.size());
+ std::transform(
+ pcl.begin(), pcl.end(), std::back_inserter(coords),
+ [](const DebugDrawerPointCloudElement & elem)
{
- const DebugDrawerPointCloudElement& e = d.pointCloud[i];
- coordinates[i][0] = e.x;
- coordinates[i][1] = e.y;
- coordinates[i][2] = e.z;
+ return SbVec3f {elem.x, elem.y, elem.z};
}
+ );
+ pclCoords->point.setValues(0, coords.size(), coords.data());
+ pclSep->addChild(pclCoords);
- SoMaterial* material = new SoMaterial;
- material->ambientColor.setValue(0.2, 0.2, 0.2);
- material->diffuseColor.setValue(0.2, 0.2, 0.2);
- sep->addChild(material);
-
- SoCoordinate3* coords = new SoCoordinate3;
- coords->point.setValues(0, sz, coordinates);
- sep->addChild(coords);
+ SoDrawStyle* pclStye = new SoDrawStyle;
+ pclStye->pointSize = d.pointCloud.pointSize;
+ pclSep->addChild(pclStye);
- SoPointSet* pointSet = new SoPointSet;
- sep->addChild(pointSet);
+ pclSep->addChild(new SoPointSet);
- layer.addedPointCloudVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
+ layer.addedPointCloudVisualizations[d.name] = pclSep;
+ layer.mainNode->addChild(pclSep);
}
void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
@@ -1939,40 +1953,51 @@ namespace armarx
return;
}
- SoSeparator* sep = new SoSeparator;
- int sz = d.pointCloud.size();
+ const auto& pcl = d.pointCloud.points;
- SoSeparator* points = new SoSeparator;
- sep->addChild(points);
- SoSphere* sphere = new SoSphere;
- sphere->radius = 25;
+ SoSeparator* pclSep = new SoSeparator;
- for (int i = 0; i < sz; i++)
+ SoMaterial* pclMat = new SoMaterial;
+ std::vector<SbColor> colors;
+ colors.reserve(pcl.size());
+ std::transform(
+ pcl.begin(), pcl.end(), std::back_inserter(colors),
+ [](const DebugDrawerColoredPointCloudElement & elem)
{
- SoSeparator* point = new SoSeparator;
- points->addChild(point);
-
- const DebugDrawerColoredPointCloudElement& e = d.pointCloud[i];
-
- SoTranslation* trans = new SoTranslation;
- trans->translation.setValue(e.x, e.y, e.z);
- point->addChild(trans);
+ return SbColor {elem.color.r, elem.color.g, elem.color.b};
+ }
+ );
+ pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
+ pclMat->ambientColor.setValues(0, colors.size(), colors.data());
+ pclSep->addChild(pclMat);
- SoMaterial* material = new SoMaterial;
- material->ambientColor.setValue(e.color.r, e.color.g, e.color.b);
- material->diffuseColor.setValue(e.color.r, e.color.g, e.color.b);
- material->transparency.setValue(1 - e.color.a);
- point->addChild(material);
+ SoMaterialBinding* pclMatBind = new SoMaterialBinding;
+ pclMatBind->value = SoMaterialBinding::PER_PART;
+ pclSep->addChild(pclMatBind);
+ SoCoordinate3* pclCoords = new SoCoordinate3;
+ std::vector<SbVec3f> coords;
+ coords.reserve(pcl.size());
+ std::transform(
+ pcl.begin(), pcl.end(), std::back_inserter(coords),
+ [](const DebugDrawerColoredPointCloudElement & elem)
+ {
+ return SbVec3f {elem.x, elem.y, elem.z};
+ }
+ );
+ pclCoords->point.setValues(0, coords.size(), coords.data());
+ pclSep->addChild(pclCoords);
+ SoDrawStyle* pclStye = new SoDrawStyle;
+ pclStye->pointSize = d.pointCloud.pointSize;
+ pclSep->addChild(pclStye);
- point->addChild(sphere);
- }
+ pclSep->addChild(new SoPointSet);
- ARMARX_INFO << d.name << "PointCloud Update " << sz;
+ ARMARX_INFO << d.name << "PointCloud Update " << pcl.size();
- layer.addedColoredPointCloudVisualizations[d.name] = sep;
- layer.mainNode->addChild(sep);
+ layer.addedColoredPointCloudVisualizations[d.name] = pclSep;
+ layer.mainNode->addChild(pclSep);
}
void DebugDrawerComponent::removeColoredPointCloud(const std::string& layerName, const std::string& name)
@@ -2030,4 +2055,4 @@ namespace armarx
removeColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
-}//namespace armarx
\ No newline at end of file
+}//namespace armarx
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 9f025f5239a831a8958babd77a354a53dee82237..81af0a584039272bfc1573653583bb8e6c2d7ffb 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -56,15 +56,21 @@ module armarx
sequence<LayerInformation> LayerInformationSequence;
+ const int three = 3;
+
struct DebugDrawerPointCloudElement
{
float x;
float y;
float z;
};
- sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloud;
+ sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloudElementList;
- const int three = 3;
+ struct DebugDrawerPointCloud
+ {
+ DebugDrawerPointCloudElementList points;
+ float pointSize = three;
+ };
struct DebugDrawerColoredPointCloudElement
{
@@ -72,9 +78,14 @@ module armarx
float y;
float z;
DrawColor color;
- float size = three;
};
- sequence<DebugDrawerColoredPointCloudElement> DebugDrawerColoredPointCloud;
+ sequence<DebugDrawerColoredPointCloudElement> DebugDrawerColoredPointCloudElementList;
+
+ struct DebugDrawerColoredPointCloud
+ {
+ DebugDrawerColoredPointCloudElementList points;
+ float pointSize = three;
+ };
sequence< Vector3Base > PolygonPointList;
@@ -108,7 +119,7 @@ module armarx
void setArrowVisu(string layerName, string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
void setCylinderVisu(string layerName, string cylinderName, Vector3Base globalPosition, Vector3Base direction, float length, float radius, DrawColor color);
-
+
/*!
* \brief setRobotVisu Initializes a robot visualization
* \param layerName The layer