diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index d9748e51b764e6be50397069b63ea4a58e188238..0fc8ec85132a9ef8fc240e7e8daa57b90554b0d2 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -757,17 +757,17 @@ namespace armarx
TriMeshModelPtr triMesh = TriMeshModelPtr(new TriMeshModel());
- for(DrawColor color : d.triMesh.colors)
+ for (DrawColor color : d.triMesh.colors)
{
triMesh->addColor(VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, color.a));
}
- for(DebugDrawerVertex v : d.triMesh.vertices)
+ for (DebugDrawerVertex v : d.triMesh.vertices)
{
triMesh->addVertex(Eigen::Vector3f(v.x, v.y, v.z));
}
- for(DebugDrawerFace f : d.triMesh.faces)
+ for (DebugDrawerFace f : d.triMesh.faces)
{
MathTools::TriangleFace face;
@@ -1889,14 +1889,12 @@ namespace armarx
void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
{
ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ //updates should only contain elements to add for each layer after the last clear for this layer was executed
+ //this prevents a sequence add,clear,add to result in an empty layer
+ //=>remove all objects pending for this layer
+ removeAccumulatedData(layerName);
+ accumulatedUpdateData.clearLayers.emplace(layerName);
- // check if already requested
- if (std::find(accumulatedUpdateData.clearLayers.begin(), accumulatedUpdateData.clearLayers.end(), layerName) != accumulatedUpdateData.clearLayers.end())
- {
- return;
- }
-
- accumulatedUpdateData.clearLayers.push_back(layerName);
}
void DebugDrawerComponent::clearLayerQt(const std::string& layerName)
@@ -1923,8 +1921,6 @@ namespace armarx
selectionNode->deselectAll();
- removeAccumulatedData(layerName);
-
Layer& layer = layers.at(layerName);
for (const auto & i : layer.addedCoordVisualizations)
@@ -2062,120 +2058,109 @@ namespace armarx
ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
- for (auto i = accumulatedUpdateData.coord.begin(); i != accumulatedUpdateData.coord.end(); i++)
+ // check for clear&remove
+ //(updates only contain elements to add for each layer after the last clear for this layer was executed)
+ //this prevents a sequence add,clear,add to result in an empty layer
+ for (const auto & layer : accumulatedUpdateData.clearLayers)
{
- drawCoordSystem(i->second);
+ clearLayerQt(layer);
}
+ accumulatedUpdateData.clearLayers.clear();
- accumulatedUpdateData.coord.clear();
+ for (const auto & layer : accumulatedUpdateData.removeLayers)
+ {
+ removeLayerQt(layer);
+ }
+ accumulatedUpdateData.removeLayers.clear();
- for (auto i = accumulatedUpdateData.box.begin(); i != accumulatedUpdateData.box.end(); i++)
+ //add elements
+ for (auto & e : accumulatedUpdateData.coord)
{
- drawBox(i->second);
+ drawCoordSystem(e.second);
}
+ accumulatedUpdateData.coord.clear();
+ for (auto & e : accumulatedUpdateData.box)
+ {
+ drawBox(e.second);
+ }
accumulatedUpdateData.box.clear();
- for (auto i = accumulatedUpdateData.line.begin(); i != accumulatedUpdateData.line.end(); i++)
+ for (auto & e : accumulatedUpdateData.line)
{
- drawLine(i->second);
+ drawLine(e.second);
}
-
accumulatedUpdateData.line.clear();
- for (auto i = accumulatedUpdateData.lineSet.begin(); i != accumulatedUpdateData.lineSet.end(); i++)
+ for (auto & e : accumulatedUpdateData.lineSet)
{
- drawLineSet(i->second);
+ drawLineSet(e.second);
}
-
accumulatedUpdateData.lineSet.clear();
- for (auto i = accumulatedUpdateData.sphere.begin(); i != accumulatedUpdateData.sphere.end(); i++)
+ for (auto & e : accumulatedUpdateData.sphere)
{
- drawSphere(i->second);
+ drawSphere(e.second);
}
-
accumulatedUpdateData.sphere.clear();
- for (auto i = accumulatedUpdateData.cylinder.begin(); i != accumulatedUpdateData.cylinder.end(); i++)
+ for (auto & e : accumulatedUpdateData.cylinder)
{
- drawCylinder(i->second);
+ drawCylinder(e.second);
}
-
accumulatedUpdateData.cylinder.clear();
- for (auto i = accumulatedUpdateData.text.begin(); i != accumulatedUpdateData.text.end(); i++)
+ for (auto & e : accumulatedUpdateData.text)
{
- drawText(i->second);
+ drawText(e.second);
}
-
accumulatedUpdateData.text.clear();
- for (auto i = accumulatedUpdateData.pointcloud.begin(); i != accumulatedUpdateData.pointcloud.end(); i++)
+ for (auto & e : accumulatedUpdateData.pointcloud)
{
- drawPointCloud(i->second);
+ drawPointCloud(e.second);
}
-
accumulatedUpdateData.pointcloud.clear();
- for (auto i = accumulatedUpdateData.polygons.begin(); i != accumulatedUpdateData.polygons.end(); i++)
+ for (auto & e : accumulatedUpdateData.polygons)
{
- drawPolygon(i->second);
+ drawPolygon(e.second);
}
-
accumulatedUpdateData.polygons.clear();
- for (auto i = accumulatedUpdateData.arrows.begin(); i != accumulatedUpdateData.arrows.end(); i++)
+ for (auto & e : accumulatedUpdateData.arrows)
{
- drawArrow(i->second);
+ drawArrow(e.second);
}
-
accumulatedUpdateData.arrows.clear();
for (auto i = accumulatedUpdateData.triMeshes.begin(); i != accumulatedUpdateData.triMeshes.end(); i++)
{
drawTriMesh(i->second);
}
-
accumulatedUpdateData.triMeshes.clear();
- for (auto i = accumulatedUpdateData.robots.begin(); i != accumulatedUpdateData.robots.end(); i++)
+ for (auto & e : accumulatedUpdateData.robots)
{
- ARMARX_DEBUG << "update visu / drawRobot for robot " << i->first;
- ensureExistingRobotNodes(i->second);
+ ARMARX_DEBUG << "update visu / drawRobot for robot " << e.first;
+ ensureExistingRobotNodes(e.second);
- drawRobot(i->second);
+ drawRobot(e.second);
}
-
accumulatedUpdateData.robots.clear();
- for (auto i = accumulatedUpdateData.coloredpointcloud.begin(); i != accumulatedUpdateData.coloredpointcloud.end(); i++)
+ for (auto & e : accumulatedUpdateData.coloredpointcloud)
{
- drawColoredPointCloud(i->second);
+ drawColoredPointCloud(e.second);
}
-
accumulatedUpdateData.coloredpointcloud.clear();
- for (auto i = accumulatedUpdateData.colored24Bitpointcloud.begin(); i != accumulatedUpdateData.colored24Bitpointcloud.end(); i++)
+ for (auto & e : accumulatedUpdateData.colored24Bitpointcloud)
{
- draw24BitColoredPointCloud(i->second);
+ draw24BitColoredPointCloud(e.second);
}
-
accumulatedUpdateData.colored24Bitpointcloud.clear();
- // check for clear and remove
- for (auto i = accumulatedUpdateData.clearLayers.begin(); i != accumulatedUpdateData.clearLayers.end(); i++)
- {
- clearLayerQt(*i);
- }
- accumulatedUpdateData.clearLayers.clear();
-
- for (auto i = accumulatedUpdateData.removeLayers.begin(); i != accumulatedUpdateData.removeLayers.end(); i++)
- {
- removeLayerQt(*i);
- }
- accumulatedUpdateData.removeLayers.clear();
-
onUpdateVisualization();
}
@@ -2188,13 +2173,11 @@ namespace armarx
void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
{
ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- // check if already requested
- if (std::find(accumulatedUpdateData.removeLayers.begin(), accumulatedUpdateData.removeLayers.end(), layerName) != accumulatedUpdateData.removeLayers.end())
- {
- return;
- }
-
- accumulatedUpdateData.removeLayers.push_back(layerName);
+ //updates should only contain elements to add for each layer after the last clear/remove for this layer was executed
+ //this prevents a sequence add,clear,add to result in an empty layer
+ //=>remove all objects pending for this layer
+ removeAccumulatedData(layerName);
+ accumulatedUpdateData.removeLayers.emplace(layerName);
}
void DebugDrawerComponent::removeLayerQt(const std::string& layerName)
@@ -2225,208 +2208,38 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
+ template<class UpdateDataType>
+ void removeUpdateDataFromMap(const std::string& layerName, std::map<std::string, UpdateDataType>& map)
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- std::string entryName = "__" + layerName + "__";
- {
- auto i1 = accumulatedUpdateData.coord.begin();
-
- while (i1 != accumulatedUpdateData.coord.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.coord.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.box.begin();
-
- while (i1 != accumulatedUpdateData.box.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.box.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.line.begin();
-
- while (i1 != accumulatedUpdateData.line.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.line.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.lineSet.begin();
-
- while (i1 != accumulatedUpdateData.lineSet.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.lineSet.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.sphere.begin();
-
- while (i1 != accumulatedUpdateData.sphere.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.sphere.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.cylinder.begin();
-
- while (i1 != accumulatedUpdateData.cylinder.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.cylinder.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.text.begin();
-
- while (i1 != accumulatedUpdateData.text.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.text.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.pointcloud.begin();
-
- while (i1 != accumulatedUpdateData.pointcloud.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.pointcloud.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.polygons.begin();
-
- while (i1 != accumulatedUpdateData.polygons.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.polygons.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
- {
- auto i1 = accumulatedUpdateData.triMeshes.begin();
-
- while (i1 != accumulatedUpdateData.triMeshes.end())
- {
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.triMeshes.erase(i1);
- }
- else
- {
- i1++;
- }
- }
- }
-
+ auto it = map.begin();
+ while (it != map.end())
{
- auto i1 = accumulatedUpdateData.arrows.begin();
-
- while (i1 != accumulatedUpdateData.arrows.end())
+ auto curr = it++;
+ if (curr->second.layerName == layerName)
{
- if (boost::starts_with(i1->first, entryName))
- {
- i1 = accumulatedUpdateData.arrows.erase(i1);
- }
- else
- {
- i1++;
- }
+ map.erase(curr);
}
}
+ }
- {
- ARMARX_DEBUG << "Removing all accumulated data for robot, nr of robots:" << accumulatedUpdateData.robots.size();
-
- auto i1 = accumulatedUpdateData.robots.begin();
-
- while (i1 != accumulatedUpdateData.robots.end())
- {
- ARMARX_DEBUG << "checking accumulated data " << i1->first;
-
- if (boost::starts_with(i1->first, entryName))
- {
- ARMARX_DEBUG << "removing accumulated data for " << i1->first;
- i1 = accumulatedUpdateData.robots.erase(i1);
- }
- else
- {
- i1++;
- }
- }
+ void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
+ {
+ ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- ARMARX_DEBUG << "end, nr of robots:" << accumulatedUpdateData.robots.size();
- }
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.coord);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.line);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.lineSet);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.box);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.text);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.sphere);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.cylinder);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.pointcloud);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.coloredpointcloud);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.colored24Bitpointcloud);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.polygons);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.arrows);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.robots);
+ removeUpdateDataFromMap(layerName, accumulatedUpdateData.triMeshes);
onRemoveAccumulatedData(layerName);
@@ -2460,7 +2273,6 @@ namespace armarx
::armarx::LayerInformationSequence seq {};
ScopedRecursiveLockPtr l = getScopedVisuLock();
- // @@@ TODO: FIXME: count does not include all maps!
for (const auto & layer : layers)
{
int count = layer.second.addedCoordVisualizations.size() +
@@ -2471,9 +2283,11 @@ namespace armarx
layer.second.addedSphereVisualizations.size() +
layer.second.addedCylinderVisualizations.size() +
layer.second.addedPointCloudVisualizations.size() +
- layer.second.addedArrowVisualizations.size() +
+ layer.second.addedColoredPointCloudVisualizations.size() +
+ layer.second.added24BitColoredPointCloudVisualizations.size() +
layer.second.addedPolygonVisualizations.size() +
layer.second.addedTriMeshVisualizations.size() +
+ layer.second.addedArrowVisualizations.size() +
layer.second.addedRobotVisualizations.size() +
layer.second.addedCustomVisualizations.size();
::armarx::LayerInformation info = {layer.first, layer.second.visible, count};
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index dbf74aacdb8907dc0714f916104e43938e556bb7..9e773a3b4442dc34cda8cecf560a828163c9f398 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -298,6 +298,9 @@ namespace armarx
std::string layerName;
std::string name;
bool active;
+ /**
+ * @brief Whether an existing visu should be updated
+ */
bool update;
};
@@ -418,8 +421,8 @@ namespace armarx
std::map<std::string, ArrowData> arrows;
std::map<std::string, RobotData> robots;
- std::vector< std::string > clearLayers;
- std::vector< std::string > removeLayers;
+ std::set< std::string > clearLayers;
+ std::set< std::string > removeLayers;
};
UpdateData accumulatedUpdateData;