diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 508180247963def8b29e4555b1cb8420a0920303..7a0ca2e58fefad205fd3080a07f2c7b6a1cf7305 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -179,7 +179,7 @@ namespace armarx
removeSelectionCallbacks();
selectionNode->deselectAll();
- for (auto& e : selectedElements)
+ for (auto & e : selectedElements)
{
SoNode* n = SoSelection::getByName((SELECTION_NAME_PREFIX + e).c_str());
if (n)
@@ -210,7 +210,7 @@ namespace armarx
name = name.substr(SELECTION_NAME_PREFIX.length());
// Check if selected element is 'selectable'
- for (auto& layer : selectableLayers)
+ for (auto & layer : selectableLayers)
{
if (layers[layer].addedBoxVisualizations.find(name) != layers[layer].addedBoxVisualizations.end()
|| layers[layer].addedTextVisualizations.find(name) != layers[layer].addedTextVisualizations.end()
@@ -1555,7 +1555,7 @@ namespace armarx
d.layerName = layerName;
d.name = robotName;
- for (auto& it : configuration)
+ for (auto & it : configuration)
{
d.configuration[it.first] = it.second;
}
@@ -1602,7 +1602,7 @@ namespace armarx
void DebugDrawerComponent::clearAll(const Ice::Current&)
{
- for (auto& i : layers)
+ for (auto & i : layers)
{
clearLayer(i.first);
}
@@ -1636,57 +1636,57 @@ namespace armarx
auto& layer = layers.at(layerName);
- for (const auto& i : layer.addedCoordVisualizations)
+ for (const auto & i : layer.addedCoordVisualizations)
{
removePoseVisu(layerName, i.first);
}
- for (const auto& i : layer.addedLineVisualizations)
+ for (const auto & i : layer.addedLineVisualizations)
{
removeLineVisu(layerName, i.first);
}
- for (const auto& i : layer.addedBoxVisualizations)
+ for (const auto & i : layer.addedBoxVisualizations)
{
removeBoxVisu(layerName, i.first);
}
- for (const auto& i : layer.addedTextVisualizations)
+ for (const auto & i : layer.addedTextVisualizations)
{
removeTextVisu(layerName, i.first);
}
- for (const auto& i : layer.addedSphereVisualizations)
+ for (const auto & i : layer.addedSphereVisualizations)
{
removeSphereVisu(layerName, i.first);
}
- for (const auto& i : layer.addedCylinderVisualizations)
+ for (const auto & i : layer.addedCylinderVisualizations)
{
removeCylinderVisu(layerName, i.first);
}
- for (const auto& i : layer.addedPointCloudVisualizations)
+ for (const auto & i : layer.addedPointCloudVisualizations)
{
removePointCloudVisu(layerName, i.first);
}
- for (const auto& i : layer.addedPolygonVisualizations)
+ for (const auto & i : layer.addedPolygonVisualizations)
{
removePolygonVisu(layerName, i.first);
}
- for (const auto& i : layer.addedArrowVisualizations)
+ for (const auto & i : layer.addedArrowVisualizations)
{
removeArrowVisu(layerName, i.first);
}
- for (const auto& i : layer.addedRobotVisualizations)
+ for (const auto & i : layer.addedRobotVisualizations)
{
removeRobotVisu(layerName, i.first);
}
- for (const auto& i : layer.addedCustomVisualizations)
+ for (const auto & i : layer.addedCustomVisualizations)
{
removeCustomVisu(layerName, i.first);
}
@@ -1835,6 +1835,13 @@ namespace armarx
accumulatedUpdateData.coloredpointcloud.clear();
+ for (auto i = accumulatedUpdateData.colored24Bitpointcloud.begin(); i != accumulatedUpdateData.colored24Bitpointcloud.end(); i++)
+ {
+ draw24BitColoredPointCloud(i->second);
+ }
+
+ accumulatedUpdateData.colored24Bitpointcloud.clear();
+
onUpdateVisualization();
}
@@ -2063,7 +2070,7 @@ namespace armarx
ScopedRecursiveLockPtr l = getScopedVisuLock();
StringSequence seq {};
- for (const auto& layer : layers)
+ for (const auto & layer : layers)
{
seq.push_back(layer.first);
}
@@ -2076,7 +2083,7 @@ namespace armarx
::armarx::LayerInformationSequence seq {};
ScopedRecursiveLockPtr l = getScopedVisuLock();
- for (const auto& layer : layers)
+ for (const auto & layer : layers)
{
int count = layer.second.addedCoordVisualizations.size() +
layer.second.addedLineVisualizations.size() +
@@ -2210,4 +2217,119 @@ namespace armarx
removeColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
+ void DebugDrawerComponent::draw24BitColoredPointCloud(const Colored24BitPointCloudData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ auto& layer = requestLayer(d.layerName);
+
+ remove24BitColoredPointCloud(d.layerName, d.name);
+
+ if (!d.active)
+ {
+ return;
+ }
+
+ const auto& pcl = d.pointCloud.points;
+
+ SoSeparator* pclSep = new SoSeparator;
+
+ SoMaterial* pclMat = new SoMaterial;
+ std::vector<SbColor> colors;
+ colors.reserve(pcl.size());
+ std::transform(
+ pcl.begin(), pcl.end(), std::back_inserter(colors),
+ [](const DebugDrawer24BitColoredPointCloudElement & elem)
+ {
+ return SbColor {static_cast<float>(elem.color.r) / 255.f, static_cast<float>(elem.color.g) / 255.f, static_cast<float>(elem.color.b) / 255.f};
+ }
+ );
+ pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
+ pclMat->ambientColor.setValues(0, colors.size(), colors.data());
+ pclSep->addChild(pclMat);
+
+ 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 DebugDrawer24BitColoredPointCloudElement & 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);
+
+ pclSep->addChild(new SoPointSet);
+
+ ARMARX_INFO << d.name << "PointCloud Update " << pcl.size();
+
+ layer.added24BitColoredPointCloudVisualizations[d.name] = pclSep;
+ layer.mainNode->addChild(pclSep);
+ }
+
+ void DebugDrawerComponent::remove24BitColoredPointCloud(const std::string& layerName, const std::string& name)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
+
+ auto& layer = layers.at(layerName);
+
+ if (layer.added24BitColoredPointCloudVisualizations.find(name) == layer.added24BitColoredPointCloudVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.added24BitColoredPointCloudVisualizations[name]);
+ layer.added24BitColoredPointCloudVisualizations.erase(name);
+ }
+
+ void DebugDrawerComponent::set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
+ {
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
+ Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
+ d.pointCloud = pointCloud;
+ d.layerName = layerName;
+ d.name = pointCloudName;
+ d.active = true;
+ }
+ }
+
+ void DebugDrawerComponent::set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
+ {
+ set24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
+ }
+
+ void DebugDrawerComponent::remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
+ {
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
+ Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
+ d.layerName = layerName;
+ d.name = pointCloudName;
+ d.active = false;
+ }
+ }
+
+ void DebugDrawerComponent::remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
+ {
+ remove24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
+ }
+
}//namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index bae49caa056a5aead0cd79525e30b5483a6bb521..6511755e5cc214ad43815005119aef02c107b140 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -187,6 +187,12 @@ namespace armarx
virtual void removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
virtual void removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
+ virtual void set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
+ virtual void remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
+
+
virtual void setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
virtual void setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
virtual void removePolygonVisu(const std::string& layerName, const std::string& polygonName, const ::Ice::Current& = ::Ice::Current());
@@ -332,6 +338,10 @@ namespace armarx
{
DebugDrawerColoredPointCloud pointCloud;
};
+ struct Colored24BitPointCloudData : public DrawData
+ {
+ DebugDrawer24BitColoredPointCloud pointCloud;
+ };
struct PolygonData : public DrawData
{
std::vector< Eigen::Vector3f > points;
@@ -380,6 +390,7 @@ namespace armarx
std::map<std::string, CylinderData> cylinder;
std::map<std::string, PointCloudData> pointcloud;
std::map<std::string, ColoredPointCloudData> coloredpointcloud;
+ std::map<std::string, Colored24BitPointCloudData> colored24Bitpointcloud;
std::map<std::string, PolygonData> polygons;
std::map<std::string, ArrowData> arrows;
std::map<std::string, RobotData> robots;
@@ -406,6 +417,7 @@ namespace armarx
void drawCylinder(const CylinderData& d);
void drawPointCloud(const PointCloudData& d);
void drawColoredPointCloud(const ColoredPointCloudData& d);
+ void draw24BitColoredPointCloud(const Colored24BitPointCloudData& d);
void drawPolygon(const PolygonData& d);
void drawArrow(const ArrowData& d);
void drawRobot(const RobotData& d);
@@ -418,6 +430,7 @@ namespace armarx
void removeCylinder(const std::string& layerName, const std::string& name);
void removePointCloud(const std::string& layerName, const std::string& name);
void removeColoredPointCloud(const std::string& layerName, const std::string& name);
+ void remove24BitColoredPointCloud(const std::string& layerName, const std::string& name);
void removePolygon(const std::string& layerName, const std::string& name);
void removeArrow(const std::string& layerName, const std::string& name);
void removeRobot(const std::string& layerName, const std::string& name);
@@ -438,6 +451,7 @@ namespace armarx
std::map<std::string, SoSeparator*> addedCylinderVisualizations;
std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
std::map<std::string, SoSeparator*> addedColoredPointCloudVisualizations;
+ std::map<std::string, SoSeparator*> added24BitColoredPointCloudVisualizations;
std::map<std::string, SoSeparator*> addedPolygonVisualizations;
std::map<std::string, SoSeparator*> addedArrowVisualizations;
std::map<std::string, SoSeparator*> addedRobotVisualizations;
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 8e7e85c35ed7f8ab51738c5bcc6fb6e7b4c13ceb..198ff86013a71e0cf4c6131b53aaac18a90e8e3a 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -43,6 +43,13 @@ module armarx
float a;
};
+ struct DrawColor24Bit
+ {
+ byte r;
+ byte g;
+ byte b;
+ };
+
/*!
* \brief Contains information about a layer. (name, visibility and number of elements)
@@ -87,6 +94,21 @@ module armarx
float pointSize = three;
};
+ struct DebugDrawer24BitColoredPointCloudElement
+ {
+ float x;
+ float y;
+ float z;
+ DrawColor24Bit color;
+ };
+ sequence<DebugDrawer24BitColoredPointCloudElement> DebugDrawer24BitColoredPointCloudElementList;
+
+ struct DebugDrawer24BitColoredPointCloud
+ {
+ DebugDrawer24BitColoredPointCloudElementList points;
+ float pointSize = three;
+ };
+
sequence< Vector3Base > PolygonPointList;
enum DrawStyle { FullModel, CollisionModel };
@@ -117,6 +139,7 @@ module armarx
void setSphereVisu(string layerName, string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
void setPointCloudVisu(string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
void setColoredPointCloudVisu(string layerName, string pointCloudName, DebugDrawerColoredPointCloud pointCloud);
+ void set24BitColoredPointCloudVisu(string layerName, string pointCloudName, DebugDrawer24BitColoredPointCloud pointCloud);
void setPolygonVisu(string layerName, string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
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);
@@ -155,7 +178,7 @@ module armarx
void setTextDebugLayerVisu(string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
void setSphereDebugLayerVisu(string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
void setPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerPointCloud pointCloud);
- void setColoredPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerColoredPointCloud pointCloud);
+ void set24BitColoredPointCloudDebugLayerVisu(string pointCloudName, DebugDrawer24BitColoredPointCloud pointCloud);
void setPolygonDebugLayerVisu(string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
void setArrowDebugLayerVisu(string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
void setCylinderDebugLayerVisu(string cylinderName, Vector3Base globalPosition, Vector3Base direction, float length, float radius, DrawColor color);
@@ -171,6 +194,7 @@ module armarx
void removeSphereVisu(string layerName, string sphereName);
void removePointCloudVisu(string layerName, string pointCloudName);
void removeColoredPointCloudVisu(string layerName, string pointCloudName);
+ void remove24BitColoredPointCloudVisu(string layerName, string pointCloudName);
void removePolygonVisu(string layerName, string polygonName);
void removeArrowVisu(string layerName, string arrowName);
void removeCylinderVisu(string layerName, string cylinderName);
@@ -186,6 +210,7 @@ module armarx
void removeSphereDebugLayerVisu(string sphereName);
void removePointCloudDebugLayerVisu(string pointCloudName);
void removeColoredPointCloudDebugLayerVisu(string pointCloudName);
+ void remove24BitColoredPointCloudDebugLayerVisu(string pointCloudName);
void removePolygonDebugLayerVisu(string polygonName);
void removeArrowDebugLayerVisu(string arrowName);
void removeCylinderDebugLayerVisu(string cylinderName);