diff --git a/etc/doxygen/images/GraphVisualizerGuiPlugin_ConfigDialog.png b/etc/doxygen/images/GraphVisualizerGuiPlugin_ConfigDialog.png
new file mode 100644
index 0000000000000000000000000000000000000000..b00ed68c4b172381e138e94c7a75a86290d59fb6
Binary files /dev/null and b/etc/doxygen/images/GraphVisualizerGuiPlugin_ConfigDialog.png differ
diff --git a/etc/doxygen/images/GraphVisualizerGuiPlugin_Simulation.png b/etc/doxygen/images/GraphVisualizerGuiPlugin_Simulation.png
new file mode 100644
index 0000000000000000000000000000000000000000..ad613cc6cb56f8556b3763ce4a50e80b8c87e32a
Binary files /dev/null and b/etc/doxygen/images/GraphVisualizerGuiPlugin_Simulation.png differ
diff --git a/etc/doxygen/images/GraphVisualizerGuiPlugin_Widget.png b/etc/doxygen/images/GraphVisualizerGuiPlugin_Widget.png
new file mode 100644
index 0000000000000000000000000000000000000000..8cc1f294caa29fed83d6f96ee5d71605991a0ada
Binary files /dev/null and b/etc/doxygen/images/GraphVisualizerGuiPlugin_Widget.png differ
diff --git a/etc/doxygen/images/JoystickControlWidget_widget.png b/etc/doxygen/images/JoystickControlWidget_widget.png
new file mode 100644
index 0000000000000000000000000000000000000000..b00adcb95edbf7ae27be3a00667bd092dfda856f
Binary files /dev/null and b/etc/doxygen/images/JoystickControlWidget_widget.png differ
diff --git a/etc/doxygen/images/PlatformUnitGuiPlugin_widgetpng.png b/etc/doxygen/images/PlatformUnitGuiPlugin_widgetpng.png
new file mode 100644
index 0000000000000000000000000000000000000000..43f6ba7767598bc7fd95f9db3687b909e50ecb22
Binary files /dev/null and b/etc/doxygen/images/PlatformUnitGuiPlugin_widgetpng.png differ
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 3ecd6c2be19575b47bfcff1e21a334d3102d10aa..082ae8928febccce8167fabebe59d25dc3c2d046 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -33,11 +33,15 @@
#include <Inventor/nodes/SoFont.h>
#include <Inventor/nodes/SoText2.h>
#include <Inventor/nodes/SoSphere.h>
+#include <Inventor/nodes/SoComplexity.h>
+#include <Inventor/nodes/SoCoordinate3.h>
+#include <Inventor/nodes/SoPointSet.h>
using namespace VirtualRobot;
namespace armarx{
+static const std::string DEBUG_LAYER_NAME{"debug"};
DebugDrawerComponent::DebugDrawerComponent()
{
@@ -47,12 +51,16 @@ DebugDrawerComponent::DebugDrawerComponent()
coinVisu = new SoSeparator;
coinVisu->ref();
+ layerMainNode = new SoSeparator;
+ layerMainNode->ref();
+
SoUnits *u = new SoUnits();
u->units = SoUnits::MILLIMETERS;
coinVisu->addChild(u);
+ coinVisu->addChild(layerMainNode);
}
-void DebugDrawerComponent::onInitComponent()////////////////////////////////////////////////
+void DebugDrawerComponent::onInitComponent()
{
usingTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
@@ -68,56 +76,47 @@ void DebugDrawerComponent::onConnectComponent()
void DebugDrawerComponent::onExitComponent()
{
ARMARX_DEBUG << "onExitComponent";
- layers.clear();
- coinVisu->unref();
-}
-void DebugDrawerComponent::enableVisu(bool e)///////////////////////remove/////////////////////////
-{
- ScopedRecursiveLockPtr lock = getScopedLock();
- enableLayerVisu("debug",e);
- /* if (e)
+ for(auto& layer:layers)
{
- if (coinVisu->findChild(debugVisu)<0)
- coinVisu->addChild(debugVisu);
- } else
- {
- if (coinVisu->findChild(debugVisu)>=0)
- coinVisu->removeChild(debugVisu);
- }*/
+ removeLayer(layer.first);
+ }
+ coinVisu->removeAllChildren();
+ layerMainNode->unref();
+ coinVisu->unref();
}
-
-
void DebugDrawerComponent::drawCoordSystem(const std::string &layerName, const std::string &name, Eigen::Matrix4f &globalPose, float scale)
{
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawing coord system";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
+
+ removeCoordSystem(layerName, name);
if (scale<=0)
{
- if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
+ /*if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
{
return;
}
layer->mainNode->removeChild(layer->addedCoordVisualizations[name]);
- layer->addedCoordVisualizations.erase(name);
+ layer->addedCoordVisualizations.erase(name);*/
return;
}
- if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
- {
- SoSeparator *newS = new SoSeparator;
- SoMatrixTransform* newM = new SoMatrixTransform;
- newS->addChild(newM);
- std::string n = name;
- newS->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(scale,&n));
- layer->addedCoordVisualizations[name] = newS;
- layer->mainNode->addChild(newS);
- }
- SoSeparator *s = layer->addedCoordVisualizations[name];
+ //if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
+ //{
+ SoSeparator *newS = new SoSeparator;
+ SoMatrixTransform* newM = new SoMatrixTransform;
+ newS->addChild(newM);
+ std::string n = name;
+ newS->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(scale,&n));
+ layer.addedCoordVisualizations[name] = newS;
+ layer.mainNode->addChild(newS);
+ //}
+ SoSeparator *s = layer.addedCoordVisualizations[name];
SoMatrixTransform* m = (SoMatrixTransform*)(s->getChild(0));
SbMatrix mNew = CoinVisualizationFactory::getMatrixTransform(globalPose)->matrix.getValue();
m->matrix.setValue(mNew);
@@ -125,14 +124,12 @@ void DebugDrawerComponent::drawCoordSystem(const std::string &layerName, const s
ARMARX_DEBUG << "end";
}
-
-
void DebugDrawerComponent::drawLine(const std::string &layerName, const std::string &name, Eigen::Vector3f &p1, Eigen::Vector3f &p2, float scale, VirtualRobot::VisualizationFactory::Color &color)
{
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawLine" << flush;
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeLine(layerName,name);
@@ -142,8 +139,8 @@ void DebugDrawerComponent::drawLine(const std::string &layerName, const std::str
Eigen::Matrix4f lp2 = Eigen::Matrix4f::Identity();
lp2(0,3) = p2.x(); lp2(1,3) = p2.y(); lp2(2,3) = p2.z();
newS->addChild(CoinVisualizationFactory::createCoinLine(lp1, lp2, scale, color.r, color.g, color.b));
- layer->addedLineVisualizations[name] = newS;
- layer->mainNode->addChild(newS);
+ layer.addedLineVisualizations[name] = newS;
+ layer.mainNode->addChild(newS);
}
void DebugDrawerComponent::drawBox(const std::string &layerName, const std::string &name, Eigen::Matrix4f &globalPose, float width, float height, float depth, VisualizationFactory::Color &color)
@@ -151,7 +148,7 @@ void DebugDrawerComponent::drawBox(const std::string &layerName, const std::stri
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawBox";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeBox(layerName,name);
@@ -170,8 +167,8 @@ void DebugDrawerComponent::drawBox(const std::string &layerName, const std::stri
cube->depth = depth;
newS->addChild(cube);
- layer->addedBoxVisualizations[name] = newS;
- layer->mainNode->addChild(newS);
+ layer.addedBoxVisualizations[name] = newS;
+ layer.mainNode->addChild(newS);
}
void DebugDrawerComponent::drawText(const std::string &layerName, const std::string &name, const std::string &text, const Eigen::Vector3f &position, const VisualizationFactory::Color &color, int size)
@@ -179,7 +176,7 @@ void DebugDrawerComponent::drawText(const std::string &layerName, const std::str
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawText";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeText(layerName,name);
@@ -207,8 +204,8 @@ void DebugDrawerComponent::drawText(const std::string &layerName, const std::str
te->justification = SoText2::CENTER;
ann->addChild(te);
- layer->addedTextVisualizations[name] = sep;
- layer->mainNode->addChild(sep);
+ layer.addedTextVisualizations[name] = sep;
+ layer.mainNode->addChild(sep);
}
void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::string &name, const Eigen::Vector3f &position, const VisualizationFactory::Color &color, float radius)
@@ -216,7 +213,7 @@ void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::s
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawSphere";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeSphere(layerName,name);
@@ -235,11 +232,41 @@ void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::s
sphere->radius = radius;
sep->addChild(sphere);
- layer->addedSphereVisualizations[name] = sep;
- layer->mainNode->addChild(sep);
+ layer.addedSphereVisualizations[name] = sep;
+ layer.mainNode->addChild(sep);
}
+void DebugDrawerComponent::drawPointCloud(const std::string &layerName, const std::string &name, const DebugDrawerPointCloud &pointCloud)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ ARMARX_DEBUG << "drawPointCloud";
+
+ auto& layer=requestLayer(layerName);
+
+ removePointCloud(layerName,name);
+
+ SoSeparator *sep = new SoSeparator;
+ int sz = pointCloud.size();
+ auto coordinates = new float[sz][3];
+
+ for(int i = 0; i < sz; i++)
+ {
+ DebugDrawerPointCloudElement e = pointCloud[i];
+ coordinates[i][0] = e.x;
+ coordinates[i][1] = e.y;
+ coordinates[i][2] = e.z;
+ }
+
+ SoCoordinate3 *coords = new SoCoordinate3;
+ coords->point.setValues(0, sz, coordinates);
+ sep->addChild(coords);
+
+ SoPointSet *pointSet = new SoPointSet;
+ sep->addChild(pointSet);
+ layer.addedPointCloudVisualizations[name] = sep;
+ layer.mainNode->addChild(sep);
+}
void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string &name)
{
@@ -248,13 +275,13 @@ void DebugDrawerComponent::removeLine(const std::string& layerName, const std::s
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if (layer->addedLineVisualizations.find(name) == layer->addedLineVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if (layer.addedLineVisualizations.find(name) == layer.addedLineVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedLineVisualizations[name]);
- layer->addedLineVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedLineVisualizations[name]);
+ layer.addedLineVisualizations.erase(name);
}
void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string &name)
@@ -264,14 +291,14 @@ void DebugDrawerComponent::removeBox(const std::string& layerName, const std::st
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if(layer->addedBoxVisualizations.find(name) == layer->addedBoxVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if(layer.addedBoxVisualizations.find(name) == layer.addedBoxVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedBoxVisualizations[name]);
- layer->addedBoxVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedBoxVisualizations[name]);
+ layer.addedBoxVisualizations.erase(name);
}
void DebugDrawerComponent::removeText(const std::string &layerName, const std::string &name)
@@ -281,14 +308,14 @@ void DebugDrawerComponent::removeText(const std::string &layerName, const std::s
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if(layer->addedTextVisualizations.find(name) == layer->addedTextVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if(layer.addedTextVisualizations.find(name) == layer.addedTextVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedTextVisualizations[name]);
- layer->addedTextVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedTextVisualizations[name]);
+ layer.addedTextVisualizations.erase(name);
}
void DebugDrawerComponent::removeSphere(const std::string &layerName, const std::string &name)
@@ -298,40 +325,123 @@ void DebugDrawerComponent::removeSphere(const std::string &layerName, const std:
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if(layer->addedSphereVisualizations.find(name) == layer->addedSphereVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if(layer.addedSphereVisualizations.find(name) == layer.addedSphereVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedSphereVisualizations[name]);
- layer->addedSphereVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedSphereVisualizations[name]);
+ layer.addedSphereVisualizations.erase(name);
}
+void DebugDrawerComponent::removePointCloud(const std::string &layerName, const std::string &name)
+{
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ if(layer.addedPointCloudVisualizations.find(name) == layer.addedPointCloudVisualizations.end())
+ {
+ return;
+ }
-void DebugDrawerComponent::setPoseVisu(const std::string &layerName, const std::string &poseName, const PoseBasePtr &globalPose, const Ice::Current &)
+ layer.mainNode->removeChild(layer.addedPointCloudVisualizations[name]);
+ layer.addedPointCloudVisualizations.erase(name);
+}
+
+void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string &name)
+{
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ if(layer.addedCoordVisualizations.find(name) == layer.addedCoordVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedCoordVisualizations[name]);
+ layer.addedCoordVisualizations.erase(name);
+}
+
+void DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
{
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ layer.visible=visible;
+ if (visible)
+ {
+ if (layerMainNode->findChild(layer.mainNode)<0)
+ {
+ layerMainNode->addChild(layer.mainNode);
+ }
+ } else
+ {
+ if (layerMainNode->findChild(layer.mainNode)>=0)
+ {
+ layerMainNode->removeChild(layer.mainNode);
+ }
+ }
+}
+
+void DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ if (coinVisu->findChild(layerMainNode)>=0)
+ {
+ coinVisu->removeChild(layerMainNode);
+ }
+}
+
+void DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ if (coinVisu->findChild(layerMainNode)<0)
+ {
+ coinVisu->addChild(layerMainNode);
+ }
+}
+
+void DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
+{
+
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
- drawCoordSystem(layerName,poseName,gp,1.0f);
+ drawCoordSystem(layerName,poseName,gp,scale);
+}
+
+void DebugDrawerComponent::setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
+{
+ setScaledPoseVisu(DEBUG_LAYER_NAME,poseName,globalPose,scale);
+}
+
+void DebugDrawerComponent::setPoseVisu(const std::string &layerName, const std::string &poseName, const PoseBasePtr &globalPose, const Ice::Current &)
+{
+ setScaledPoseVisu(layerName,poseName,globalPose,1.f);
}
void DebugDrawerComponent::setPoseDebugLayerVisu(const std::string &poseName, const PoseBasePtr &globalPose, const Ice::Current &)
{
- std::string d("debug");
- setPoseVisu(d,poseName,globalPose);
+ setScaledPoseVisu(DEBUG_LAYER_NAME,poseName,globalPose,1.f);
}
void DebugDrawerComponent::removePoseVisu(const std::string &layerName, const std::string &poseName, const Ice::Current &)
{
- Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
- drawCoordSystem(layerName,poseName,gp,0);
+ removeCoordSystem(layerName,poseName);
}
void DebugDrawerComponent::removePoseDebugLayerVisu(const std::string &poseName, const Ice::Current &)
{
- std::string d("debug");
- removePoseVisu(d,poseName);
+ removePoseVisu(DEBUG_LAYER_NAME,poseName);
}
void DebugDrawerComponent::setLineVisu(const std::string &layerName, const std::string &lineName, const Vector3BasePtr &globalPosition1, const Vector3BasePtr &globalPosition2, float lineWidth, const DrawColor &color, const Ice::Current &)
@@ -344,7 +454,7 @@ void DebugDrawerComponent::setLineVisu(const std::string &layerName, const std::
void DebugDrawerComponent::setLineDebugLayerVisu(const std::string &lineName, const Vector3BasePtr &globalPosition1, const Vector3BasePtr &globalPosition2, float lineWidth, const DrawColor &color, const Ice::Current &)
{
- setLineVisu("debug",lineName,globalPosition1,globalPosition2, lineWidth, color);
+ setLineVisu(DEBUG_LAYER_NAME,lineName,globalPosition1,globalPosition2, lineWidth, color);
}
void DebugDrawerComponent::removeLineVisu(const std::string &layerName, const std::string &lineName, const Ice::Current &)
@@ -354,7 +464,7 @@ void DebugDrawerComponent::removeLineVisu(const std::string &layerName, const st
void DebugDrawerComponent::removeLineDebugLayerVisu(const std::string &lineName, const Ice::Current &)
{
- removeLineVisu("debug",lineName);
+ removeLineVisu(DEBUG_LAYER_NAME,lineName);
}
void DebugDrawerComponent::setBoxVisu(const std::string &layerName, const std::string &boxName, const PoseBasePtr &globalPose, const Vector3BasePtr &dimensions, const DrawColor &color, const Ice::Current &)
@@ -367,7 +477,7 @@ void DebugDrawerComponent::setBoxVisu(const std::string &layerName, const std::s
void DebugDrawerComponent::setBoxDebugLayerVisu(const std::string &boxName, const PoseBasePtr &globalPose, const Vector3BasePtr &dimensions, const DrawColor &color, const Ice::Current &)
{
- setBoxVisu("debug", boxName, globalPose, dimensions, color);
+ setBoxVisu(DEBUG_LAYER_NAME, boxName, globalPose, dimensions, color);
}
void DebugDrawerComponent::removeBoxVisu(const std::string &layerName, const std::string &boxName, const Ice::Current &)
@@ -377,7 +487,7 @@ void DebugDrawerComponent::removeBoxVisu(const std::string &layerName, const std
void DebugDrawerComponent::removeBoxDebugLayerVisu(const std::string &boxName, const Ice::Current &)
{
- removeBoxVisu("debug", boxName);
+ removeBoxVisu(DEBUG_LAYER_NAME, boxName);
}
void DebugDrawerComponent::setTextVisu(const std::string &layerName, const std::string &textName, const std::string &text, const Vector3BasePtr &globalPosition, const DrawColor &color, int size, const Ice::Current &)
@@ -389,7 +499,7 @@ void DebugDrawerComponent::setTextVisu(const std::string &layerName, const std::
void DebugDrawerComponent::setTextDebugLayerVisu(const std::string &textName, const std::string &text, const Vector3BasePtr &globalPosition, const DrawColor &color, int size, const Ice::Current &)
{
- setTextVisu("debug", textName, text, globalPosition, color, size);
+ setTextVisu(DEBUG_LAYER_NAME, textName, text, globalPosition, color, size);
}
void DebugDrawerComponent::removeTextVisu(const std::string &layerName, const std::string &textName, const Ice::Current &)
@@ -399,7 +509,7 @@ void DebugDrawerComponent::removeTextVisu(const std::string &layerName, const st
void DebugDrawerComponent::removeTextDebugLayerVisu(const std::string &textName, const Ice::Current &)
{
- removeTextVisu("debug", textName);
+ removeTextVisu(DEBUG_LAYER_NAME, textName);
}
void DebugDrawerComponent::setSphereVisu(const std::string &layerName, const std::string &sphereName, const Vector3BasePtr &globalPosition, const DrawColor &color, float radius, const Ice::Current &)
@@ -411,7 +521,7 @@ void DebugDrawerComponent::setSphereVisu(const std::string &layerName, const std
void DebugDrawerComponent::setSphereDebugLayerVisu(const std::string &sphereName, const Vector3BasePtr &globalPosition, const DrawColor &color, float radius, const Ice::Current &)
{
- setSphereVisu("debug", sphereName, globalPosition, color, radius);
+ setSphereVisu(DEBUG_LAYER_NAME, sphereName, globalPosition, color, radius);
}
void DebugDrawerComponent::removeSphereVisu(const std::string &layerName, const std::string &sphereName, const Ice::Current &)
@@ -421,10 +531,30 @@ void DebugDrawerComponent::removeSphereVisu(const std::string &layerName, const
void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string &sphereName, const Ice::Current &)
{
- removeSphereVisu("debug", sphereName);
+ removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
}
-void DebugDrawerComponent::clear(const std::string &layerName, const Ice::Current &)
+void DebugDrawerComponent::setPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const Ice::Current &)
+{
+ drawPointCloud(layerName, pointCloudName, pointCloud);
+}
+
+void DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const Ice::Current &)
+{
+ setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
+}
+
+void DebugDrawerComponent::removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const Ice::Current &)
+{
+ removePointCloud(layerName, pointCloudName);
+}
+
+void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string &pointCloudName, const Ice::Current &)
+{
+ removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
+}
+
+void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::Current &)
{
if(!hasLayer(layerName))
{
@@ -432,14 +562,21 @@ void DebugDrawerComponent::clear(const std::string &layerName, const Ice::Curren
return;
}
ARMARX_VERBOSE << "Clearing layer " << layerName;
- layers.at(layerName)->clear();
+
+ ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ layer.addedCoordVisualizations.clear();
+ layer.addedLineVisualizations.clear();
+ layer.addedBoxVisualizations.clear();
+ layer.addedTextVisualizations.clear();
+ layer.addedSphereVisualizations.clear();
+ layer.addedPointCloudVisualizations.clear();
+ layer.mainNode->removeAllChildren();
}
void DebugDrawerComponent::clearDebugLayer(const Ice::Current &)
{
- ARMARX_VERBOSE << "Clearing debug layer...";
-
- clear("debug");
+ clearLayer(DEBUG_LAYER_NAME);
}
void DebugDrawerComponent::setMutex(boost::shared_ptr<boost::recursive_mutex> m)
@@ -460,7 +597,7 @@ SoSeparator* DebugDrawerComponent::getVisualization()
return coinVisu;
}
-std::shared_ptr<DebugDrawerComponent::Layer> DebugDrawerComponent::requestLayer(const std::string& layerName)
+DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
{
ScopedRecursiveLockPtr l = getScopedLock();
if(hasLayer(layerName))
@@ -468,67 +605,19 @@ std::shared_ptr<DebugDrawerComponent::Layer> DebugDrawerComponent::requestLayer(
return layers.at(layerName);
}
ARMARX_VERBOSE << "Created layer "<<layerName;
- layers[layerName]=std::shared_ptr<DebugDrawerComponent::Layer>{new Layer(*this)};
- return layers.at(layerName);
-}
-DebugDrawerComponent::Layer::Layer(DebugDrawerComponent& parentDrawer):
- parent(parentDrawer),
- mainNode{new SoSeparator{}},
- addedCoordVisualizations{},
- addedLineVisualizations{},
- addedBoxVisualizations{},
- addedTextVisualizations{},
- addedSphereVisualizations{},
- visible{true}
-{
+ SoSeparator* mainNode=new SoSeparator{};
mainNode->ref();
- ScopedRecursiveLockPtr l = parent.getScopedLock();
- parent.coinVisu->addChild(mainNode);
-}
-
-DebugDrawerComponent::Layer::Layer(Layer && other):
- parent(other.parent),
- mainNode{other.mainNode},
- addedCoordVisualizations{std::move(other.addedCoordVisualizations)},
- addedLineVisualizations{std::move(other.addedLineVisualizations)},
- addedBoxVisualizations{std::move(other.addedBoxVisualizations)},
- addedTextVisualizations{std::move(other.addedTextVisualizations)},
- addedSphereVisualizations{std::move(other.addedSphereVisualizations)},
- visible{other.visible}
-{
- other.mainNode=nullptr;
-}
-
-DebugDrawerComponent::Layer::~Layer()
-{
- if(mainNode)
- {
- ScopedRecursiveLockPtr l = parent.getScopedLock();
- if (parent.coinVisu->findChild(mainNode)>=0)
- {
- parent.coinVisu->removeChild(mainNode);
- }
- clear();
- mainNode->unref();
- }
-}
-
-void DebugDrawerComponent::Layer::clear()
-{
- ScopedRecursiveLockPtr l = parent.getScopedLock();
- addedCoordVisualizations.clear();
- addedLineVisualizations.clear();
- addedBoxVisualizations.clear();
- addedTextVisualizations.clear();
- addedSphereVisualizations.clear();
- mainNode->removeAllChildren();
+ layerMainNode->addChild(mainNode);
+ layers[layerName]=Layer();
+ layers.at(layerName).mainNode=mainNode;
+ layers.at(layerName).visible=true;
+ return layers.at(layerName);
}
-bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)/////////////////////////////////////
+bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
{
ScopedRecursiveLockPtr l = getScopedLock();
- layers.find(std::string{layerName});
return layers.find(layerName)!=layers.end();
}
@@ -536,34 +625,27 @@ void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice
{
if(!hasLayer(layerName))
{
+ ARMARX_VERBOSE << "Layer " << layerName<<" can't be removed, because it does not exist.";
return;
}
+ ARMARX_VERBOSE << "Removing layer " << layerName;
+
ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ layerMainNode->removeChild(layer.mainNode);
+ clearLayer(layerName);
+ layer.mainNode->unref();
layers.erase(layerName);
}
void DebugDrawerComponent::enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current&)
{
- if(!hasLayer(layerName))
- {
- return;
- }
- ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- layer->visible=visible;
- if (visible)
- {
- if (coinVisu->findChild(layer->mainNode)<0)
- {
- coinVisu->addChild(layer->mainNode);
- }
- } else
- {
- if (coinVisu->findChild(layer->mainNode)>=0)
- {
- coinVisu->removeChild(layer->mainNode);
- }
- }
+ setLayerVisibility(layerName,visible);
+}
+
+void DebugDrawerComponent::enableDebugLayerVisu(bool visible, const ::Ice::Current&)
+{
+ enableLayerVisu(DEBUG_LAYER_NAME,visible);
}
StringSequence DebugDrawerComponent::layerNames(const ::Ice::Current&)
@@ -577,22 +659,21 @@ StringSequence DebugDrawerComponent::layerNames(const ::Ice::Current&)
return seq;
}
-
::armarx::LayerInformationSequence DebugDrawerComponent::layerInformation(const ::Ice::Current&)
{
::armarx::LayerInformationSequence seq{};
ScopedRecursiveLockPtr l = getScopedLock();
for(const auto& layer:layers)
{
- int count = layer.second->addedCoordVisualizations.size()+
- layer.second->addedLineVisualizations.size()+
- layer.second->addedBoxVisualizations.size()+
- layer.second->addedTextVisualizations.size()+
- layer.second->addedSphereVisualizations.size();
- ::armarx::LayerInformation info={layer.first,layer.second->visible,count};
+ int count = layer.second.addedCoordVisualizations.size()+
+ layer.second.addedLineVisualizations.size()+
+ layer.second.addedBoxVisualizations.size()+
+ layer.second.addedTextVisualizations.size()+
+ layer.second.addedSphereVisualizations.size()+
+ layer.second.addedPointCloudVisualizations.size();
+ ::armarx::LayerInformation info={layer.first,layer.second.visible,count};
seq.push_back(info);
}
return seq;
}
-
}//namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index dbd61808a8581df0b0b02c6edd9b46b3ca834583..495eacabc937451201cd64e12a18c16fbc257dfe 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -45,9 +45,9 @@
namespace armarx{
-/**
- * @class ArmarXPhysicsWorldPropertyDefinitions
- * @brief
+/*!
+ * \class ArmarXPhysicsWorldPropertyDefinitions
+ * \brief
*/
class DebugDrawerPropertyDefinitions:
public ComponentPropertyDefinitions
@@ -78,8 +78,8 @@ public:
- /**
- * @see PropertyUser::createPropertyDefinitions()
+ /*!
+ * \see PropertyUser::createPropertyDefinitions()
*/
virtual PropertyDefinitionsPtr createPropertyDefinitions()
{
@@ -87,15 +87,11 @@ public:
getConfigIdentifier()));
}
- /*!
- * \brief Enable or disable debug layer visu
- * \param e
- */
- void enableVisu(bool e);
-
/* Inherited from DebugDrawerInterface. */
virtual void setPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current());
+ virtual void setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current());
virtual void setPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current());
+ virtual void setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current());
virtual void removePoseVisu(const std::string& layerName, const std::string& poseName, const ::Ice::Current& = ::Ice::Current());
virtual void removePoseDebugLayerVisu(const std::string& poseName, const ::Ice::Current& = ::Ice::Current());
@@ -119,18 +115,26 @@ public:
virtual void removeSphereVisu(const std::string &layerName, const std::string &sphereName, const ::Ice::Current& = ::Ice::Current());
virtual void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
-
- virtual void clear(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+ virtual void clearLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
virtual void clearDebugLayer(const ::Ice::Current& = ::Ice::Current());
-
virtual bool hasLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
virtual void removeLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+
virtual void enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current& = ::Ice::Current());
+ virtual void enableDebugLayerVisu(bool visible, const ::Ice::Current& = ::Ice::Current());
+
virtual ::armarx::StringSequence layerNames(const ::Ice::Current& = ::Ice::Current());
virtual ::armarx::LayerInformationSequence layerInformation(const ::Ice::Current& = ::Ice::Current());
+ virtual void disableAllLayers(const ::Ice::Current& = ::Ice::Current());
+ virtual void enableAllLayers(const ::Ice::Current& = ::Ice::Current());
+
/*!
* \brief getScopedLock If using the coin visualization it must be ensured that all rendering calls are protected with this mutex
* \return The lock that is automatically destructed when leaving the current scope.
@@ -152,32 +156,29 @@ protected:
void drawBox(const std::string& layerName, const std::string &name, Eigen::Matrix4f &globalPose, float width, float height, float depth, VirtualRobot::VisualizationFactory::Color &color);
void drawText(const std::string& layerName, const std::string &name, const std::string &text, const Eigen::Vector3f &position, const VirtualRobot::VisualizationFactory::Color &color, int size);
void drawSphere(const std::string& layerName, const std::string &name, const Eigen::Vector3f &position, const VirtualRobot::VisualizationFactory::Color &color, float radius);
-
+ void drawPointCloud(const std::string& layerName, const std::string &name, const DebugDrawerPointCloud &pointCloud);
+
+ void removeCoordSystem(const std::string& layerName, const std::string &name);
void removeLine(const std::string& layerName, const std::string &name);
void removeBox(const std::string& layerName, const std::string &name);
void removeText(const std::string& layerName, const std::string &name);
void removeSphere(const std::string& layerName, const std::string &name);
+ void removePointCloud(const std::string& layerName, const std::string &name);
+
+ void setLayerVisibility(const std::string& layerName, bool visible);
+ /*!
+ * \brief Contains data for a layer.
+ */
struct Layer
{
- Layer(DebugDrawerComponent& parentDrawer);
- Layer(Layer & other) = delete;
- Layer& operator=(const Layer&) = delete;
- Layer(Layer && other);
- Layer & operator= ( Layer && other)=delete;
- ~Layer();
-
- void clear();
-
- DebugDrawerComponent& parent;
-
SoSeparator* mainNode;
-
std::map<std::string, SoSeparator*> addedCoordVisualizations;
std::map<std::string, SoSeparator*> addedLineVisualizations;
std::map<std::string, SoSeparator*> addedBoxVisualizations;
std::map<std::string, SoSeparator*> addedTextVisualizations;
std::map<std::string, SoSeparator*> addedSphereVisualizations;
+ std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
bool visible;
};
@@ -187,14 +188,19 @@ protected:
* \param layerName The layer.
* \return The requested layer.
*/
- std::shared_ptr<Layer> requestLayer(const std::string& layerName);
+ Layer &requestLayer(const std::string& layerName);
SoSeparator* coinVisu;
- //SoSeparator* debugVisu;
- bool enable;
+ /*!
+ * \brief Main node for all layers
+ */
+ SoSeparator* layerMainNode;
- std::map<const std::string, std::shared_ptr<Layer>> layers;
+ /*!
+ * \brief All existing layers.
+ */
+ std::map<const std::string, Layer> layers;
boost::shared_ptr<boost::recursive_mutex> mutex;
};
diff --git a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerConfigDialog.ui b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerConfigDialog.ui
index 3adc87c8b47c22bb4e2f2b2cc11cabbb5a506932..86617a6529e9e660aeeaf82100232c4eb2b11c67 100644
--- a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerConfigDialog.ui
+++ b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerConfigDialog.ui
@@ -7,28 +7,24 @@
<x>0</x>
<y>0</y>
<width>651</width>
- <height>80</height>
+ <height>105</height>
</rect>
</property>
<property name="windowTitle">
<string>Dialog</string>
</property>
- <layout class="QVBoxLayout" name="verticalLayout">
- <item>
- <layout class="QHBoxLayout" name="horizontalLayout">
- <item>
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Debug drawer</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLineEdit" name="editDebugDrawerProxyName"/>
- </item>
- </layout>
+ <layout class="QFormLayout" name="formLayout">
+ <property name="fieldGrowthPolicy">
+ <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+ </property>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Debug drawer</string>
+ </property>
+ </widget>
</item>
- <item>
+ <item row="6" column="0">
<widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -38,6 +34,19 @@
</property>
</widget>
</item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="editDebugDrawerProxyName"/>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="editDebugDrawerLayerName"/>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Layer name</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
<resources/>
diff --git a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp
index 78d886b2151afe768bb3b2416b83d45e4a0c4c0d..a802584c83c0eec7cb59f664304ab3d18ff6fb15 100644
--- a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp
+++ b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.cpp
@@ -13,9 +13,6 @@
#include <QLineEdit>
#include <QHBoxLayout>
-#include <QGraphicsLineItem>
-#include <QGraphicsEllipseItem>
-
//std
#include <memory>
#include <sstream>
@@ -38,6 +35,7 @@ static const ::Ice::Float LINE_WIDTH_DEFAULT =5;
* @brief The width of selected lines drawn onto the debug layer and scene.
*/
static const ::Ice::Float LINE_WIDTH_SELECTED=10;
+
/**
* @brief The default color of lines drawn onto the debug layer and scene.
*/
@@ -72,21 +70,12 @@ static const float SCENE_LINE_SCALE_FACTOR=SCENE_SCALE_FACTOR;
*/
static const float VIEW_ROTATE_STEP_SIZE_CC=45;
-/**
- * @brief The type of node ids. (This type implies the node exists)
- */
-typedef const std::string NodeId;
-/**
- * @brief The type of edge ids. (This type implies the edge exists)
- */
-typedef const std::pair<const std::string,const std::string> EdgeId;
-
/**
* @brief Returns the name used on the debug layer.
* @param edge The edge.
* @return The name used on the debug layer.
*/
-inline std::string iceName(const EdgeId& edge)
+inline std::string iceName(const GraphVisualizerWidget::EdgeId& edge)
{
std::stringstream s;
s<<"edge_"<<edge.first<<"_"<<edge.second;
@@ -98,7 +87,7 @@ inline std::string iceName(const EdgeId& edge)
* @param nodeName The node.
* @return The name used on the debug layer.
*/
-inline std::string iceName(const NodeId& nodeName)
+inline std::string iceName(const GraphVisualizerWidget::NodeId& nodeName)
{
return nodeName;
}
@@ -114,7 +103,7 @@ GraphVisualizerGuiPlugin::GraphVisualizerGuiPlugin()
GraphVisualizerWidget::GraphVisualizerWidget() :
debugDrawerTopicName{"DebugDrawerUpdates"},
viewAngle{0},
- layerName{"DebugDrawerUpdates_Graph"}
+ debugDrawerLayerName{"DebugDrawerUpdates_Graph"}
{
// init gui
ui.setupUi(getWidget());
@@ -135,7 +124,7 @@ void GraphVisualizerWidget::onInitComponent()
void GraphVisualizerWidget::onConnectComponent()
{
debugDrawer = getTopic<DebugDrawerInterfacePrx>(debugDrawerTopicName);
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////todo remove test function
+ //todo remove test function
QObject::connect(ui.b,SIGNAL(clicked ()),this,SLOT(draw()));
//tables
@@ -163,25 +152,27 @@ QPointer<QDialog> GraphVisualizerWidget::getConfigDialog(QWidget* parent)
dialog = new GraphVisualizerConfigDialog(parent);
}
dialog->ui->editDebugDrawerProxyName->setText(QString::fromStdString(debugDrawerTopicName));
+ dialog->ui->editDebugDrawerLayerName->setText(QString::fromStdString(debugDrawerLayerName));
return qobject_cast<GraphVisualizerConfigDialog*>(dialog);
}
void GraphVisualizerWidget::configured()
{
debugDrawerTopicName = dialog->ui->editDebugDrawerProxyName->text().toStdString();
- layerName = debugDrawerTopicName+std::string{"_Graph"};
+ debugDrawerLayerName = dialog->ui->editDebugDrawerLayerName->text().toStdString();
}
void GraphVisualizerWidget::loadSettings(QSettings *settings)
{
debugDrawerTopicName = settings->value("debugDrawerTopicName", QString::fromStdString(debugDrawerTopicName)).toString().toStdString();
- layerName = debugDrawerTopicName+std::string{"_Graph"};
+ debugDrawerLayerName = settings->value("debugDrawerLayerName", QString::fromStdString(debugDrawerLayerName)).toString().toStdString();
}
void GraphVisualizerWidget::saveSettings(QSettings *settings)
{
settings->setValue("debugDrawerTopicName", QString::fromStdString(debugDrawerTopicName));
+ settings->setValue("debugDrawerLayerName", QString::fromStdString(debugDrawerLayerName));
}
@@ -200,7 +191,7 @@ void GraphVisualizerWidget::addEdge(const std::string& node1, const std::string&
if(hasEdge(node1,node2))
{
//nothing needs to be updated
- ARMARX_WARNING<<"Edge: "<<node1<<", "<<node2<<" already existed.";
+ ARMARX_VERBOSE<<"Edge: "<<node1<<", "<<node2<<" already existed.";
return;
}
auto edgeId =toEdge(node1,node2);
@@ -215,9 +206,16 @@ void GraphVisualizerWidget::addEdge(const std::string& node1, const std::string&
ui.tableWidgetEdges->setItem(row,1,new QTableWidgetItem{QString::fromStdString(node2)});
//debug layer will be done later
//scene
- auto graphicsItem= scene->addLine(node1dat.pos->x,node1dat.pos->y,
- node2dat.pos->x,node2dat.pos->y);
- graphicsItem->setToolTip(QString{"Edge:"}+QString::fromStdString(node1)+QChar{2194}+QString::fromStdString(node2));
+ QGraphicsLineItem* graphicsItem= dynamic_cast<QGraphicsLineItem*>(new GraphVisualizerGraphicsLineItem{
+ *this, edgeId,
+ node1dat.pos->x,-node1dat.pos->y,
+ node2dat.pos->x,-node2dat.pos->y
+ });
+ //auto graphicsItem= scene->addLine(node1dat.pos->x,-node1dat.pos->y,
+ // node2dat.pos->x,-node2dat.pos->y);
+ scene->addItem(graphicsItem);
+ //setToolTip on graphicsItem does not work
+ dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString{"Edge:"}+QString::fromStdString(node1)+QString{" <-> "}+QString::fromStdString(node2));
//data
EdgeData data{graphicsItem,row,false,false};
edges[edgeId]=data;
@@ -230,7 +228,7 @@ void GraphVisualizerWidget::addNode(const ::armarx::FramedVector3BasePtr& node,
if(hasNode(node->frame))
{
NodeData& oldNode=nodes.at(node->frame);
- ARMARX_WARNING<<"Node: "<<node->frame<<"was overwritten! Old: "
+ ARMARX_WARNING<<"Node: "<<node->frame<<" was overwritten! Old: "
<<oldNode.pos->x<<", " <<oldNode.pos->y<<", " <<oldNode.pos->z<<"| New: "
<<node->x<<", " <<node->y<<", " <<node->z
<<" [file: "<<__FILE__<<" | line: "<<__LINE__<<" | function: "<<__PRETTY_FUNCTION__<<"]";
@@ -239,12 +237,17 @@ void GraphVisualizerWidget::addNode(const ::armarx::FramedVector3BasePtr& node,
ui.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex,1,new QTableWidgetItem{QString::number(node->x)});
ui.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex,2,new QTableWidgetItem{QString::number(node->y)});
ui.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex,3,new QTableWidgetItem{QString::number(node->z)});
- //scene
- oldNode.graphicsItem->setPos(node->x,node->y);
//data
::armarx::Vector3Ptr pos{new ::armarx::Vector3{Eigen::Vector3f{node->x,node->y,node->z}}};
-
oldNode.pos=pos;
+ //update connected edges
+ for(const auto& edge:edges)
+ {
+ if((edge.first.first==node->frame) || (edge.first.second==node->frame))
+ {
+ updateEdge(edge.first);
+ }
+ }
}
else
{
@@ -257,13 +260,19 @@ void GraphVisualizerWidget::addNode(const ::armarx::FramedVector3BasePtr& node,
ui.tableWidgetNodes->setItem(row,2,new QTableWidgetItem{QString::number(node->y)});
ui.tableWidgetNodes->setItem(row,3,new QTableWidgetItem{QString::number(node->z)});
//scene
- auto graphicsItem= scene->addEllipse(node->x,node->y,0,0);
+ QGraphicsEllipseItem* graphicsItem= dynamic_cast<QGraphicsEllipseItem*>(new GraphVisualizerGraphicsEllipseItem{
+ *this, node->frame,
+ node->x,-node->y,0,0
+ });
+ //auto graphicsItem= scene->addEllipse(node->x,-node->y,0,0);
+ scene->addItem(graphicsItem);
+ //setToolTip on graphicsItem does not work
graphicsItem->setZValue(std::numeric_limits<qreal>::max());
- graphicsItem->setToolTip(QString{"Node:"}+QString::fromStdString(node->frame));
+ dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString{"Node:"}+QString::fromStdString(node->frame));
//data
::armarx::Vector3Ptr pos{new ::armarx::Vector3{Eigen::Vector3f{node->x,node->y,node->z}}};
- NodeData data{pos,nullptr, graphicsItem, row,false,false};
+ NodeData data{pos, nullptr, graphicsItem, row,false,false};
nodes[node->frame]=data;
}
updateNode(node->frame);
@@ -277,7 +286,7 @@ void GraphVisualizerWidget::clearEdges(const Ice::Current&)
scene->removeItem(edge.second.graphicsItem);
delete edge.second.graphicsItem;
//remove from debug layer
- debugDrawer->removePoseDebugLayerVisu(iceName(edge.first));
+ debugDrawer->removePoseVisu(debugDrawerLayerName,iceName(edge.first));
}
//clear table widget
ui.tableWidgetEdges->clearContents();
@@ -291,11 +300,11 @@ void GraphVisualizerWidget::clearGraph(const Ice::Current&)
//remove from debug layer
for(auto& edge:edges)
{
- debugDrawer->removeLineDebugLayerVisu(iceName(edge.first));
+ debugDrawer->removeLineVisu(debugDrawerLayerName,iceName(edge.first));
}
for(auto& node:nodes)
{
- debugDrawer->removePoseDebugLayerVisu(iceName(node.first));
+ debugDrawer->removePoseVisu(debugDrawerLayerName,iceName(node.first));
}
//clear scene
scene->clear();
@@ -342,7 +351,7 @@ void GraphVisualizerWidget::updateEdge(const EdgeId& id)
auto lineWidth=(data.selected)?LINE_WIDTH_SELECTED:LINE_WIDTH_DEFAULT;
//debug layer
- debugDrawer->setLineVisu(layerName,
+ debugDrawer->setLineVisu(debugDrawerLayerName,
iceName(id),
nodes.at(id.first).pos,
nodes.at(id.second).pos,
@@ -375,14 +384,14 @@ void GraphVisualizerWidget::updateNode(const NodeId& id)
//debug layer
const Eigen::Vector3f eigPos{data.pos->x,data.pos->y,data.pos->z};
- data.pose = ::armarx::PoseBasePtr{new ::armarx::Pose{Eigen::Matrix3f::Identity()*lineWidth,eigPos}};
- debugDrawer->setPoseVisu(layerName,iceName(id),data.pose);
+ data.pose = ::armarx::PoseBasePtr{new ::armarx::Pose{Eigen::Matrix3f::Identity(),eigPos}};
+ debugDrawer->setScaledPoseVisu(debugDrawerLayerName,iceName(id),data.pose,lineWidth/2);
//scene
data.graphicsItem->setPen(QPen{qColor});
data.graphicsItem->setBrush(QBrush{qColor});
data.graphicsItem->setRect(data.pos->x-lineWidth*SCENE_NODES_SCALE_FACTOR/2,
- data.pos->y-lineWidth*SCENE_NODES_SCALE_FACTOR/2,
+ -data.pos->y-lineWidth*SCENE_NODES_SCALE_FACTOR/2,
lineWidth*SCENE_NODES_SCALE_FACTOR,
lineWidth*SCENE_NODES_SCALE_FACTOR);
//table
@@ -430,32 +439,27 @@ void GraphVisualizerWidget::highlightNode(const std::string& name, bool highligh
void GraphVisualizerWidget::nodeTableDoubleClicked(int row, int)
{
- //search item and update it
- for(auto& node:nodes)
- {
- if(node.second.tableWidgetNodesIndex==row)
- {
- //flip selected bool
- node.second.selected^=true;
- //update
- updateNode(node.first);
- }
- }
+ nodeDoubleClicked(ui.tableWidgetNodes->item(row,0)->text().toStdString());
}
void GraphVisualizerWidget::edgeTableDoubleClicked(int row, int)
{
- //search item and update it
- for(auto& edge:edges)
- {
- if(edge.second.tableWidgetEdgesIndex==row)
- {
- //flip selected bool
- edge.second.selected^=true;
- //update
- updateEdge(edge.first);
- }
- }
+ edgeDoubleClicked(toEdge(
+ ui.tableWidgetEdges->item(row,0)->text().toStdString(),
+ ui.tableWidgetEdges->item(row,1)->text().toStdString()
+ ));
+}
+
+void GraphVisualizerWidget::nodeDoubleClicked(NodeId id)
+{
+ nodes.at(id).selected^=true;
+ updateNode(id);
+}
+
+void GraphVisualizerWidget::edgeDoubleClicked(EdgeId id)
+{
+ edges.at(id).selected^=true;
+ updateEdge(id);
}
void GraphVisualizerWidget::redraw(const Ice::Current&)
@@ -466,7 +470,7 @@ void GraphVisualizerWidget::redraw(const Ice::Current&)
auto lineWidth=(edge.second.selected )?LINE_WIDTH_SELECTED:LINE_WIDTH_DEFAULT;
//debug layer
- debugDrawer->setLineVisu(layerName,
+ debugDrawer->setLineVisu(debugDrawerLayerName,
iceName(edge.first),
nodes.at(edge.first.first).pos,
nodes.at(edge.first.second).pos,
@@ -475,7 +479,7 @@ void GraphVisualizerWidget::redraw(const Ice::Current&)
}
for(const auto& node:nodes)
{
- debugDrawer->setPoseVisu(layerName,iceName(node.first),node.second.pose);
+ debugDrawer->setPoseVisu(debugDrawerLayerName,iceName(node.first),node.second.pose);
}
}
@@ -540,19 +544,20 @@ void GraphVisualizerWidget::draw()
{
clearGraph();
- static ::armarx::FramedVector3Ptr tableS {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,7300.f,1000.f}, "table" }};
- static ::armarx::FramedVector3Ptr fridge {new ::armarx::FramedVector3{Eigen::Vector3f{2150.f,7750.f,1000.f}, "fridge"}};
- static ::armarx::FramedVector3Ptr sink {new ::armarx::FramedVector3{Eigen::Vector3f{2500.f,9700.f,1000.f}, "sink" }};
- static ::armarx::FramedVector3Ptr hub2 {new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,1000.f}, "hub2" }};
- static ::armarx::FramedVector3Ptr hub1 {new ::armarx::FramedVector3{Eigen::Vector3f{2900.f,8000.f,1000.f}, "hub1" }};
- static ::armarx::FramedVector3Ptr hub3 {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,2200.f,1000.f}, "hub3" }};
- static ::armarx::FramedVector3Ptr hub4 {new ::armarx::FramedVector3{Eigen::Vector3f{1900.f,3000.f,1000.f}, "hub4" }};
- static ::armarx::FramedVector3Ptr counter {new ::armarx::FramedVector3{Eigen::Vector3f{1890.f,4050.f,1000.f}, "counter"}};
+ static ::armarx::FramedVector3Ptr tableS {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,7300.f,1000.f}, "table" }};
+ static ::armarx::FramedVector3Ptr fridge {new ::armarx::FramedVector3{Eigen::Vector3f{2150.f,7750.f,1000.f}, "fridge" }};
+ static ::armarx::FramedVector3Ptr sink {new ::armarx::FramedVector3{Eigen::Vector3f{2500.f,9700.f,1000.f}, "sink" }};
+ static ::armarx::FramedVector3Ptr hub2fst{new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,5000.f}, "hub2" }};
+ static ::armarx::FramedVector3Ptr hub2snd{new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,1000.f}, "hub2" }};
+ static ::armarx::FramedVector3Ptr hub1 {new ::armarx::FramedVector3{Eigen::Vector3f{2900.f,8000.f,1000.f}, "hub1" }};
+ static ::armarx::FramedVector3Ptr hub3 {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,2200.f,1000.f}, "hub3" }};
+ static ::armarx::FramedVector3Ptr hub4 {new ::armarx::FramedVector3{Eigen::Vector3f{1900.f,3000.f,1000.f}, "hub4" }};
+ static ::armarx::FramedVector3Ptr counter{new ::armarx::FramedVector3{Eigen::Vector3f{1890.f,4050.f,1000.f}, "counter"}};
addNode(tableS);
addNode(fridge);
addNode(sink);
- addNode(hub2);
+ addNode(hub2fst);
addNode(hub1);
addNode(hub3);
addNode(hub4);
@@ -571,6 +576,7 @@ void GraphVisualizerWidget::draw()
resetHilight();
highlightEdge("hub2","hub3");
+
highlightEdge("hub3","hub4");
highlightEdge("hub3","hub4",false);
@@ -578,6 +584,8 @@ void GraphVisualizerWidget::draw()
highlightNode("counter");
highlightNode("counter",false);
+ addNode(hub2snd);
+ addEdge("hub2","hub3");
/*
3400,7300,1000,"table"
2150,7750,1000,"fridge"
@@ -586,7 +594,7 @@ void GraphVisualizerWidget::draw()
2900,8000,1000,"hub1"
3400,2200,1000,"hub3"
1900,3000,1000,"hub4"
- 1890,4050,1000,"counter"
+ 189graphicsItem0,4050,1000,"counter"
"hub1","hub2"
diff --git a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.h b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.h
index e55e5a9079b7d123ee222f4bcd1401214d7e7075..515b7794ae7602eeb4f7f6525d0aef42eb07c6eb 100644
--- a/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.h
+++ b/source/RobotAPI/gui_plugins/GraphVisualizerPlugin/GraphVisualizerGuiPlugin.h
@@ -40,6 +40,8 @@
#include <QtGui/QMainWindow>
#include <QDialog>
#include <QGraphicsScene>
+#include <QGraphicsLineItem>
+#include <QGraphicsEllipseItem>
//std
#include <string>
@@ -55,8 +57,130 @@
namespace armarx
{
+ class GraphVisualizerGraphicsEllipseItem;
+ class GraphVisualizerGraphicsLineItem;
+
class GraphVisualizerConfigDialog;
+ /**
+ * @brief A plugin used to visualize a undirected graph and draw it to a debug layer.
+ * @ingroup ArmarXGuiPlugins
+ * @see GraphVisualizerWidget
+ *
+ * This plugin implements the ice interface GraphVisualizerInterface and therefore
+ * can be provided with a undirected graph.
+ *
+ * The graph is drawn to a debug layer and a scene located in the plugin.
+ * The plugin has tables containing information about the nodes and edges.
+ * Nodes on the debug layer are visualized as coordinate systems.
+ * Nodes on the scene are visualized as circles.
+ * Edges on debug layer and scene are visualized with lines.
+ *
+ * Nodes can be added with and
+ * @code{.cpp}
+ * addNode(const ::armarx::FramedVector3BasePtr& p)
+ * @endcode
+ * are identified with the string stored in p.
+ *
+ * Edges can be added with
+ * @code{.cpp}
+ * addEdge(const ::std::string& fst, const ::std::string& snd)
+ * @endcode
+ * and are identified with {fst,snd}.
+ * fst and snd have to be the names of already existing nodes.
+ * The order of fst and snd does not matter.
+ *
+ * The existence can be checked with following methodes:
+ * @code{.cpp}
+ * hasNode(const ::std::string& name)
+ * hasEdge(const ::std::string& fst, const ::std::string& snd)
+ * @endcode
+ *
+ * All edges (the whole graph) can be deleted with:
+ * @code{.cpp}
+ * clearEdges()
+ * clearGraph()
+ * @endcode
+ *
+ * If the graph has to be redrawn to the debug layer use
+ * @code{.cpp}
+ * redraw()
+ * @endcode
+ *
+ * Edges and nodes have four states: {selected, not selected}X{highlighted, not highlighted}
+ *
+ * Selection affects the width of lines and the size of nodes. Selected lines are thicker and nodes have an increased size.
+ * Selection can be toggled by double clicking the element in the table or the scene.
+ *
+ * Highlighting affects the color. If not highlighted elements are blue. If highlighted elements are green.
+ * (nodes drawn on a debug layer cant change color)
+ * The highlight can be set and cleared with the functions
+ * @code{.cpp}
+ * highlightNode(const ::std::string& name, bool highlighted)
+ * highlightEdge(const ::std::string& fst, const ::std::string& snd, bool highlighted)
+ * resetHilight()
+ * @endcode
+ *
+ *
+ * The graph used in the following examples can be created with following code:
+ * @code{.cpp}
+ * static ::armarx::FramedVector3Ptr table {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,7300.f,1000.f}, "table" }};
+ * static ::armarx::FramedVector3Ptr fridge {new ::armarx::FramedVector3{Eigen::Vector3f{2150.f,7750.f,1000.f}, "fridge" }};
+ * static ::armarx::FramedVector3Ptr sink {new ::armarx::FramedVector3{Eigen::Vector3f{2500.f,9700.f,1000.f}, "sink" }};
+ * static ::armarx::FramedVector3Ptr hub2 {new ::armarx::FramedVector3{Eigen::Vector3f{3750.f,5150.f,5000.f}, "hub2" }};
+ * static ::armarx::FramedVector3Ptr hub1 {new ::armarx::FramedVector3{Eigen::Vector3f{2900.f,8000.f,1000.f}, "hub1" }};
+ * static ::armarx::FramedVector3Ptr hub3 {new ::armarx::FramedVector3{Eigen::Vector3f{3400.f,2200.f,1000.f}, "hub3" }};
+ * static ::armarx::FramedVector3Ptr hub4 {new ::armarx::FramedVector3{Eigen::Vector3f{1900.f,3000.f,1000.f}, "hub4" }};
+ * static ::armarx::FramedVector3Ptr counter{new ::armarx::FramedVector3{Eigen::Vector3f{1890.f,4050.f,1000.f}, "counter"}};
+ *
+ * //prx is a proxy passing the commands to the plugin
+ * //add nodes
+ * prx->addNode(table);
+ * prx->addNode(fridge);
+ * prx->addNode(sink);
+ * prx->addNode(hub2);
+ * prx->addNode(hub1);
+ * prx->addNode(hub3);
+ * prx->addNode(hub4);
+ * prx->addNode(counter);
+ *
+ * //add edges
+ * prx->addEdge("hub1","hub2");
+ * prx->addEdge("hub1","table");
+ * prx->addEdge("hub1","sink");
+ * prx->addEdge("hub1","fridge");
+ * prx->addEdge("hub2","hub3");
+ * prx->addEdge("hub3","hub4");
+ * prx->addEdge("hub4","counter");
+ *
+ * //highlight a node and an edge
+ * prx->highlightEdge("hub2","hub3");
+ * prx->highlightNode("table");
+ * @endcode
+ *
+ * @image html GraphVisualizerGuiPlugin_ConfigDialog.png "The config dialog for the plugin." width=300px
+ * You can set the topic of the used debug drawer and the used debug layer.
+ *
+ * @image html GraphVisualizerGuiPlugin_Simulation.png "The graph drawn to the debug layer." width=300px
+ * @image html GraphVisualizerGuiPlugin_Widget.png "The plugin's ui." width=300px
+ *
+ * The ui has 5 sections
+ * -# Display options for the graph.
+ * - a. Rotate the graph clockwise
+ * - b. Rotate the graph counter clockwise
+ * - c. Zoom factor for the graph
+ * - d. Rotate and zoom the graph to display most of it. (The rotation is a multiple of pi/4)
+ * -# Is the scene containing the graph
+ * -# The table of nodes.
+ * -# The table of edges.
+ * -# Triggers a repaint for the debug layer.
+ *
+ * - A) Shows a highlighted and selected node
+ * - B) Shows a selected edge
+ * - C) Shows a highlighted edge
+ * - D) Shows a node. (no highlight or selection)
+ * - E) Shows the tool tip of an edge.
+ */
class GraphVisualizerGuiPlugin :
public ArmarXGuiPlugin
{
@@ -69,7 +193,9 @@ namespace armarx
};
/**
- * @brief A Widget used to visualize a undirected graph and draw it to a debug layer.
+ * @brief A widget used to visualize a undirected graph and draw it to a debug layer.
+ * @ingroup ArmarXGuiPlugins
+ * @see GraphVisualizerGuiPlugin
*/
class GraphVisualizerWidget :
public ArmarXComponentWidgetController,
@@ -78,12 +204,26 @@ namespace armarx
Q_OBJECT
public:
+ /**
+ * @brief The type of node ids. (This type implies the node exists)
+ */
+ typedef const std::string NodeId;
+
+ /**
+ * @brief The type of edge ids. (This type implies the edge exists)
+ */
+ typedef const std::pair<const std::string,const std::string> EdgeId;
+
+
GraphVisualizerWidget();
~GraphVisualizerWidget()
{
}
// inherited from Component
+ /**
+ * @see
+ */
virtual void onInitComponent();
virtual void onConnectComponent();
virtual void onExitComponent();
@@ -141,16 +281,28 @@ namespace armarx
void draw();
/**
- * @brief Toggles the double clicked node's selection.
+ * @brief Toggles the double clicked node's selection state.
* @param row Identifies the node.
*/
void nodeTableDoubleClicked(int row, int);
/**
- * @brief Toggles the double clicked edge's selection.
+ * @brief Toggles the double clicked edge's selection state.
* @param row Identifies the edge.
*/
void edgeTableDoubleClicked(int row, int);
+ /**
+ * @brief Toggles the double clicked node's selection state.
+ * @param id Identifies the node.
+ */
+ void nodeDoubleClicked(NodeId id);
+
+ /**
+ * @brief Toggles the double clicked edge's selection state.
+ * @param id Identifies the edge.
+ */
+ void edgeDoubleClicked(EdgeId id);
+
/**
* @brief Rotates the view clockwise.
*
@@ -176,15 +328,6 @@ namespace armarx
void adjustView();
private:
- /**
- * @brief The type of node ids. (This type implies the node exists)
- */
- typedef const std::string NodeId;
- /**
- * @brief The type of edge ids. (This type implies the edge exists)
- */
- typedef const std::pair<const std::string,const std::string> EdgeId;
-
/**
* @brief The NodeData struct holds data required for the node.
* The name is stored in the key used in the map nodes.
@@ -287,6 +430,10 @@ namespace armarx
/**
* @brief The scene displayed in the widget.
+ *
+ * For y coordinates -pos->y is used to mirror the scene on the y axis.
+ * If pos->y would be used the graph displayed in the scene would not
+ * match the graph drawn to the debug layer.
*/
QPointer<QGraphicsScene> scene;
@@ -308,12 +455,87 @@ namespace armarx
/**
* @brief The layer to draw on.
*/
- std::string layerName;
+ std::string debugDrawerLayerName;
+
+ friend class GraphVisualizerGraphicsEllipseItem;
+ friend class GraphVisualizerGraphicsLineItem;
};
/**
* @brief Boost shared pointer to a GraphVisualizerWidget.
*/
typedef boost::shared_ptr<GraphVisualizerWidget> GraphVisualizerGuiPluginPtr;
+
+
+ /**
+ * @brief Required to override the double click event. This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsEllipseItem: public QGraphicsEllipseItem
+ {
+ public:
+ typedef GraphVisualizerWidget::NodeId NodeId;
+
+ GraphVisualizerGraphicsEllipseItem(GraphVisualizerWidget& visuWidget,NodeId name, qreal x, qreal y, qreal width, qreal height, QGraphicsItem * parent = nullptr):
+ QGraphicsEllipseItem{x,y,width,height,parent},
+ id{name},
+ parentVisuWidget(visuWidget)
+ {
+ }
+
+ virtual ~GraphVisualizerGraphicsEllipseItem()
+ {
+ }
+ protected:
+ virtual void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ parentVisuWidget.nodeDoubleClicked(id);
+ }
+ private:
+ /**
+ * @brief Required to identify the element.
+ */
+ const NodeId id;
+
+ /**
+ * @brief Required to call nodeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ */
+ GraphVisualizerWidget& parentVisuWidget;
+ };
+
+ /**
+ * @brief Required to override the double click event. This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsLineItem: public QGraphicsLineItem
+ {
+ public:
+ typedef GraphVisualizerWidget::EdgeId EdgeId;
+
+ GraphVisualizerGraphicsLineItem(GraphVisualizerWidget& visuWidget,EdgeId name, qreal x1, qreal y1, qreal x2, qreal y2, QGraphicsItem * parent = 0 ):
+ QGraphicsLineItem{x1,y1,x2,y2,parent},
+ id{name},
+ parentVisuWidget(visuWidget)
+ {
+ }
+
+ virtual ~GraphVisualizerGraphicsLineItem()
+ {
+ }
+ signals:
+ protected:
+ virtual void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ parentVisuWidget.edgeDoubleClicked(id);
+ }
+ private:
+ /**
+ * @brief Required to identify the element.
+ */
+ const EdgeId id;
+
+ /**
+ * @brief Required to call edgeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ */
+ GraphVisualizerWidget& parentVisuWidget;
+ };
}
#endif
diff --git a/source/RobotAPI/gui_plugins/PlatformUnitPlugin/JoystickControlWidget.h b/source/RobotAPI/gui_plugins/PlatformUnitPlugin/JoystickControlWidget.h
index b11fb1732b9cffb365c9bccf116b985164ef9cc5..877795cbf12ffebfd1b69b80b8ddb668ac6767af 100644
--- a/source/RobotAPI/gui_plugins/PlatformUnitPlugin/JoystickControlWidget.h
+++ b/source/RobotAPI/gui_plugins/PlatformUnitPlugin/JoystickControlWidget.h
@@ -79,6 +79,38 @@ namespace armarx
/**
* @brief Provides a simple joystick control.
+ *
+ * The widget emits signals when the control is moved.
+ *
+ * The signal
+ * @code{.cpp}
+ * positionChanged(QPointF)
+ * @endcode
+ * passes the nibble's current position.
+ * The position is in the unit circle.
+ * The x-axis is horizontal and increases to the right.
+ * The y-axis is vertical and increases upwards.
+ *
+ * The signal
+ * @code{.cpp}
+ * rotationChanged(double)
+ * @endcode
+ * passes the position vectors rotation in polar coordinates (-pi,pi].
+ * The up position is 0.
+ * The down position is pi.
+ * The Quadrants 1 and 4 have positive value.
+ *
+ * If the constructor is called with useQuadrant3and4==true the control is the whole unit circle.
+ * If the constructor is called with useQuadrant3and4==false the control is the unit circle's upper semicircle. (y>=0)
+ *
+ * Possible positions can be influenced with
+ * \code{.cpp}
+ * setSteps(int steps);
+ * \endcode
+ * - steps>0 : only position vectors with a length from {n/steps | n in {0,1,...,steps} are valid. The nibble snaps to a valid position.
+ * - steps<=0: all positions in the unit circle are valid.
+ *
+ * @image html JoystickControlWidget_widget.png "Left: A widget with 2 steps using the whole unit circle. Right: A widget using the unit circle's upper semi circle." width=300px
*/
class JoystickControlWidget : public QWidget
{
diff --git a/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 1b6fd0f98c87cb6fb7c50c950f8a2872abd7d5c6..22387c044906d8340a29f5bdfcc27634633861f3 100644
--- a/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -165,9 +165,11 @@ void PlatformUnitWidget::controlTimerTick()
::Ice::Float posYInc = 0;
- ::Ice::Float rotationDelta=platformRotation-platformRotationAtMoveStart;
+ ::Ice::Float rotationDelta=0;
if(std::abs(len)>0.01)
{
+ //we are moving and need to adjust the rotation
+ rotationDelta=platformRotation-platformRotationAtMoveStart;
//did the platform just start to move?
if(!platformMoves)
{
@@ -189,7 +191,7 @@ void PlatformUnitWidget::controlTimerTick()
platformMoves=false;
}
//only update positions if required
- if(rotInc!=0 || posXInc !=0 || posYInc!=0)
+ if(rotInc!=0 || posXInc !=0 || posYInc!=0|| rotInc!=0)
{
platformUnitProxy->moveRelative(posXInc,posYInc,rotInc-rotationDelta, posAcc, rotAcc);
}
diff --git a/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index b0992092cb79a0ebbcc6cd432bca39669c358664..d418fbffe2c0bf4467d99b980ccfd9905e420494 100644
--- a/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
@@ -69,6 +69,11 @@ namespace armarx
\ingroup ArmarXGuiPlugins
\see PlatformUnitGuiPlugin
+
+ \image html PlatformUnitGuiPlugin_widgetpng.png "The plugin's ui." width=300px
+ -# The current position and rotation, fields to enter a new target and a button to set the platform in motion.
+ -# A joystick like control widget to move the platform. It has two speed level. The platform does not rotate to move in a direction. Up moves the platform forward.
+ -# A joystick like control widget to rotate the platform.
*/
class PlatformUnitWidget :
public ArmarXComponentWidgetController,
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index cd0af64087449314b8a725f94ccbf835b90a1974..e0d94d096cad18f6c637495c6a45bb075c674791 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -56,6 +56,14 @@ module armarx
sequence<LayerInformation> LayerInformationSequence;
+ struct DebugDrawerPointCloudElement
+ {
+ float x;
+ float y;
+ float z;
+ };
+ sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloud;
+
/*!
* \brief A layered drawing interface.
* All drawing operations are identified with a layer name in order to distinguish different drawing entitties.
@@ -71,10 +79,12 @@ module armarx
* \param globalPose The pose in global coordinate syetem.
*/
void setPoseVisu(string layerName, string poseName, PoseBase globalPose);
+ void setScaledPoseVisu(string layerName, string poseName, PoseBase globalPose, float scale);
void setLineVisu(string layerName, string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
void setBoxVisu(string layerName, string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
void setTextVisu(string layerName, string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
void setSphereVisu(string layerName, string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
+ void setPointCloudVisu(string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
/*!
* \brief setPoseVisu draws on the "debug" layer
@@ -82,10 +92,12 @@ module armarx
* \param globalPose
*/
void setPoseDebugLayerVisu(string poseName, PoseBase globalPose);
+ void setScaledPoseDebugLayerVisu(string poseName, PoseBase globalPose, float scale);
void setLineDebugLayerVisu(string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
void setBoxDebugLayerVisu(string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
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);
/*!
* \brief Remove visualization of coordinate system.
@@ -97,6 +109,7 @@ module armarx
void removeBoxVisu(string layerName, string boxName);
void removeTextVisu(string layerName, string textName);
void removeSphereVisu(string layerName, string sphereName);
+ void removePointCloudVisu(string layerName, string pointCloudName);
/*!
* \brief Removes pose from the "debug" layer.
@@ -106,12 +119,13 @@ module armarx
void removeBoxDebugLayerVisu(string boxName);
void removeTextDebugLayerVisu(string textName);
void removeSphereDebugLayerVisu(string sphereName);
+ void removePointCloudDebugLayerVisu(string pointCloudName);
/*!
* \brief clear removes all visualizations for the given layer
* \param layerName The name identifies the layer.
*/
- void clear(string layerName);
+ void clearLayer(string layerName);
/*!
* \brief clearDebugLayer calls <code>clear("debug");</code>
*/
@@ -124,6 +138,8 @@ module armarx
*/
void enableLayerVisu(string layerName, bool visible);
+ void enableDebugLayerVisu(bool visible);
+
/*!
* \brief Returns the names of all layers.
* \return The names of all layers.
@@ -149,6 +165,18 @@ module armarx
*/
void removeLayer(string layerName);
+ /*!
+ * \brief Disables the visibility of all layers. Only enableAllLayers() can reverse this.
+ * \see enableAllLayers
+ */
+ void disableAllLayers();
+
+ /*!
+ * \brief If layers were disabled they are visualized again.
+ * If a layer's visibility was changed the changes are applied.
+ */
+ void enableAllLayers();
+
};