diff --git a/scenarios/ArVizExample/config/RobotToArVizApp.cfg b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
index b33276fe2dc50c8d6eadcb7beb004710f336e825..55d034d9c687c16ad2f34f458f85ca66da54cdff 100644
--- a/scenarios/ArVizExample/config/RobotToArVizApp.cfg
+++ b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
@@ -109,6 +109,14 @@
# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.RobotToArViz.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ArVizStorageName = ArVizStorage
+
+
# ArmarX.RobotToArViz.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
diff --git a/source/RobotAPI/components/ArViz/Client/Client.cpp b/source/RobotAPI/components/ArViz/Client/Client.cpp
index 3e6bcf38cf777e924dab5be2af0e0c86890f9427..268da627405450db9c12ac2dcba4871fff505f7b 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Client.cpp
@@ -71,6 +71,8 @@ Client Client::createForGuiPlugin(Component& component,
CommitResult Client::commit(const StagedCommit& commit)
{
CommitResult result;
+
+ ARMARX_CHECK_NOT_NULL(storage);
result.data_ = storage->commitAndReceiveInteractions(commit.data_);
return result;
}
@@ -78,6 +80,8 @@ CommitResult Client::commit(const StagedCommit& commit)
CommitResultAsync Client::commitAsync(const StagedCommit& commit)
{
CommitResultAsync result;
+
+ ARMARX_CHECK_NOT_NULL(storage);
result.async = storage->begin_commitAndReceiveInteractions(commit.data_);
result.storage = storage;
return result;
@@ -92,6 +96,7 @@ void Client::commit(const std::vector<Layer>& layers)
updates.push_back(layer.data_);
}
// This commit call still uses the legacy topic API
+ ARMARX_CHECK_NOT_NULL(topic);
topic->updateLayers(updates);
}
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 5500e774720824ad5aa72de8c76ab3f3db25d7de..958cfad6137a30b71d48804b312c033f68896f48 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -146,31 +146,6 @@ namespace viz
}
}
- bool isTranslation() const
- {
- return data_.type & data::InteractionFeedbackType::TRANSLATION_FLAG;
- }
-
- bool isRotation() const
- {
- return data_.type & data::InteractionFeedbackType::ROTATION_FLAG;
- }
-
- bool isAxisX() const
- {
- return data_.type & data::InteractionFeedbackType::AXIS_X_FLAG;
- }
-
- bool isAxisY() const
- {
- return data_.type & data::InteractionFeedbackType::AXIS_Y_FLAG;
- }
-
- bool isAxisZ() const
- {
- return data_.type & data::InteractionFeedbackType::AXIS_Z_FLAG;
- }
-
bool isTransformBegin() const
{
return data_.type & data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG;
@@ -206,14 +181,15 @@ namespace viz
return data_.chosenContextMenuEntry;
}
- Eigen::Matrix4f originalPose() const
+ Eigen::Matrix4f transformation() const
{
- return toEigen(data_.originalPose);
+ return toEigen(data_.transformation);
}
- Eigen::Matrix4f chosenPose() const
+ Eigen::Vector3f scale() const
{
- return toEigen(data_.chosenPose);
+ Eigen::Vector3f result(data_.scale.e0, data_.scale.e1, data_.scale.e2);
+ return result;
}
data::InteractionFeedback data_;
@@ -283,6 +259,7 @@ namespace viz
struct Client
{
Client() = default;
+ Client(const Client&) = default;
Client(armarx::Component& component,
std::string const& topicNameProperty = "ArVizTopicName",
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index 346130bbad0df4b67dfe4c15584b0c67200cfd60..a7c433454c587ae4a2c2c6fb142fde4ee3d7597b 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -17,6 +17,22 @@ namespace armarx::viz
{
using data::ColoredPoint;
+ struct AxesFlags
+ {
+ bool x = false;
+ bool y = false;
+ bool z = false;
+ bool local = false;
+ };
+
+ static const AxesFlags AXES_X = {true, false, false, false};
+ static const AxesFlags AXES_Y = {false, true, false, false};
+ static const AxesFlags AXES_Z = {false, false, true, false};
+ static const AxesFlags AXES_XY = {true, true, false, false};
+ static const AxesFlags AXES_YZ = {false, true, true, false};
+ static const AxesFlags AXES_XZ = {true, false, true, false};
+ static const AxesFlags AXES_XYZ = {true, true, true, false};
+
struct InteractionDescription
{
using Self = InteractionDescription;
@@ -41,42 +57,44 @@ namespace armarx::viz
return selection();
}
- Self& translation(bool x = true, bool y = true, bool z = true)
+ Self& translation(AxesFlags const& axes = AXES_XYZ)
{
- data_.enableFlags |= (x ? data::InteractionEnableFlags::TRANSLATION_X : 0);
- data_.enableFlags |= (y ? data::InteractionEnableFlags::TRANSLATION_Y : 0);
- data_.enableFlags |= (z ? data::InteractionEnableFlags::TRANSLATION_Z : 0);
+ data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::TRANSLATION_X : 0);
+ data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::TRANSLATION_Y : 0);
+ data_.enableFlags |= (axes.z ? data::InteractionEnableFlags::TRANSLATION_Z : 0);
// Translation implies selection
return selection();
}
- Self& rotation(bool x = true, bool y = true, bool z = true)
+ Self& rotation(AxesFlags const& axes = AXES_XYZ)
{
- data_.enableFlags |= (x ? data::InteractionEnableFlags::ROTATION_X : 0);
- data_.enableFlags |= (y ? data::InteractionEnableFlags::ROTATION_Y : 0);
- data_.enableFlags |= (z ? data::InteractionEnableFlags::ROTATION_Z : 0);
+ data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::ROTATION_X : 0);
+ data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::ROTATION_Y : 0);
+ data_.enableFlags |= (axes.z ? data::InteractionEnableFlags::ROTATION_Z : 0);
// Rotation implies selection
return selection();
}
- Self& globalAxes(bool global = true)
+ Self& scaling(AxesFlags const& axes = AXES_XYZ)
{
- if (global)
- {
- data_.enableFlags |= data::InteractionEnableFlags::GLOBAL_AXES;
- }
- else
- {
- data_.enableFlags &= ~data::InteractionEnableFlags::GLOBAL_AXES;
- }
- return *this;
+ data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::SCALING_X : 0);
+ data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::SCALING_Y : 0);
+ data_.enableFlags |= (axes.z ? data::InteractionEnableFlags::SCALING_Z : 0);
+ // Rotation implies selection
+ return selection();
}
- Self& fullTransform()
+ Self& transform()
{
return translation().rotation();
}
+ Self& hideDuringTransform()
+ {
+ data_.enableFlags |= data::InteractionEnableFlags::TRANSFORM_HIDE;
+ return *this;
+ }
+
data::InteractionDescription data_;
};
@@ -94,6 +112,9 @@ namespace armarx::viz
: data_(new ElementT)
{
data_->id = id;
+ data_->scale.e0 = 1.0f;
+ data_->scale.e1 = 1.0f;
+ data_->scale.e2 = 1.0f;
}
DerivedT& id(const std::string& id)
@@ -103,7 +124,6 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
- // TODO: add more overloads
DerivedT& position(float x, float y, float z)
{
auto& pose = data_->pose;
@@ -204,12 +224,26 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
- DerivedT& scale(float scale)
+ DerivedT& scale(Eigen::Vector3f scale)
{
- data_->scale = scale;
+ data_->scale.e0 = scale.x();
+ data_->scale.e1 = scale.y();
+ data_->scale.e2 = scale.z();
return *static_cast<DerivedT*>(this);
}
+ DerivedT& scale(float x, float y, float z)
+ {
+ data_->scale.e0 = x;
+ data_->scale.e1 = y;
+ data_->scale.e2 = z;
+
+ return *static_cast<DerivedT*>(this);
+ }
+ DerivedT& scale(float s)
+ {
+ return scale(s, s, s);
+ }
DerivedT& hide()
{
diff --git a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
index 45a21b24152352e8d911eba0782a6d2638f64484..a77f79632d3f29955f46773d0aea5c5602dfd0f9 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
@@ -70,8 +70,10 @@ namespace armarx::viz
PointCloud& points(std::vector<ColoredPoint> const& ps)
{
- data_->points = ps;
-
+ std::size_t memorySize = ps.size() * sizeof(ps[0]);
+ Ice::Byte* begin = (Ice::Byte*)ps.data();
+ Ice::Byte* end = begin + memorySize;
+ data_->points.assign(begin, end);
return *this;
}
@@ -82,12 +84,20 @@ namespace armarx::viz
{
if (isfinite(p))
{
- data_->points.push_back(p);
+ addPointUnchecked(p);
}
return *this;
}
+ PointCloud& addPointUnchecked(ColoredPoint const& p)
+ {
+ Ice::Byte* begin = (Ice::Byte*)&p;
+ Ice::Byte* end = begin + sizeof(p);
+ data_->points.insert(data_->points.end(), begin, end);
+ return *this;
+ }
+
PointCloud& addPoint(float x, float y, float z, const data::Color& color)
{
ColoredPoint p;
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
index f16ae2ac1a46eac428476fc1987672da280dd733..6208ac28e66c86230f0626970c3a994b7909dee3 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
@@ -19,10 +19,14 @@ namespace armarx::viz::coin
transform = new SoTransform;
material = new SoMaterial;
+ switch_ = new SoSwitch;
+ switch_->whichChild = SO_SWITCH_ALL;
+
separator = new SoSeparator;
separator->addChild(units);
separator->addChild(transform);
separator->addChild(material);
+ separator->addChild(switch_);
}
void ElementVisualization::updateBase(data::Element const& element)
@@ -30,7 +34,7 @@ namespace armarx::viz::coin
auto& p = element.pose;
transform->translation.setValue(p.x, p.y, p.z);
transform->rotation.setValue(p.qx, p.qy, p.qz, p.qw);
- transform->scaleFactor.setValue(element.scale, element.scale, element.scale);
+ transform->scaleFactor.setValue(element.scale.e0, element.scale.e1, element.scale.e2);
auto color = element.color;
const float conv = 1.0f / 255.0f;
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
index 595db28934519220c59c5e48ed97607a509a0f29..c29794f2c363a68d39f688bfd7c9aefcba15a319 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
@@ -3,6 +3,7 @@
#include <RobotAPI/interface/ArViz/Elements.h>
#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoSwitch.h>
#include <memory>
@@ -29,6 +30,7 @@ namespace armarx::viz::coin
SoUnits* units;
SoTransform* transform;
SoMaterial* material;
+ SoSwitch* switch_;
// TODO: Transform to flag system
bool wasUpdated = true;
bool visible = true;
@@ -77,7 +79,7 @@ namespace armarx::viz::coin
DataType* createDerived() final
{
DataType* result = new DataType;
- result->separator->addChild(result->node);
+ result->switch_->addChild(result->node);
return result;
}
diff --git a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
index 5ca14001aff6c7be187ad4c7736e86fd3ab6b3c6..743f0f5a50ae46d86a13047898dd698cd1b7aa75 100644
--- a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
@@ -22,6 +22,7 @@ void armarx::viz::CoinVisualizer::registerVisualizationTypes()
{
using namespace armarx::viz::coin;
+ elementVisualizersTypes.reserve(16);
elementVisualizers.reserve(16);
registerVisualizerFor<VisualizationBox>();
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
index 14b2331cad2c96ff419dcd5441424f4dafb5d491..5b4afb2bb562e0b52aac7bc954ed3f2526058c0f 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
@@ -37,9 +37,8 @@ namespace armarx::viz::coin
__attribute__((target("default")))
bool update(ElementType const& element)
{
- data::ColoredPointList const& pcl = element.points;
-
- int pclSize = (int)pcl.size();
+ data::ColoredPoint const* pclData = (data::ColoredPoint const*)element.points.data();
+ int pclSize = element.points.size() / sizeof(data::ColoredPoint);
int singleBufferSize = pclSize * 3;
buffer.resize(singleBufferSize * 2);
@@ -48,7 +47,6 @@ namespace armarx::viz::coin
float* coordsData = buffer.data();
float* colorsData = buffer.data() + singleBufferSize;
- data::ColoredPoint const* pclData = pcl.data();
for (int i = 0; i < pclSize; ++i)
{
data::ColoredPoint point = pclData[i];
@@ -75,9 +73,8 @@ namespace armarx::viz::coin
__attribute__((target("sse4.1")))
bool update(ElementType const& element)
{
- data::ColoredPointList const& pcl = element.points;
-
- int pclSize = (int)pcl.size();
+ float* pclIn = (float*)element.points.data();
+ int pclSize = element.points.size() / sizeof(data::ColoredPoint);
// Enlarge and align the buffers
int singleBufferSize = (pclSize + 3) * 3;
@@ -90,7 +87,6 @@ namespace armarx::viz::coin
float* positionsData = buffer.data();
float* colorsData = buffer.data() + singleBufferSize;
- float* pclIn = (float*)pcl.data();
float* colorsOut = colorsData;
float* positionsOut = positionsData;
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 35d7a4357f23dd886e08dc871621b265c9d24b65..a01c4b37c08698a77fea6588d46963fee6e6c6df 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -139,13 +139,17 @@ namespace armarx::viz::coin
ARMARX_DEBUG << "Loading robot from file " << VAROUT(project) << ", " << VAROUT(filename);
result.robot = loadRobot(project, filename);
-
- RobotInstancePool& instancePool = robotCache.emplace_back();
- instancePool.project = project;
- instancePool.filename = filename;
- instancePool.robots.push_back(result.robot);
- instancePool.usedInstances = 1;
-
+ if (result.robot)
+ {
+ RobotInstancePool& instancePool = robotCache.emplace_back();
+ instancePool.project = project;
+ instancePool.filename = filename;
+ instancePool.robots.push_back(result.robot);
+ instancePool.usedInstances = 1;
+ } else
+ {
+ ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", " << VAROUT(filename) << "could not be loaded!";
+ }
return result;
}
}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 0b5826a8975afca18a2b4181238b20e2d3a0fb18..f652218f7ad6a8944f1c12a20d65477a8821ef42 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -7,6 +7,9 @@
#include <Inventor/SoPath.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
namespace armarx::viz
{
namespace coin
@@ -14,6 +17,15 @@ namespace coin
void clearRobotCache();
void clearObjectCache();
}
+ static const int ANY_TRANSFORM = data::InteractionEnableFlags::TRANSLATION_X |
+ data::InteractionEnableFlags::TRANSLATION_Y |
+ data::InteractionEnableFlags::TRANSLATION_Z |
+ data::InteractionEnableFlags::ROTATION_X |
+ data::InteractionEnableFlags::ROTATION_Y |
+ data::InteractionEnableFlags::ROTATION_Z |
+ data::InteractionEnableFlags::SCALING_X |
+ data::InteractionEnableFlags::SCALING_Y |
+ data::InteractionEnableFlags::SCALING_Z;
struct CoinVisualizerWrapper : IceUtil::Shared
{
@@ -42,6 +54,24 @@ namespace coin
this_->onSelectEvent(path, data::InteractionFeedbackType::DESELECT);
}
+ static void startManipulationCallback(void* data, SoDragger* dragger)
+ {
+ CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
+ this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
+ }
+
+ static void duringManipulationCallback(void* data, SoDragger* dragger)
+ {
+ CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
+ this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
+ }
+
+ static void finishManipulationCallback(void* data, SoDragger* dragger)
+ {
+ CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
+ this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_END_FLAG);
+ }
+
static const char* toString(CoinVisualizerState state)
{
switch (state)
@@ -89,7 +119,7 @@ namespace coin
&CoinVisualizerWrapper::onUpdateSuccess,
&CoinVisualizerWrapper::onUpdateFailure);
- root = new SoSeparator;
+ manipulatorGroup = new SoSeparator;
// The SoSelection node enable selection of nodes via mouse click / ray casting
selection = new SoSelection;
@@ -97,7 +127,9 @@ namespace coin
selection->addDeselectionCallback(&deselectionCallback, this);
selection->setUserData(this);
- selection->addChild(root);
+ root = new SoSeparator;
+ root->addChild(manipulatorGroup);
+ root->addChild(selection);
// Preallocate some space for layers
layers.data.reserve(32);
@@ -178,7 +210,7 @@ namespace coin
// Create a new layer
SoSeparator* coinNode = new SoSeparator;
coinNode->ref();
- root->addChild(coinNode);
+ selection->addChild(coinNode);
layerIt = layers.data.insert(layerIt, CoinLayer(layerID, coinNode));
layerIt->elements.reserve(64);
@@ -236,33 +268,10 @@ namespace coin
// Has an interaction been added?
viz::data::InteractionDescription& oldInteraction = oldElement->data->interaction;
viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
- // TODO: Also handle the case, when an interaction is removed!
if (newInteraction.enableFlags != oldInteraction.enableFlags
|| oldInteraction.contextMenuOptions != newInteraction.contextMenuOptions)
{
- // Lookup the interaction entry
- ElementInteractionData* foundInteraction = nullptr;
- for (auto& interaction : elementInteractions)
- {
- if (interaction->layer == layer->id
- && interaction->element == updatedElement.id)
- {
- foundInteraction = interaction.get();
- }
- }
- if (foundInteraction == nullptr)
- {
- // Need to add a new entry
- foundInteraction = elementInteractions.emplace_back(
- new ElementInteractionData).get();
- }
- foundInteraction->layer = layer->id;
- foundInteraction->element = updatedElement.id;
- foundInteraction->interaction = newInteraction;
-
- // Add user data to Coin node
- oldElement->visu->separator->setUserData(foundInteraction);
- oldElement->visu->separator->setName("InteractiveNode");
+ addOrUpdateInteraction(layer->id, updatedElement.id, newInteraction, oldElement->visu.get());
}
oldElement->data = updatedElementPtr;
@@ -282,29 +291,7 @@ namespace coin
viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
if (newInteraction.enableFlags != 0)
{
- // Lookup the interaction entry
- ElementInteractionData* foundInteraction = nullptr;
- for (auto& interaction : elementInteractions)
- {
- if (interaction->layer == layer->id
- && interaction->element == updatedElement.id)
- {
- foundInteraction = interaction.get();
- }
- }
- if (foundInteraction == nullptr)
- {
- // Need to add a new entry
- foundInteraction = elementInteractions.emplace_back(
- new ElementInteractionData).get();
- }
- foundInteraction->layer = layer->id;
- foundInteraction->element = updatedElement.id;
- foundInteraction->interaction = newInteraction;
-
- // Add user data to Coin node
- elementVisu->separator->setUserData(foundInteraction);
- elementVisu->separator->setName("InteractiveNode");
+ addOrUpdateInteraction(layer->id, updatedElement.id, newInteraction, elementVisu.get());
}
layer->node->addChild(elementVisu->separator);
@@ -324,9 +311,40 @@ namespace coin
std::string typeName = armarx::GetTypeString(elementType);
ARMARX_WARNING << deactivateSpam(typeName, 1)
<< "CoinElementVisualizer returned null for type: " << typeName << "\n"
- << "You need to register a visualizer for each type in ArViz/Coin/Visualizer.cpp";
+ << "You need to register a visualizer for each type in ArViz/Coin/RegisterVisualizationTypes.cpp";
+ }
+ }
+ }
+
+ void CoinVisualizer::addOrUpdateInteraction(CoinLayerID const& layerID, std::string const& elementID,
+ data::InteractionDescription const& interactionDesc,
+ coin::ElementVisualization* visu)
+ {
+ // Lookup the interaction entry
+ ElementInteractionData* foundInteraction = nullptr;
+ for (auto& interaction : elementInteractions)
+ {
+ if (interaction->layer == layerID
+ && interaction->element == elementID)
+ {
+ foundInteraction = interaction.get();
}
}
+ if (foundInteraction == nullptr)
+ {
+ // Need to add a new entry
+ foundInteraction = elementInteractions.emplace_back(
+ new ElementInteractionData).get();
+ }
+ foundInteraction->layer = layerID;
+ foundInteraction->element = elementID;
+
+ foundInteraction->interaction = interactionDesc;
+ foundInteraction->visu = visu;
+
+ // Add user data to Coin node
+ visu->separator->setUserData(foundInteraction);
+ visu->separator->setName("InteractiveNode");
}
void CoinVisualizer::removeElementsIfNotUpdated(CoinLayer* layer)
@@ -374,7 +392,8 @@ namespace coin
std::unique_lock<std::mutex> lock(stateMutex);
storage = stateStorage;
selection->deselectAll();
- root->removeAllChildren();
+ selection->removeAllChildren();
+ manipulatorGroup->removeAllChildren();
interactionFeedbackBuffer.clear();
elementInteractions.clear();
selectedElement = nullptr;
@@ -493,13 +512,13 @@ namespace coin
return;
}
- int childIndex = root->findChild(layer->node);
+ int childIndex = selection->findChild(layer->node);
if (childIndex < 0)
{
// Layer is currently not visible
if (visible)
{
- root->addChild(layer->node);
+ selection->addChild(layer->node);
}
}
else
@@ -513,7 +532,7 @@ namespace coin
{
selection->deselectAll();
}
- root->removeChild(childIndex);
+ selection->removeChild(childIndex);
}
}
}
@@ -561,27 +580,10 @@ namespace coin
return;
}
- ElementInteractionData const* id = elementInteractions[index].get();
-
- CoinLayer* layer = layers.findLayer(id->layer);
- if (layer == nullptr)
- {
- ARMARX_WARNING << "Selected an element whose layer does not exist: \n"
- << "Layer: " << id->layer.first << "/" << id->layer.second
- << ", element: " << id->element;
- return;
- }
- CoinLayerElement* element = layer->findElement(id->element);
- if (element == nullptr)
- {
- ARMARX_WARNING << "Selected an element which does not exist: \n"
- << "Layer: " << id->layer.first << "/" << id->layer.second
- << ", element: " << id->element;
- return;
- }
+ ElementInteractionData* id = elementInteractions[index].get();
selection->deselectAll();
- selection->select(element->visu->separator);
+ selection->select(id->visu->separator);
}
static ElementInteractionData* findInteractionDataOnPath(SoPath* path)
@@ -626,14 +628,73 @@ namespace coin
}
return;
}
+ int enableFlags = id->interaction.enableFlags;
+ if ((enableFlags & data::InteractionEnableFlags::SELECT) == 0)
+ {
+ // Element was not marked for selection
+ return;
+ }
if (eventType == data::InteractionFeedbackType::SELECT)
{
selectedElement = id;
+
+ // Does the element support transform interactions?
+ if (enableFlags & ANY_TRANSFORM)
+ {
+ manipulatorGroup->removeAllChildren();
+ // We need to create the manipulator everytime to achieve the desired effect
+ // If we reuse an instance, the manipulator gets stuck to old objects...
+ manipulator = new SoTransformerManip;
+ SoDragger* dragger = manipulator->getDragger();
+ dragger->addStartCallback(&startManipulationCallback, this);
+ dragger->addMotionCallback(&duringManipulationCallback, this);
+ dragger->addFinishCallback(&finishManipulationCallback, this);
+
+ manipulatorGroup->addChild(manipulator);
+
+ // We add the same visualization node again
+ SoSeparator* newSep = new SoSeparator();
+ int childNum = id->visu->separator->getNumChildren();
+ for (int i = 0; i < childNum; ++i)
+ {
+ SoNode* child = id->visu->separator->getChild(i);
+ if (SoSwitch* switch_ = dynamic_cast<SoSwitch*>(child))
+ {
+ child = switch_->copy();
+ }
+ newSep->addChild(child);
+ }
+ manipulatorGroup->addChild(newSep);
+ manipulator->unsquishKnobs();
+
+ if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
+ {
+ id->visu->switch_->whichChild = SO_SWITCH_NONE;
+ }
+
+ }
}
else
{
+ if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
+ {
+ // Now, we should apply the transformation to the original object (at least temporary)
+ // This avoids flickering, after the object is deselected
+ SbMatrix manipMatrix;
+ manipMatrix.setTransform(1000.0f * manipulator->translation.getValue(), manipulator->rotation.getValue(),
+ manipulator->scaleFactor.getValue(), manipulator->scaleOrientation.getValue(),
+ manipulator->center.getValue());
+ id->visu->transform->multLeft(manipMatrix);
+
+ SbVec3f translation = id->visu->transform->translation.getValue();
+ ARMARX_IMPORTANT << "Visu translation: " << translation[0] << ", " << translation[1] << ", " << translation[2];
+
+ id->visu->switch_->whichChild = SO_SWITCH_ALL;
+ }
+
selectedElement = nullptr;
+ manipulatorGroup->removeAllChildren();
}
viz::data::InteractionFeedback& feedback = interactionFeedbackBuffer.emplace_back();
@@ -644,8 +705,232 @@ namespace coin
feedback.revision = pulledUpdates.revision;
}
+ SbVec3f translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
+ {
+ SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
+ SbVec3f t_added_rotation_and_scale;
+ r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
+ t_added_rotation_and_scale -= t_o;
+
+ SbVec3f t_added_rotation;
+ r_m.multVec(t_o, t_added_rotation);
+ t_added_rotation -= t_o;
+ SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
+ return t_added_scale;
+ }
+
+ SbVec3f translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
+ {
+ SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
+ SbVec3f t_added_rotation_and_scale;
+ r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
+ t_added_rotation_and_scale -= t_o;
+
+ // Do we need to exclude
+// SbVec3f t_added_rotation;
+// r_m.multVec(t_o, t_added_rotation);
+// t_added_rotation -= t_o;
+// SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
+ return t_added_rotation_and_scale;
+ }
+
+ Eigen::Matrix4f toEigen(SbMat const& mat)
+ {
+ Eigen::Matrix4f result;
+ for (int y = 0; y < 4; ++y)
+ {
+ for (int x = 0; x < 4; ++x)
+ {
+ result(x, y) = mat[y][x];
+ }
+ }
+
+ return result;
+ }
+
+ // This should constrain the rotation according to the enabled axes
+ static SbRotation constrainRotation(SbRotation input, int enableFlags)
+ {
+ SbMatrix mat;
+ mat.setRotate(input);
+ Eigen::Matrix4f mat_eigen = toEigen(mat);
+ Eigen::Matrix3f mat_rot = mat_eigen.block<3, 3>(0, 0);
+ Eigen::Vector3f rpy = mat_rot.eulerAngles(0, 1, 2);
+ // ARMARX_INFO << "rpy before: " << rpy.transpose();
+ if ((enableFlags & data::InteractionEnableFlags::ROTATION_X) == 0)
+ {
+ rpy(0) = 0.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::ROTATION_Y) == 0)
+ {
+ rpy(1) = 0.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::ROTATION_Z) == 0)
+ {
+ rpy(2) = 0.0f;
+ }
+ // ARMARX_INFO << "rpy after: " << rpy.transpose();
+
+ mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX())
+ * Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY())
+ * Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
+ Eigen::Quaternionf q(mat_rot);
+
+ SbRotation result(q.x(), q.y(), q.z(), q.w());
+ return result;
+ }
+
+ static SbVec3f constrainScaling(SbVec3f input, int enableFlags)
+ {
+ SbVec3f result = input;
+ if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
+ {
+ result[0] = 1.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::SCALING_Y) == 0)
+ {
+ result[1] = 1.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::SCALING_Z) == 0)
+ {
+ result[2] = 1.0f;
+ }
+ return result;
+ }
+
+ void CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
+ {
+ if (state != CoinVisualizerState::RUNNING)
+ {
+ return;
+ }
+ if (selectedElement == nullptr)
+ {
+ ARMARX_WARNING << "A manipulation event was fired but no element is selected";
+ return;
+ }
+
+ // If there is an entry in the feedback buffer already, update it
+ // This way, we prevent multiple events being sent to the client
+ viz::data::InteractionFeedback* newFeedback = nullptr;
+ for (viz::data::InteractionFeedback& feedback : interactionFeedbackBuffer)
+ {
+ if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM
+ && feedback.component == selectedElement->layer.first
+ && feedback.layer == selectedElement->layer.second
+ && feedback.element == selectedElement->element)
+ {
+ // This is a transform interaction concerning the same element
+ newFeedback = &feedback;
+ }
+ }
+ if (newFeedback == nullptr)
+ {
+ // Create a new interaction
+ newFeedback = &interactionFeedbackBuffer.emplace_back();
+ newFeedback->component = selectedElement->layer.first;
+ newFeedback->layer = selectedElement->layer.second;
+ newFeedback->element = selectedElement->element;
+ }
+
+ // TODO: Add more flags about translation/rotation, axes
+ // Can we get this information from the dragger?
+ newFeedback->type = data::InteractionFeedbackType::TRANSFORM | eventType;
+ newFeedback->revision = pulledUpdates.revision;
+
+
+ // Transformation applied by the manipulator
+ SbVec3f t_m = manipulator->translation.getValue();
+ SbRotation r_m_old = manipulator->rotation.getValue();
+ SbVec3f s_m_old = manipulator->scaleFactor.getValue();
+
+ int enableFlags = selectedElement->interaction.enableFlags;
+
+ SbRotation r_m = constrainRotation(r_m_old, enableFlags);
+ SbVec3f s_m = constrainScaling(s_m_old, enableFlags);
+
+ // Transformation applied to the object
+ // Translation is in mm, but the manipulator works in m!
+ SbVec3f t_o = 0.001f * selectedElement->visu->transform->translation.getValue();
+
+ if (s_m != s_m_old)
+ {
+ manipulator->scaleFactor.setValue(s_m);
+
+ // Remove motion induced by scaling
+ SbVec3f t_old = translationDueToScaling(r_m_old, t_m, s_m_old, t_o);
+ SbVec3f t_new = translationDueToScaling(r_m, t_m, s_m, t_o);
+ SbVec3f t_diff = t_new - t_old;
+ t_m -= t_diff;
+ }
+ if (r_m != r_m_old)
+ {
+ manipulator->rotation.setValue(r_m);
+
+ // Remove motion induced by rotation
+ SbVec3f t_old = translationDueToRotation(r_m_old, t_m, s_m, t_o);
+ SbVec3f t_new = translationDueToRotation(r_m, t_m, s_m, t_o);
+ SbVec3f t_diff = t_new - t_old;
+ t_m -= t_diff;
+ }
+
+ // TODO: Should we use the rotation of the object?
+ // SbRotation r_o = selectedElement->visu->transform->rotation.getValue();
+ // TODO: Do we need to consider scale of the object as well?
+ // SbVec3f s_o = selectedElement->visu->transform->scaleFactor.getValue();
+
+ // This value stays constant during rotation and scaling!
+ // It is zero when the manipulation first starts
+ // So we can reproduce the perceived motion here (hopefully)
+ SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
+ SbVec3f t_added_rotation_and_scale;
+ r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
+ t_added_rotation_and_scale -= t_o;
+ SbVec3f delta_t = t_added_rotation_and_scale + t_m;
+
+ SbVec3f t_added_rotation;
+ r_m.multVec(t_o, t_added_rotation);
+ t_added_rotation -= t_o;
+ SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
+
+ // Prevent translation along disabled axes
+ if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_X) == 0)
+ {
+ delta_t[0] = 0.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Y) == 0)
+ {
+ delta_t[1] = 0.0f;
+ }
+ if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Z) == 0)
+ {
+ delta_t[2] = 0.0f;
+ }
+
+ SbVec3f t_m_projected = delta_t - t_added_rotation_and_scale;
+ manipulator->translation.setValue(t_m_projected);
+
+ // t_m_projected is the correct value for the manipulator, but it still contains translation due to scaling!
+ // We should subtract the translation induced by scaling!
+ SbVec3f t_m_non_scaled = t_m_projected + t_added_scale;
+
+ data::GlobalPose& transformation = newFeedback->transformation;
+ transformation.x = 1000.0f * t_m_non_scaled[0];
+ transformation.y = 1000.0f * t_m_non_scaled[1];
+ transformation.z = 1000.0f * t_m_non_scaled[2];
+ transformation.qw = r_m[3];
+ transformation.qx = r_m[0];
+ transformation.qy = r_m[1];
+ transformation.qz = r_m[2];
+
+ armarx::Vector3f& scale = newFeedback->scale;
+ scale.e0 = s_m[0];
+ scale.e1 = s_m[1];
+ scale.e2 = s_m[2];
+ }
+
void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
{
- coin::exportToVRML(root, exportFilePath);
+ coin::exportToVRML(selection, exportFilePath);
}
}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index adda36b9ae47404a3f59062c649dad681cd0502f..573e86708b32818db41f866f5e99b739bf44c271 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -4,6 +4,7 @@
#include <RobotAPI/interface/ArViz/Component.h>
+#include <Inventor/manips/SoTransformerManip.h>
#include <Inventor/nodes/SoSelection.h>
#include <Inventor/nodes/SoSeparator.h>
@@ -191,6 +192,7 @@ namespace armarx::viz
CoinLayerID layer;
std::string element;
viz::data::InteractionDescription interaction;
+ coin::ElementVisualization* visu = nullptr;
};
class CoinVisualizer
@@ -230,6 +232,9 @@ namespace armarx::viz
CoinLayer& findOrAddLayer(CoinLayerID const& layerID);
void addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update);
+ void addOrUpdateInteraction(CoinLayerID const& layerID, std::string const& elementID,
+ data::InteractionDescription const& interactionDesc,
+ coin::ElementVisualization* visu);
void removeElementsIfNotUpdated(CoinLayer* layer);
std::vector<CoinLayerID> getLayerIDs();
@@ -238,6 +243,7 @@ namespace armarx::viz
void selectElement(int index);
void onSelectEvent(SoPath* path, int eventType);
+ void onManipulation(SoDragger* dragger, int eventType);
// These are selectable element IDs and need to be persistent in memory.
// We store a raw pointer to these into the SoSeperator objects of Coin.
// Later, we can retrieve the element ID from this pointer, if it has been selected.
@@ -261,6 +267,8 @@ namespace armarx::viz
SoSelection* selection = nullptr;
SoSeparator* root = nullptr;
+ SoSeparator* manipulatorGroup = nullptr;
+ SoTransformerManip* manipulator = nullptr;
std::atomic<CoinVisualizerUpdateResult> updateResult{CoinVisualizerUpdateResult::SUCCESS};
data::LayerUpdates pulledUpdates;
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index d772854de1792982ecf4015f7b61fd412ab6fb09..c0499f7d3868f7d1317b3110924a9f60fa71861b 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -353,7 +353,7 @@ namespace armarx
p.color.g = 255.0 * heightT;
p.color.b = 255.0 * (1.0 - heightT);
- pc.addPoint(p);
+ pc.addPointUnchecked(p);
}
}
@@ -389,7 +389,7 @@ namespace armarx
.position(pos)
.orientation(Eigen::Quaternionf(orientation))
.file("PriorKnowledgeData",
- "PriorKnowledgeData/objects/Maintenance/spraybottle/spraybottle.wrl");
+ "PriorKnowledgeData/objects/Maintenance/cable-ties/cable-ties.wrl");
layer.add(spraybottle);
layer.add(viz::Pose("spraybottle pose").pose(pos, orientation.toRotationMatrix()));
@@ -507,11 +507,15 @@ namespace armarx
viz::Box box = viz::Box("box")
.position(Eigen::Vector3f(2000.0f, 0.0f, 2000.0f))
.size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
- .color(viz::Color::fromRGBA(0, 165, 255));
+ .color(viz::Color::fromRGBA(0, 165, 255))
+ .scale(2.0f);
// Enable some interaction possibilities
box.enable(viz::interaction()
.selection()
- .contextMenu({"First Option", "Second Option", "Third Option"}));
+ .contextMenu({"First Option", "Second Option", "Third Option"})
+ .rotation()
+ .translation(viz::AXES_XY)
+ .scaling(viz::AXES_XYZ));
layer.add(box);
@@ -524,7 +528,10 @@ namespace armarx
// Enable some interaction possibilities
cyl.enable(viz::interaction()
.selection()
- .contextMenu({"Cyl Option 1", "Cyl Option 2"}));
+ .contextMenu({"Cyl Option 1", "Cyl Option 2"})
+ .rotation()
+ .translation(viz::AXES_YZ)
+ .scaling());
layer.add(cyl);
}
@@ -585,7 +592,7 @@ namespace armarx
<< result.revision();
- CycleUtil c(20);
+ CycleUtil c(25);
while (!task->isStopped())
{
double timeInSeconds = TimeUtil::GetTime().toSecondsDouble();
@@ -611,7 +618,13 @@ namespace armarx
stage.reset();
// We can stage multiple layers at once.
// This is equivalent to calling add(layer) multiple times.
- stage.add({testLayer, exampleLayer, pointsLayer, objectsLayer, disAppearingLayer, robotHandsLayer});
+ stage.add({testLayer,
+ exampleLayer,
+ pointsLayer,
+ objectsLayer,
+ disAppearingLayer,
+ robotHandsLayer
+ });
// We can request interaction feedback for specific layers
stage.requestInteraction(interactionLayer);
@@ -632,6 +645,31 @@ namespace armarx
<< "] Chosen context menu: "
<< interaction.chosenContextMenuEntry();
}
+ else if (interaction.type() == viz::InteractionFeedbackType::Transform)
+ {
+ std::string transformState;
+ if (interaction.isTransformBegin())
+ {
+ transformState = "Begin";
+ }
+ else if (interaction.isTransformDuring())
+ {
+ transformState = "During";
+ }
+ else if (interaction.isTransformEnd())
+ {
+ transformState = "End";
+ }
+ else
+ {
+ transformState = "<Unknwon>";
+ }
+ ARMARX_INFO << "[" << interaction.layer()
+ << "/" << interaction.element()
+ << "] Transformation " << transformState
+ << ": \n" << interaction.transformation()
+ << "\n scale: " << interaction.scale().transpose();
+ }
else
{
ARMARX_INFO << "[" << interaction.layer()
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5b68bd042d49ba38a8f54415ea3f232dc8d2bf45
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
@@ -0,0 +1,613 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ArVizInteractExample
+ * @author Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/Component.h>
+
+namespace armarx
+{
+ struct SingleSlider
+ {
+ SingleSlider(std::string const& name, viz::Color color)
+ : box(name)
+ , color(color)
+ {
+
+ }
+
+ viz::Box box;
+ viz::Color color;
+
+ Eigen::Vector3f initial = Eigen::Vector3f::Zero();
+ Eigen::Vector3f translation = Eigen::Vector3f::Zero();
+
+ };
+
+ struct SlidersState
+ {
+ SlidersState(Eigen::Vector3f origin)
+ : origin(origin)
+ , x("BoxX", viz::Color::red())
+ , y("BoxY", viz::Color::green())
+ , z("BoxZ", viz::Color::blue())
+ , sphere("Sphere")
+ {
+ float boxSize = 50.0f;
+
+ x.initial = origin + Eigen::Vector3f(0.5f * ARROW_LENGTH, 0.0f, 0.0f);
+ x.box.position(x.initial)
+ .color(x.color)
+ .size(boxSize)
+ .enable(viz::interaction()
+ .translation(viz::AXES_X)
+ .hideDuringTransform());
+
+ y.initial = origin + Eigen::Vector3f(0.0f, 0.5f * ARROW_LENGTH, 0.0f);
+ y.box.position(y.initial)
+ .color(y.color)
+ .size(boxSize)
+ .enable(viz::interaction()
+ .translation(viz::AXES_Y)
+ .hideDuringTransform());
+
+ z.initial = origin + Eigen::Vector3f(0.0f, 0.0f, 0.5f * ARROW_LENGTH);
+ z.box.position(z.initial)
+ .color(z.color)
+ .size(boxSize)
+ .enable(viz::interaction()
+ .translation(viz::AXES_Z)
+ .hideDuringTransform());
+
+ sphere.position(origin + 0.5f * ARROW_LENGTH * Eigen::Vector3f(1.0f, 1.0f, 1.0f))
+ .color(viz::Color::orange())
+ .radius(30.0f);
+ }
+
+ static constexpr const float ARROW_LENGTH = 1000.0f;
+
+ void visualize(viz::Client& arviz)
+ {
+ layerInteract = arviz.layer("Sliders");
+
+ float arrowWidth = 10.0f;
+
+ viz::Arrow arrowX = viz::Arrow("ArrowX")
+ .color(viz::Color::red())
+ .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
+ .width(arrowWidth);
+ layerInteract.add(arrowX);
+
+ viz::Arrow arrowY = viz::Arrow("ArrowY")
+ .color(viz::Color::green())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
+ .width(arrowWidth);
+ layerInteract.add(arrowY);
+
+ viz::Arrow arrowZ = viz::Arrow("ArrowZ")
+ .color(viz::Color::blue())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
+ .width(arrowWidth);
+ layerInteract.add(arrowZ);
+
+
+ layerInteract.add(x.box);
+ layerInteract.add(y.box);
+ layerInteract.add(z.box);
+
+ layerResult = arviz.layer("SlidersResult");
+ layerResult.add(sphere);
+ }
+
+ void handle(viz::InteractionFeedback const& interaction,
+ viz::StagedCommit* stage)
+ {
+ std::string const& element = interaction.element();
+ Eigen::Matrix4f transform = interaction.transformation();
+ Eigen::Vector3f translation = transform.block<3, 1>(0, 3);
+
+ SingleSlider* slider = nullptr;
+ if (element == "BoxX")
+ {
+ slider = &x;
+ }
+ else if (element == "BoxY")
+ {
+ slider = &y;
+ }
+ else if (element == "BoxZ")
+ {
+ slider = &z;
+ }
+ else
+ {
+ ARMARX_WARNING << "Unknown interaction: " << element;
+ return;
+ }
+
+ switch (interaction.type())
+ {
+ case viz::InteractionFeedbackType::Transform:
+ {
+ slider->translation = translation;
+
+ Eigen::Vector3f spherePosition(
+ x.initial.x() + x.translation.x(),
+ y.initial.y() + y.translation.y(),
+ z.initial.z() + z.translation.z());
+ sphere.position(spherePosition);
+
+ stage->add(layerResult);
+ } break;
+
+ case viz::InteractionFeedbackType::Select:
+ {
+ // Do nothing
+ } break;
+
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // If an object is deselected, we apply the transformation
+ slider->initial = slider->initial + slider->translation;
+ slider->translation = Eigen::Vector3f::Zero();
+ ARMARX_IMPORTANT << "Setting position to "
+ << slider->initial.transpose();
+ slider->box.position(slider->initial);
+
+ stage->add(layerInteract);
+ } break;
+
+ default:
+ {
+ // Do nothing for the other interaction types
+ } break;
+ }
+
+
+ }
+
+ Eigen::Vector3f origin;
+ SingleSlider x;
+ SingleSlider y;
+ SingleSlider z;
+
+ viz::Sphere sphere;
+
+ viz::Layer layerInteract;
+ viz::Layer layerResult;
+ };
+
+
+ enum class SpawnerType
+ {
+ Box,
+ Cylinder,
+ Sphere,
+ };
+
+ enum class SpawnerOption
+ {
+ DeleteAll = 0,
+ DeleteType = 1,
+ };
+
+ struct Spawner
+ {
+ SpawnerType type = SpawnerType::Box;
+
+ Eigen::Vector3f position = Eigen::Vector3f::Zero();
+ float size = 100.0f;
+ viz::Color color = viz::Color::black();
+
+ void visualize(int i, viz::Layer& layer)
+ {
+ viz::InteractionDescription interaction = viz::interaction()
+ .selection().transform().scaling()
+ .contextMenu({"Delete All", "Delete All of Type"});
+ std::string name = "Spawner_" + std::to_string(i);
+ switch (type)
+ {
+ case SpawnerType::Box:
+ {
+ viz::Box box = viz::Box(name)
+ .position(position)
+ .size(size)
+ .color(color)
+ .enable(interaction);
+ layer.add(box);
+ } break;
+ case SpawnerType::Cylinder:
+ {
+ viz::Cylinder cylinder = viz::Cylinder(name)
+ .position(position)
+ .radius(size*0.5f)
+ .height(size)
+ .color(color)
+ .enable(interaction);
+ layer.add(cylinder);
+ } break;
+ case SpawnerType::Sphere:
+ {
+ viz::Sphere sphere = viz::Sphere(name)
+ .position(position)
+ .radius(size*0.5f)
+ .color(color)
+ .enable(interaction);
+ layer.add(sphere);
+ } break;
+ }
+ }
+ };
+
+ struct SpawnedObject
+ {
+ int index = 0;
+ Spawner* source = nullptr;
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ Eigen::Vector3f scale = Eigen::Vector3f::Ones();
+
+ void visualize(viz::Layer& layer)
+ {
+ viz::InteractionDescription interaction = viz::interaction().none();
+ std::string name = "Object_" + std::to_string(index);
+
+ Eigen::Matrix4f initial = Eigen::Matrix4f::Identity();
+ initial.block<3, 1>(0, 3) = source->position;
+ Eigen::Matrix4f pose = transform * initial;
+
+ switch (source->type)
+ {
+ case SpawnerType::Box:
+ {
+ viz::Box box = viz::Box(name)
+ .pose(pose)
+ .scale(scale)
+ .size(source->size)
+ .color(source->color)
+ .enable(interaction);
+ layer.add(box);
+ } break;
+ case SpawnerType::Cylinder:
+ {
+ viz::Cylinder cylinder = viz::Cylinder(name)
+ .pose(pose)
+ .scale(scale)
+ .radius(source->size * 0.5f)
+ .height(source->size)
+ .color(source->color)
+ .enable(interaction);
+ layer.add(cylinder);
+ } break;
+ case SpawnerType::Sphere:
+ {
+ viz::Sphere sphere = viz::Sphere(name)
+ .pose(pose)
+ .scale(scale)
+ .radius(source->size * 0.5f)
+ .color(source->color)
+ .enable(interaction);
+ layer.add(sphere);
+ } break;
+ }
+ }
+ };
+
+ struct SpawnersState
+ {
+ SpawnersState(Eigen::Vector3f origin)
+ : origin(origin)
+ {
+ float size = 100.0f;
+ {
+ Spawner& spawner = spawners.emplace_back();
+ spawner.type = SpawnerType::Box;
+ spawner.position = origin + Eigen::Vector3f(750.0f, 500.0f, 0.5f * size);
+ spawner.color = viz::Color::cyan();
+ }
+ {
+ Spawner& spawner = spawners.emplace_back();
+ spawner.type = SpawnerType::Cylinder;
+ spawner.position = origin + Eigen::Vector3f(1250.0f, 500.0f, 0.5f * size);
+ spawner.color = viz::Color::magenta();
+ }
+ {
+ Spawner& spawner = spawners.emplace_back();
+ spawner.type = SpawnerType::Sphere;
+ spawner.position = origin + Eigen::Vector3f(1000.0f, 750.0f, 0.5f * size);
+ spawner.color = viz::Color::yellow();
+ }
+ }
+
+ void visualize(viz::Client& arviz)
+ {
+ layerSpawners = arviz.layer("Spawners");
+
+ int index = 0;
+ for (Spawner& spawner: spawners)
+ {
+ spawner.visualize(index, layerSpawners);
+ index += 1;
+ }
+
+ layerObjects = arviz.layer("SpawnedObjects");
+ }
+
+ void handle(viz::InteractionFeedback const& interaction,
+ viz::StagedCommit* stage)
+ {
+ Spawner* spawner = nullptr;
+ for (int i = 0; i < (int)spawners.size(); ++i)
+ {
+ std::string name = "Spawner_" + std::to_string(i);
+ if (interaction.element() == name)
+ {
+ spawner = &spawners[i];
+ }
+ }
+
+ switch (interaction.type())
+ {
+ case viz::InteractionFeedbackType::Select:
+ {
+ // Create a spawned object
+ spawnedObject.index = spawnedObjectCounter++;
+ spawnedObject.source = spawner;
+ spawnedObject.transform = Eigen::Matrix4f::Identity();
+ spawnedObject.scale.setOnes();
+ } break;
+
+ case viz::InteractionFeedbackType::Transform:
+ {
+ // Update state of spawned object
+ spawnedObject.transform = interaction.transformation();
+ spawnedObject.scale = interaction.scale();
+ if (interaction.isTransformBegin() || interaction.isTransformDuring())
+ {
+ // Visualize all other objects except the currently spawned one
+ layerObjects.clear();
+ for (auto& object : objects)
+ {
+ object.visualize(layerObjects);
+ }
+ stage->add(layerObjects);
+ }
+ if (interaction.isTransformEnd())
+ {
+ spawnedObject.visualize(layerObjects);
+ stage->add(layerObjects);
+ }
+ } break;
+
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // Save state of spawned object
+ objects.push_back(spawnedObject);
+ } break;
+
+ case viz::InteractionFeedbackType::ContextMenuChosen:
+ {
+ SpawnerOption option = (SpawnerOption)(interaction.chosenContextMenuEntry());
+ switch (option)
+ {
+ case SpawnerOption::DeleteAll:
+ {
+ objects.clear();
+ layerObjects.clear();
+
+ stage->add(layerObjects);
+ } break;
+ case SpawnerOption::DeleteType:
+ {
+ auto newEnd = std::remove_if(objects.begin(), objects.end(),
+ [spawner](SpawnedObject const& obj)
+ {
+ return obj.source == spawner;
+ });
+ objects.erase(newEnd, objects.end());
+
+ layerObjects.clear();
+ for (auto& object : objects)
+ {
+ object.visualize(layerObjects);
+ }
+
+ stage->add(layerObjects);
+ } break;
+ }
+ }
+
+ default:
+ {
+ // Ignore other interaction types
+ } break;
+ }
+ }
+
+ Eigen::Vector3f origin;
+
+ std::vector<Spawner> spawners;
+ SpawnedObject spawnedObject;
+ int spawnedObjectCounter = 0;
+ std::vector<SpawnedObject> objects;
+
+ viz::Layer layerSpawners;
+ viz::Layer layerObjects;
+ };
+
+ /**
+ * @defgroup Component-ArVizInteractExample ArVizInteractExample
+ * @ingroup RobotAPI-Components
+ *
+ * An example for how to visualize 3D elements via the 3D visualization
+ * framework ArViz.
+ *
+ * The example creates several layers, fills them with visualization
+ * elements, and commits them to ArViz.
+ *
+ * To see the result:
+ * \li Start the component `ArVizStorage`
+ * \li Open the gui plugin `ArViz`
+ * \li Start the component `ArVizInteractExample`
+ *
+ * The scenario `ArVizInteractExample` starts the necessary components,
+ * including the example component.
+ *
+ *
+ * A component which wants to visualize data via ArViz should:
+ * \li `#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>`
+ * \li Inherit from the `armarx::ArVizComponentPluginUser`. This adds the
+ * necessary properties (e.g. the topic name) and provides a
+ * ready-to-use ArViz client called `arviz`.
+ * \li Use the inherited ArViz client variable `arviz` of type `viz::Client`
+ * to create layers, add visualization elements to the layers,
+ * and commit the layers to the ArViz topic.
+ *
+ * \see ArVizInteractExample
+ *
+ *
+ * @class ArVizInteractExample
+ * @ingroup Component-ArVizInteractExample
+ *
+ * @brief An example for how to use ArViz.
+ *
+ * @see @ref Component-ArVizInteractExample
+ */
+ struct ArVizInteractExample :
+ virtual armarx::Component,
+ // Deriving from armarx::ArVizComponentPluginUser adds necessary properties
+ // and provides a ready-to-use ArViz client called `arviz`.
+ virtual armarx::ArVizComponentPluginUser
+ {
+ std::string getDefaultName() const override
+ {
+ return "ArVizInteractExample";
+ }
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ {
+ armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+ return defs;
+ }
+
+ void onInitComponent() override
+ { }
+
+ void onConnectComponent() override
+ {
+ task = new RunningTask<ArVizInteractExample>(this, &ArVizInteractExample::run);
+ task->start();
+ }
+
+ void onDisconnectComponent() override
+ {
+ const bool join = true;
+ task->stop(join);
+ task = nullptr;
+ }
+
+ void onExitComponent() override
+ { }
+
+
+ void run()
+ {
+ viz::StagedCommit stage;
+
+ viz::Layer regions = arviz.layer("Regions");
+ Eigen::Vector3f origin1(-2000.0f, 0.0f, 0.0f);
+ Eigen::Vector3f origin2(0.0f, 0.0f, 0.0f);
+ Eigen::Vector3f origin3(-2000.0f, -2000.0f, 0.0f);
+ Eigen::Vector3f origin4(0.0f, -2000.0f, 0.0f);
+ {
+ viz::Cylinder separatorX = viz::Cylinder("SeparatorX")
+ .fromTo(origin1, origin1 + 4000.0f * Eigen::Vector3f::UnitX())
+ .radius(5.0f);
+ regions.add(separatorX);
+
+ viz::Cylinder separatorY = viz::Cylinder("SeparatorY")
+ .fromTo(origin4, origin4 + 4000.0f * Eigen::Vector3f::UnitY())
+ .radius(5.0f);
+ regions.add(separatorY);
+
+ stage.add(regions);
+ }
+
+
+ SlidersState sliders(origin1 + Eigen::Vector3f(500.0f, 500.0f, 0.0f));
+ SpawnersState spawners(origin2);
+
+ sliders.visualize(arviz);
+ stage.add(sliders.layerInteract);
+ stage.add(sliders.layerResult);
+
+ spawners.visualize(arviz);
+ stage.add(spawners.layerSpawners);
+ stage.add(spawners.layerObjects);
+
+ viz::CommitResult result = arviz.commit(stage);
+ ARMARX_INFO << "Initial commit at revision: " << result.revision();
+
+ CycleUtil c(25.0f);
+ while (!task->isStopped())
+ {
+ result = arviz.commit(stage);
+
+ // Reset the stage, so that it can be rebuild during the interaction handling
+ stage.reset();
+
+ stage.requestInteraction(sliders.layerInteract);
+ stage.requestInteraction(spawners.layerSpawners);
+
+ viz::InteractionFeedbackRange interactions = result.interactions();
+ for (viz::InteractionFeedback const& interaction : interactions)
+ {
+ if (interaction.layer() == "Sliders")
+ {
+ sliders.handle(interaction, &stage);
+ }
+ if (interaction.layer() == "Spawners")
+ {
+ spawners.handle(interaction, &stage);
+ }
+ }
+
+ c.waitForCycleDuration();
+ }
+ }
+
+ RunningTask<ArVizInteractExample>::pointer_type task;
+
+ };
+
+ ARMARX_DECOUPLED_REGISTER_COMPONENT(ArVizInteractExample);
+}
+
+int main(int argc, char* argv[])
+{
+ return armarx::DecoupledMain(argc, argv);
+}
+
diff --git a/source/RobotAPI/components/ArViz/Example/CMakeLists.txt b/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
index 2e21d21e1a0bc042bfb70e0dcd08a2891de3bd56..40d0771c47fd68b984c8ce560877b110500e2aa1 100644
--- a/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
@@ -16,3 +16,20 @@ set(HEADERS
)
armarx_add_component_executable("${SOURCES}" "${HEADERS}")
+
+armarx_component_set_name("ArVizInteractExample")
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ DecoupledSingleComponent
+ ArViz
+ RobotAPIComponentPlugins # For ArVizComponentPluginUser
+)
+
+set(SOURCES
+ ArVizInteractExample.cpp
+)
+set(HEADERS
+)
+
+armarx_add_component_executable("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_base.cpp b/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
index 0d261f82d2c771bc39d3c347e4df3f04466a6768..ae5f23e56d2f3603622b1c3f0f6c6d28debc4590 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
@@ -101,7 +101,7 @@ namespace armarx::viz
element.pose = j.at("pose").get<data::GlobalPose>();
element.color = j.at("color").get<data::Color>();
element.flags = j.at("flags").get<int>();
- element.scale = j.at("scale").get<float>();
+ element.scale = j.at("scale").get<armarx::Vector3f>();
}
}
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
index 39200985c55a98e19f119c6e08e0d45565ca2f9a..bfb083d844beb8d64851cd14725f049692d1028e 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
@@ -154,11 +154,13 @@ namespace armarx::viz
j["transparency"] = pointCloud.transparency;
j["pointSizeInPixels"] = pointCloud.pointSizeInPixels;
- j["# Points"] = pointCloud.points.size();
+ std::size_t numPoints = pointCloud.points.size() / sizeof (ColoredPoint);
+ j["# Points"] = numPoints;
j[json::meta::KEY]["# Points"] = json::meta::READ_ONLY;
- j["Points[0:10]"] = ColoredPointList(pointCloud.points.begin(),
- pointCloud.points.begin() + std::min(size_t(10), pointCloud.points.size()));
+ ColoredPoint const* begin = (ColoredPoint const*)pointCloud.points.data();
+ ColoredPoint const* end = begin + std::min(std::size_t(10), numPoints);
+ j["Points[0:10]"] = ColoredPointList(begin, end);
}
void data::from_json(const nlohmann::json& j, ElementPointCloud& pointCloud)
{
diff --git a/source/RobotAPI/components/ArViz/test/CMakeLists.txt b/source/RobotAPI/components/ArViz/test/CMakeLists.txt
index cfc6cc1c611ffb7fb6f1e8363a1708a812be644a..5bfd0e643cc3540fe5a3b49259025592ba5dd19e 100644
--- a/source/RobotAPI/components/ArViz/test/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/test/CMakeLists.txt
@@ -2,5 +2,5 @@
# Libs required for the tests
SET(LIBS ${LIBS} ArmarXCore ArViz)
-armarx_add_test(ArViz_Client_PointCloudTest Client/PointCloudTest.cpp "${LIBS}")
+#armarx_add_test(ArViz_Client_PointCloudTest Client/PointCloudTest.cpp "${LIBS}")
armarx_add_test(ArViz_Client_ColorTest Client/ColorTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp b/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
index c791c6ed7f719240304740f76d335b65f6b98564..2100834633ab64dc8a2f9fe7fe23298cb510ef0c 100644
--- a/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
+++ b/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
@@ -75,7 +75,6 @@ namespace PointCloudTestNamespace
}
viz::PointCloud pc {"pointcloud"};
- std::vector<viz::ColoredPoint>& ps = pc.data_->points;
const std::vector<PointXYZRGBL> points
{
@@ -128,8 +127,10 @@ BOOST_AUTO_TEST_CASE(test_addPoint_color_types)
using namespace PointCloudTestNamespace;
namespace viz = armarx::viz;
viz::PointCloud pc("pointcloud");
- auto& points = pc.data_->points;
+ viz::ColoredPoint* begin = (viz::ColoredPoint*)pc.data_->points.data();
+ viz::ColoredPoint* end = begin + (pc.data_->points.size() / sizeof (viz::ColoredPoint));
+ viz::ColoredPointList points(begin, end);
// XYZ, default color
pc.addPoint(PointXYZ());
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index d5ffd4c911804296d10c756577b3c573b3174235..10b31920d68599552ab3dc0713a41bf8ab56033f 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -499,6 +499,7 @@ namespace armarx
void ExampleMemoryClient::commitExamplesWithLinks()
{
+ ARMARX_IMPORTANT << "Committing multiple entity updates with links ...";
const armem::Time time = armem::Time::now();
armem::Commit commit;
@@ -551,6 +552,32 @@ namespace armarx
{
ARMARX_WARNING << commitResult.allErrorMessages();
}
+
+
+ // Resolve memory IDs via memory name system (works for IDs from different servers).
+ ARMARX_IMPORTANT << "Resolving multiple memory IDs via Memory Name System:";
+ {
+ std::vector<armem::MemoryID> ids;
+ for (armem::EntityUpdateResult& result : commitResult.results)
+ {
+ ids.push_back(result.snapshotID);
+ }
+ ARMARX_CHECK_EQUAL(ids.size(), commit.updates.size());
+
+ std::map<armem::MemoryID, armem::wm::EntityInstance> instances =
+ memoryNameSystem().resolveEntityInstances(ids);
+ ARMARX_CHECK_EQUAL(instances.size(), commit.updates.size());
+
+ std::stringstream ss;
+ for (const auto& [id, instance]: instances)
+ {
+ ss << "- Snapshot " << id << " "
+ << "\n--> Instance" << instance.id()
+ << " (# keys in data: " << instance.data()->childrenSize() << ")"
+ << "\n";
+ }
+ ARMARX_INFO << ss.str();
+ }
}
void ExampleMemoryClient::commitExampleImages()
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 25c81d4ac78aff9057daf6fb9dc318ed753eead6..40d70b0d55cc93a372a058f0a850aa344a426bcd 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -258,17 +258,14 @@ namespace armarx
using SensorAndControl = ControlThreadOutputBuffer::Entry;
}
-#define _detail_ARMARX_RT_REDIRECT( ...) ([&]{ \
- ARMARX_INFO << deactivateSpam(1) << "Redirected RT Logging:\n";\
- return &::armarx::detail::RtMessageLogEntryNull::Instance;}())
-#define ARMARX_RT_LOGF(...) (*((::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance())? _detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION,__LINE__, __VA_ARGS__, true) : _detail_ARMARX_RT_REDIRECT(__VA_ARGS__)))
+#define ARMARX_RT_LOGF(...) (*(_detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION,__LINE__, __VA_ARGS__, true)))
#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...) \
([&]{ \
using namespace ::armarx; \
using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase; \
- struct RtMessageLogEntry : RtMessageLogEntryBase \
+ struct RtMessageLogEntry : RtMessageLogEntryBase \
{ \
using TupleT = decltype(std::make_tuple(__VA_ARGS__)); \
const TupleT tuple; \
@@ -283,10 +280,16 @@ namespace armarx
RtMessageLogEntry(TupleT tuple) : tuple{std::move(tuple)} {} \
RtMessageLogEntry(const RtMessageLogEntry&) = default; \
}; \
- ARMARX_CHECK_NOT_NULL(::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()); \
- return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance() \
+ if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()) { \
+ return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance() \
->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__); \
- }())
+ } else { \
+ *(loghelper(file_, line_, func_)) << "Redirected RT Logging:\n" \
+ << RtMessageLogEntry(std::make_tuple(__VA_ARGS__)).format(); \
+ return dynamic_cast<RtMessageLogEntryBase*> \
+ (&::armarx::detail::RtMessageLogEntryNull::Instance); \
+ } \
+ }())
#define ARMARX_RT_LOGF_DEBUG(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::DEBUG)
#define ARMARX_RT_LOGF_VERBOSE(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::VERBOSE)
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 7aa05e3a197d4054ea04eaaa8ebc0f68304c50cc..1962d259c0d2329c9870f1e3d314fdea17a1f60d 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -1121,7 +1121,7 @@ namespace armarx
SoNode* ArVizWidgetController::getScene()
{
- return visualizer.selection;
+ return visualizer.root;
}
static const std::string CONFIG_KEY_STORAGE = "Storage";
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index ca27a0e1075e1896e786dd76d74f340a0e7a9404..5833f259e501b2e47106bc1c9195158f30fccd83 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -42,14 +42,22 @@ module data
const int TRANSLATION_X = 4;
const int TRANSLATION_Y = 8;
const int TRANSLATION_Z = 16;
+ const int TRANSLATION_LOCAL = 32;
// Enable rotation along the three axes
- const int ROTATION_X = 32;
- const int ROTATION_Y = 64;
- const int ROTATION_Z = 128;
-
- // Are the axes global (1) or object local (0)?
- const int GLOBAL_AXES = 256;
+ const int ROTATION_X = 64;
+ const int ROTATION_Y = 128;
+ const int ROTATION_Z = 256;
+ const int ROTATION_LOCAL = 512;
+
+ // Enable scaling along the three axes
+ const int SCALING_X = 1024;
+ const int SCALING_Y = 2048;
+ const int SCALING_Z = 4096;
+ const int SCALING_LOCAL = 8192;
+
+ // Hide the original object during the transformation
+ const int TRANSFORM_HIDE = 16384;
};
struct InteractionDescription
@@ -69,19 +77,10 @@ module data
const int TRANSFORM = 4;
- // Flag to indicate the kind of transformation
- const int TRANSLATION_FLAG = 16;
- const int ROTATION_FLAG = 32;
-
- // Flag to indicate the axis used for transformation
- const int AXIS_X_FLAG = 64;
- const int AXIS_Y_FLAG = 128;
- const int AXIS_Z_FLAG = 256;
-
// Flag to indicate state of the transformation
- const int TRANSFORM_BEGIN_FLAG = 512;
- const int TRANSFORM_DURING_FLAG = 1024;
- const int TRANSFORM_END_FLAG = 2048;
+ const int TRANSFORM_BEGIN_FLAG = 16;
+ const int TRANSFORM_DURING_FLAG = 32;
+ const int TRANSFORM_END_FLAG = 64;
};
struct InteractionFeedback
@@ -100,9 +99,9 @@ module data
// Chosen context menu entry is only relevant for type == CONTEXT_MENU_CHOSEN
int chosenContextMenuEntry = 0;
- // Original and chosen poase are only relevant for type == TRANSFORM
- GlobalPose originalPose;
- GlobalPose chosenPose;
+ // Applied transformation is only relevant for type == TRANSFORM
+ GlobalPose transformation;
+ Vector3f scale;
};
sequence<InteractionFeedback> InteractionFeedbackSeq;
@@ -120,7 +119,7 @@ module data
InteractionDescription interaction;
GlobalPose pose;
- float scale = 1.0f;
+ Vector3f scale;
Color color;
int flags = 0;
};
@@ -211,7 +210,7 @@ module data
class ElementPointCloud extends Element
{
- ColoredPointList points;
+ Ice::ByteSeq points;
float transparency = 0.0f;
float pointSizeInPixels = 1.0f;
};
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index a29354a4374653f8b0ccfb0fc60f3783656acf1e..3c311d2249c3dc7ebe6baee5779c9337b6e19a01 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -71,10 +71,10 @@ namespace armarx::armem
std::vector<std::string> CommitResult::allErrorMessages() const
{
- return simox::alg::apply([](const EntityUpdateResult & res)
+ return simox::alg::apply(results, [](const EntityUpdateResult & res)
{
return res.errorMessage;
- }, results);
+ });
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
index dc84c257a0f08307cb30971390d717534c6db2f4..76b9ad73575e49d9e0d9fb33531bfdfe24173010 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
@@ -109,8 +109,12 @@ namespace armarx::armem::server::query_proc::base
const ProviderSegmentT& providerSegment,
const armem::query::data::CoreSegmentQuery& query) const
{
- ResultProviderSegmentT& added = result.addProviderSegment(providerSegment.name(), providerSegment.aronType());
- childProcessor.process(added, query.providerSegmentQueries, providerSegment);
+ ResultProviderSegmentT* child = result.findProviderSegment(providerSegment.name());
+ if (child == nullptr)
+ {
+ child = &result.addProviderSegment(providerSegment.name(), providerSegment.aronType());
+ }
+ childProcessor.process(*child, query.providerSegmentQueries, providerSegment);
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index 85d6326393bc6e4f97db4c8d4d1e26f07b8bd9b1..3fc3deecb6acbe1beb3a1817e67885a4919f4ff1 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -110,8 +110,12 @@ namespace armarx::armem::server::query_proc::base
const CoreSegmentT& coreSegment,
const armem::query::data::MemoryQuery& query) const
{
- ResultCoreSegmentT& added = result.addCoreSegment(coreSegment.name(), coreSegment.aronType());
- childProcessor.process(added, query.coreSegmentQueries, coreSegment);
+ ResultCoreSegmentT* child = result.findCoreSegment(coreSegment.name());
+ if (child == nullptr)
+ {
+ child = &result.addCoreSegment(coreSegment.name(), coreSegment.aronType());
+ }
+ childProcessor.process(*child, query.coreSegmentQueries, coreSegment);
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
index bc7f92031bc7c7f940c219d226908d072e9cfe22..7b138e9ac624b13c4bb8bbf11dcfb29ce2992f59 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
@@ -107,8 +107,12 @@ namespace armarx::armem::server::query_proc::base
const EntityT& entity,
const armem::query::data::ProviderSegmentQuery& query) const
{
- ResultEntityT& added = result.addEntity(entity.name());
- childProcessor.process(added, query.entityQueries, entity);
+ ResultEntityT* child = result.findEntity(entity.name());
+ if (child == nullptr)
+ {
+ child = &result.addEntity(entity.name());
+ }
+ childProcessor.process(*child, query.entityQueries, entity);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index e5945103cf4956fae90644b630cc61132624227b..68c0d5e7df911f23b06fcc582108bbf9f205e49c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -49,13 +49,13 @@ namespace armarx::armem::robot_state
bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
- VirtualRobot::RobotPtr
+ [[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
const armem::Time& timestamp,
const VirtualRobot::RobotIO::RobotDescription& loadMode =
VirtualRobot::RobotIO::RobotDescription::eStructure);
- VirtualRobot::RobotPtr
+ [[nodiscard]] VirtualRobot::RobotPtr
getSynchronizedRobot(const std::string& name,
const armem::Time& timestamp,
const VirtualRobot::RobotIO::RobotDescription& loadMode =