diff --git a/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTest.cpp b/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTest.cpp
index ae00f7755b5f71eaeaaf7907617d065542f8c612..5a99c14e9b873e04609d24fc6a1a5eeed393c812 100644
--- a/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTest.cpp
+++ b/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTest.cpp
@@ -27,10 +27,10 @@ namespace armarx
{
void DebugDrawerTest::onInitComponent()
{
-
-
+
+
}
-
+
void DebugDrawerTest::onConnectComponent()
{
@@ -38,29 +38,42 @@ namespace armarx
try
{
prxDD = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
- std::string robFile="Armar3/robotmodel/ArmarIII.xml";
- std::string project="Armar3";
+ std::string robFile = "RobotAPI/robots/Armar3/ArmarIII.xml";
+ std::string project = "RobotAPI";
ARMARX_INFO << "adding robot visu";
prxDD->setRobotVisu("TestVisu", "TestRobot", robFile, project, DrawStyle::FullModel);
ARMARX_INFO << "setting robot position";
Eigen::Matrix4f p;
p.setIdentity();
- p(0,3) = 1000.0f;
- p(1,3) = 1000.0f;
+ p(0, 3) = 1000.0f;
+ p(1, 3) = 1000.0f;
PosePtr gp(new Pose(p));
prxDD->updateRobotPose("TestVisu", "TestRobot", gp);
+ ARMARX_INFO << "sleeping 10 seconds...";
+ usleep(10000000);
+
+ ARMARX_INFO << "setting config of elbow L";
+ NameValueMap conf;
+ conf["Elbow L"] = 0.5;
+ conf["Elbow XY"] = 0.5; // should be ignored!
+
+ prxDD->updateRobotConfig("TestVisu", "TestRobot", conf);
+
+ ARMARX_INFO << "sleeping 10 seconds...";
+ usleep(10000000);
+
ARMARX_INFO << "adding second robot visu";
prxDD->setRobotVisu("TestVisu", "TestRobot2", robFile, project, DrawStyle::FullModel);
ARMARX_INFO << "setting second robot position";
- p(0,3) = 3000.0f;
- p(1,3) = 3000.0f;
+ p(0, 3) = 3000.0f;
+ p(1, 3) = 3000.0f;
PosePtr gp2(new Pose(p));
prxDD->updateRobotPose("TestVisu", "TestRobot2", gp2);
ARMARX_INFO << "sleeping 10 seconds...";
- usleep (10000000);
+ usleep(10000000);
ARMARX_INFO << "removing second robot visu";
prxDD->removeRobotVisu("TestVisu", "TestRobot2");
@@ -74,33 +87,34 @@ namespace armarx
prxDD->updateRobotColor("TestVisu", "TestRobot", c);
ARMARX_INFO << "sleeping 10 seconds...";
- usleep (10000000);
+ usleep(10000000);
ARMARX_INFO << "clearing layer";
prxDD->clearLayer("TestVisu");
ARMARX_INFO << "sleeping 10 seconds...";
- usleep (10000000);
+ usleep(10000000);
ARMARX_INFO << "adding robot visu";
prxDD->setRobotVisu("TestVisu", "TestRobot", robFile, project, DrawStyle::FullModel);
ARMARX_INFO << "setting robot position 6000/1000";
- p(0,3) = 6000.0f;
- p(1,3) = 1000.0f;
+ p(0, 3) = 6000.0f;
+ p(1, 3) = 1000.0f;
PosePtr gp3(new Pose(p));
prxDD->updateRobotPose("TestVisu", "TestRobot", gp3);
ARMARX_INFO << "adding second robot visu";
prxDD->setRobotVisu("TestVisu", "TestRobot2", robFile, project, DrawStyle::FullModel);
ARMARX_INFO << "setting second robot position 6000/3000";
- p(0,3) = 6000.0f;
- p(1,3) = 3000.0f;
+ p(0, 3) = 6000.0f;
+ p(1, 3) = 3000.0f;
PosePtr gp4(new Pose(p));
prxDD->updateRobotPose("TestVisu", "TestRobot2", gp4);
- } catch (...)
+ }
+ catch (...)
{
ARMARX_IMPORTANT << "TEST FAILED";
return;
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 7a0ca2e58fefad205fd3080a07f2c7b6a1cf7305..d3df4657dba7c1276ddde5da3e4fb2d93bb23005 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()
@@ -685,6 +685,39 @@ namespace armarx
layer.mainNode->addChild(sep);
}
+ void DebugDrawerComponent::ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ if (!d.active)
+ {
+ return;
+ }
+
+ // load or get robot
+ RobotPtr rob = requestRobot(d);
+
+ if (!rob)
+ {
+ ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
+ return;
+ }
+
+ std::map < std::string, float >::iterator i = d.configuration.begin();
+ while (i != d.configuration.end())
+ {
+ if (!rob->hasRobotNode(i->first))
+ {
+ ARMARX_WARNING << deactivateSpam() << "Robot " << rob->getName() << " does not know RobotNode " << i->first;
+ i = d.configuration.erase(i);
+ }
+ else
+ {
+ i++;
+ }
+ }
+ }
+
void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -1555,7 +1588,7 @@ namespace armarx
d.layerName = layerName;
d.name = robotName;
- for (auto & it : configuration)
+ for (auto& it : configuration)
{
d.configuration[it.first] = it.second;
}
@@ -1602,13 +1635,26 @@ namespace armarx
void DebugDrawerComponent::clearAll(const Ice::Current&)
{
- for (auto & i : layers)
+ for (auto& i : layers)
{
clearLayer(i.first);
}
}
void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
+ {
+ ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+
+ // 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)
{
ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
@@ -1636,57 +1682,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);
}
@@ -1822,6 +1868,7 @@ namespace armarx
for (auto i = accumulatedUpdateData.robots.begin(); i != accumulatedUpdateData.robots.end(); i++)
{
ARMARX_DEBUG << "update visu / drawRobot for robot " << i->first;
+ ensureExistingRobotNodes(i->second);
drawRobot(i->second);
}
@@ -1842,6 +1889,19 @@ namespace armarx
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();
}
@@ -1851,10 +1911,22 @@ namespace armarx
return layers.find(layerName) != layers.end();
}
- //todo: in some rare cases the mutex3D lock does not work and results in a broken coin timer setup. No updates of the scene will be drawn in this case
- // -> check for a qt-thread solution
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);
+ }
+
+ void DebugDrawerComponent::removeLayerQt(const std::string& layerName)
+ {
+ ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
if (!hasLayer(layerName))
{
if (verbose)
@@ -1870,9 +1942,8 @@ namespace armarx
ARMARX_VERBOSE << "Removing layer " << layerName;
}
- clearLayer(layerName);
+ clearLayerQt(layerName);
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
auto& layer = layers.at(layerName);
layerMainNode->removeChild(layer.mainNode);
layer.mainNode->unref();
@@ -2070,7 +2141,7 @@ namespace armarx
ScopedRecursiveLockPtr l = getScopedVisuLock();
StringSequence seq {};
- for (const auto & layer : layers)
+ for (const auto& layer : layers)
{
seq.push_back(layer.first);
}
@@ -2083,7 +2154,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() +
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 6511755e5cc214ad43815005119aef02c107b140..c231930010733505f544495a5f8e02fd5b372855 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -394,6 +394,9 @@ namespace armarx
std::map<std::string, PolygonData> polygons;
std::map<std::string, ArrowData> arrows;
std::map<std::string, RobotData> robots;
+
+ std::vector< std::string > clearLayers;
+ std::vector< std::string > removeLayers;
};
UpdateData accumulatedUpdateData;
@@ -435,6 +438,13 @@ namespace armarx
void removeArrow(const std::string& layerName, const std::string& name);
void removeRobot(const std::string& layerName, const std::string& name);
+
+ /*!
+ * QT safe version, should not be called from an ICE thread
+ */
+ void clearLayerQt(const std::string& layerName);
+ void removeLayerQt(const std::string& layerName);
+
void setLayerVisibility(const std::string& layerName, bool visible);
/*!
@@ -468,6 +478,12 @@ namespace armarx
Layer& requestLayer(const std::string& layerName);
VirtualRobot::RobotPtr requestRobot(const RobotData& d);
+ /*!
+ * \brief ensureExistingRobotNodes Checks if all robotnodes of configuration exist in robot. If not the specific configuration entry is removed from the list.
+ * \param d
+ */
+ void ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d);
+
std::map < std::string, VirtualRobot::RobotPtr > activeRobots;
SoSeparator* coinVisu;
SoSelection* selectionNode;