Skip to content
Snippets Groups Projects
Commit 603b026d authored by Raphael Grimm's avatar Raphael Grimm
Browse files

added colored point cloud version using 24bit for color (instead of 4*32) to... 

added colored point cloud version using 24bit for color (instead of 4*32) to the DebugDrawerComponent
parent 1f2b165c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+ 139
− 17
View file @ 603b026d
......@@ -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
source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+ 14
− 0
View file @ 603b026d
......@@ -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;
......
source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+ 26
− 1
View file @ 603b026d
......@@ -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);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment