diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index c88020b14e4efb3061253bd514c66125db7cbe01..450604aaeb5599b57daa3ec62f83fd1297f4ecd8 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -60,6 +60,27 @@ using namespace VirtualRobot;
namespace armarx
{
+ void selection_callback(void* userdata, SoPath* path)
+ {
+ // Force visualization update
+ path->getTail()->touch();
+
+ if (userdata)
+ {
+ ((DebugDrawerComponent*) userdata)->selectionCallback(path);
+ }
+ }
+
+ void deselection_callback(void* userdata, SoPath* path)
+ {
+ // Force visualization update
+ path->getTail()->touch();
+
+ if (userdata)
+ {
+ ((DebugDrawerComponent*) userdata)->deselectionCallback(path);
+ }
+ }
static const std::string DEBUG_LAYER_NAME
{"debug"
@@ -73,9 +94,13 @@ namespace armarx
timerSensor = NULL;
verbose = true;
- coinVisu = new SoSeparator;
+ coinVisu = new SoSelection;
coinVisu->ref();
+ coinVisu->policy = SoSelection::SHIFT;
+ coinVisu->addSelectionCallback(selection_callback, this);
+ coinVisu->addDeselectionCallback(deselection_callback, this);
+
layerMainNode = new SoSeparator;
layerMainNode->ref();
@@ -83,6 +108,70 @@ namespace armarx
u->units = SoUnits::MILLIMETERS;
coinVisu->addChild(u);
coinVisu->addChild(layerMainNode);
+
+ // Debug
+ enableSelections("");
+ }
+
+ void DebugDrawerComponent::enableSelections(const std::string& layerName, const ::Ice::Current&)
+ {
+ coinVisu->addSelectionCallback(selection_callback, this);
+ coinVisu->addDeselectionCallback(deselection_callback, this);
+ }
+
+ void DebugDrawerComponent::disableSelections(const std::string& layerName, const ::Ice::Current&)
+ {
+ coinVisu->removeSelectionCallback(selection_callback, this);
+ coinVisu->removeDeselectionCallback(deselection_callback, this);
+ coinVisu->deselectAll();
+ }
+
+ void DebugDrawerComponent::selectionChangeReport()
+ {
+ listenerPrx->reportSelectionChanged(getSelectedListNames());
+ }
+
+ void DebugDrawerComponent::selectionCallback(SoPath* path)
+ {
+ selectionChangeReport();
+ }
+
+ void DebugDrawerComponent::deselectionCallback(SoPath* path)
+ {
+ selectionChangeReport();
+ }
+
+ DebugDrawerSelectionList DebugDrawerComponent:: getSelectedListNames()
+ {
+ DebugDrawerSelectionList selectedElements;
+
+ const SoPathList* selectedList = coinVisu->getList();
+
+ for (int i = 0; i < selectedList->getLength(); i++)
+ {
+ SoPath* path = selectedList->operator [](i);
+ const char* name = path->getTail()->getName().getString();
+ selectedElements.push_back(name);
+ }
+
+ return selectedElements;
+ }
+
+ void DebugDrawerComponent::reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const Ice::Current&)
+ {
+ disableSelections("");
+
+ for (auto iterator = selectedElements.begin(); iterator != selectedElements.end(); ++iterator)
+ {
+ coinVisu->select(coinVisu->getByName(iterator->c_str()));
+ }
+
+ enableSelections("");
+ }
+
+ DebugDrawerSelectionList DebugDrawerComponent:: getSelections(const ::Ice::Current&)
+ {
+ return getSelections();
}
void DebugDrawerComponent::setVisuUpdateTime(float visuUpdatesPerSec)
@@ -135,6 +224,8 @@ namespace armarx
execTaskVisuUpdates->setDelayWarningTolerance(100);*/
}
+ offeringTopic(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
+ usingTopic(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
}
void DebugDrawerComponent::updateVisualizationCB(void* data, SoSensor* sensor)
@@ -153,6 +244,8 @@ namespace armarx
void DebugDrawerComponent::onConnectComponent()
{
verbose = true;
+
+ listenerPrx = getTopic<DebugDrawerListenerPrx>(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
}
void DebugDrawerComponent::onDisconnectComponent()
@@ -317,6 +410,7 @@ namespace armarx
newS->addChild(material);
SoCube* cube = new SoCube;
+ cube->setName(d.name.c_str());
cube->width = d.width;
cube->height = d.height;
cube->depth = d.depth;
@@ -1403,7 +1497,7 @@ namespace armarx
d.layerName = layerName;
d.name = robotName;
- for (auto& it : configuration)
+ for (auto & it : configuration)
{
d.configuration[it.first] = it.second;
}
@@ -1450,7 +1544,7 @@ namespace armarx
void DebugDrawerComponent::clearAll(const Ice::Current&)
{
- for (auto& i : layers)
+ for (auto & i : layers)
{
clearLayer(i.first);
}
@@ -1478,62 +1572,63 @@ namespace armarx
ARMARX_VERBOSE << "Clearing layer " << layerName;
}
- removeAccumulatedData(layerName);
+ coinVisu->deselectAll();
+ removeAccumulatedData(layerName);
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);
}
@@ -1910,7 +2005,7 @@ namespace armarx
ScopedRecursiveLockPtr l = getScopedVisuLock();
StringSequence seq {};
- for (const auto& layer : layers)
+ for (const auto & layer : layers)
{
seq.push_back(layer.first);
}
@@ -1923,7 +2018,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 6b64750ab7eb8056da2b0da3a08be61fd98928d4..f6ab78a1f678b574b5291a6bb62d793fe53a2161 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -28,6 +28,7 @@
// Coin3D & SoQt
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoSelection.h>
#include <Inventor/sensors/SoTimerSensor.h>
// ArmarX
@@ -61,6 +62,7 @@ namespace armarx
{
defineOptionalProperty<bool>("ShowDebugDrawing", true, "The simulator implements the DebugDrawerInterface. The debug visualizations (e.g. coordinate systems) can enabled/disbaled with this flag.");
defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<std::string>("DebugDrawerSelectionTopic", "DebugDrawerSelections", "Name of the DebugDrawerSelectionTopic");
}
};
@@ -102,7 +104,7 @@ namespace armarx
* @ingroup Component-DebugDrawerComponent
*/
class DebugDrawerComponent :
- virtual public armarx::DebugDrawerInterface,
+ virtual public armarx::DebugDrawerInterfaceAndListener,
virtual public Component
{
public:
@@ -231,6 +233,18 @@ namespace armarx
virtual void disableAllLayers(const ::Ice::Current& = ::Ice::Current());
virtual void enableAllLayers(const ::Ice::Current& = ::Ice::Current());
+ // Methods for selection management
+ virtual void enableSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+ virtual void disableSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+ virtual DebugDrawerSelectionList getSelections(const ::Ice::Current& = ::Ice::Current());
+ virtual DebugDrawerSelectionList getSelectedListNames();
+ virtual void selectionChangeReport();
+
+ void selectionCallback(SoPath* path);
+ void deselectionCallback(SoPath* path);
+
+ void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, 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.
@@ -440,7 +454,7 @@ namespace armarx
VirtualRobot::RobotPtr requestRobot(const RobotData& d);
std::map < std::string, VirtualRobot::RobotPtr > activeRobots;
- SoSeparator* coinVisu;
+ SoSelection* coinVisu;
bool verbose;
@@ -462,6 +476,8 @@ namespace armarx
ScopedRecursiveLockPtr getScopedAccumulatedDataLock();
+ DebugDrawerListenerPrx listenerPrx;
+
float cycleTimeMS;
//PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
SoTimerSensor* timerSensor;
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 81af0a584039272bfc1573653583bb8e6c2d7ffb..4272d868345fa80af102fac15846013901f2e774 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -91,6 +91,8 @@ module armarx
enum DrawStyle { FullModel, CollisionModel };
+ sequence<string> DebugDrawerSelectionList;
+
/*!
* \brief A layered drawing interface.
* All drawing operations are identified with a layer name in order to distinguish different drawing entitties.
@@ -249,9 +251,30 @@ module armarx
*/
void enableAllLayers();
+ /*!
+ * \brief Enable selections in visualizations
+ */
+ void enableSelections(string layerName);
+
+ /*!
+ * \brief Disable selections in visualizations
+ */
+ void disableSelections(string layerName);
+
+ /*!
+ * \brief Return currently selected elements
+ */
+ DebugDrawerSelectionList getSelections();
};
+ interface DebugDrawerListener
+ {
+ void reportSelectionChanged(DebugDrawerSelectionList selectedObjects);
+ };
+ interface DebugDrawerInterfaceAndListener extends DebugDrawerInterface, DebugDrawerListener
+ {
+ };
};
#endif