diff --git a/etc/cmake/Findsick_scan_base.cmake b/etc/cmake/Findsick_scan_base.cmake
index 391b22908e16b2808efbff5bdf5105d1bd2aada9..faef3317d146d5fb44bddda029cf17d85128e3f2 100644
--- a/etc/cmake/Findsick_scan_base.cmake
+++ b/etc/cmake/Findsick_scan_base.cmake
@@ -5,7 +5,12 @@
include(FindPackageHandleStandardArgs)
-message(STATUS ${sick_scan_base_DIR})
+
+if (DEFINED ENV{sick_scan_base_DIR})
+ if(NOT DEFINED sick_scan_base_DIR)
+ set(sick_scan_base_DIR $ENV{sick_scan_base_DIR})
+ endif()
+endif()
#if(NOT "$ENV{sick_scan_base_DIR}" EQUAL "")
# set(sick_scan_base_DIR $ENV{sick_scan_base_DIR} CACHE PATH "Path to sick_scan_base" FORCE)
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index 1fbb408ccc47d4aaea245b50a8b0da3adbd302cf..c49bc46f12ab45bcd034c21b92d1eb94e8c0e0b9 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -22,10 +22,6 @@
#include "ArVizStorage.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/IceBlobToObject.h>
#include <ArmarXCore/core/util/ObjectToIceBlob.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -33,7 +29,7 @@
#include <SimoxUtility/json/json.hpp>
#include <iomanip>
-
+#include <optional>
namespace armarx
{
@@ -137,7 +133,7 @@ namespace armarx
std::unique_lock<std::mutex> lock(historyMutex);
revision += 1;
- IceUtil::Time now = TimeUtil::GetTime();
+ IceUtil::Time now = IceUtil::Time::now();
long nowInMicroSeconds = now.toMicroSeconds();
for (auto& update : updates)
@@ -156,7 +152,7 @@ namespace armarx
{
layer = historyEntry;
found = true;
- continue;
+ break;
}
}
if (!found)
@@ -184,6 +180,90 @@ namespace armarx
}
}
+ viz::data::CommitResult ArVizStorage::commitAndReceiveInteractions(viz::data::CommitInput const& input, const Ice::Current&)
+ {
+ viz::data::CommitResult result;
+
+ {
+ std::unique_lock<std::mutex> lock(historyMutex);
+
+ revision += 1;
+ IceUtil::Time now = IceUtil::Time::now();
+ long nowInMicroSeconds = now.toMicroSeconds();
+
+ // Insert updates into the history and update the current state
+ for (viz::data::LayerUpdate const& update : input.updates)
+ {
+ viz::data::TimestampedLayerUpdate& historyEntry = history.emplace_back();
+ historyEntry.revision = revision;
+ historyEntry.timestampInMicroseconds = nowInMicroSeconds;
+ historyEntry.update = update;
+
+ // Insert or create the layer
+ bool found = false;
+ for (auto& layer : currentState)
+ {
+ if (layer.update.component == update.component
+ && layer.update.name == update.name)
+ {
+ layer = historyEntry;
+ found = true;
+ break;
+ }
+ }
+ if (!found)
+ {
+ currentState.push_back(historyEntry);
+ }
+ }
+
+ // Trim the history if max size has been exceeded
+ long currentHistorySize = history.size();
+ if (currentHistorySize >= maxHistorySize)
+ {
+ {
+ std::unique_lock<std::mutex> lock(recordingMutex);
+ if (recordingMetaData.id.size() > 0)
+ {
+ auto& newBatch = recordingBuffer.emplace_back();
+ newBatch.initialState = recordingInitialState;
+ newBatch.updates = std::move(history);
+ recordingInitialState = currentState;
+
+ recordingCondition.notify_one();
+ }
+ }
+ history.clear();
+ }
+
+ // Find relevant interactions
+ if (input.interactionComponent.size() > 0)
+ {
+ auto interactionsEnd = interactionBuffer.end();
+ auto foundInteractionsBegin = std::partition(interactionBuffer.begin(), interactionsEnd,
+ [&input](viz::data::InteractionFeedback const& interaction)
+ {
+ if (interaction.component == input.interactionComponent)
+ {
+ for (std::string const& layer : input.interactionLayers)
+ {
+ if (interaction.layer == layer)
+ {
+ return false;
+ }
+ }
+ }
+ return true;
+ });
+
+ result.interactions.assign(foundInteractionsBegin, interactionsEnd);
+ interactionBuffer.erase(foundInteractionsBegin, interactionsEnd);
+ }
+ }
+
+ return result;
+ }
+
viz::data::LayerUpdates ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
@@ -205,6 +285,46 @@ namespace armarx
return result;
}
+ viz::data::LayerUpdates ArVizStorage::pullUpdatesSinceAndSendInteractions(
+ Ice::Long revision, viz::data::InteractionFeedbackSeq const& interactions, const Ice::Current& c)
+ {
+ viz::data::LayerUpdates result;
+
+ std::unique_lock<std::mutex> lock(historyMutex);
+
+ for (auto& interaction : interactions)
+ {
+ for (auto& entry : interactionBuffer)
+ {
+ if (entry.component == interaction.component
+ && entry.layer == interaction.layer
+ && entry.element == interaction.element)
+ {
+ // Should we check whether this interaction is compatible / should be overwritten?
+ entry = interaction;
+ goto for_end;
+ }
+ }
+
+ // Interaction did not exist, add it to the buffer
+ interactionBuffer.push_back(interaction);
+
+ for_end: ;
+ }
+
+ result.updates.reserve(currentState.size());
+ for (auto& layer : currentState)
+ {
+ if (layer.revision > revision)
+ {
+ result.updates.push_back(layer.update);
+ }
+ }
+ result.revision = this->revision;
+
+ return result;
+ }
+
void ArVizStorage::record()
{
while (!recordingTask->isStopped())
@@ -340,17 +460,9 @@ void armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
return;
}
- const auto startT = TimeUtil::GetTime();
-
auto& first = batch.updates.front();
auto& last = batch.updates.back();
- ARMARX_ON_SCOPE_EXIT
- {
- ARMARX_INFO << "Storing batch " << first.revision << " - " << last.revision << " took "
- << (TimeUtil::GetTime() - startT).toSecondsDouble() << "s";
- };
-
batch.header.index = -1; // TODO: Should we save the index?
batch.header.firstRevision = first.revision;
batch.header.lastRevision = last.revision;
@@ -413,12 +525,11 @@ std::string armarx::ArVizStorage::startRecording(std::string const& newRecording
return recordingMetaData.id;
}
- auto t = std::time(nullptr);
- auto tm = *std::localtime(&t);
+ IceUtil::Time now = IceUtil::Time::now();
std::ostringstream id;
id << newRecordingPrefix
<< '_'
- << std::put_time(&tm, "%Y-%m-%d_%H-%M-%S");
+ << now.toString("%Y-%m-%d_%H-%M-%S");
std::string newRecordingID = id.str();
recordingPath = historyPath / newRecordingID;
@@ -500,12 +611,6 @@ armarx::viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice
armarx::viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID, Ice::Long batchIndex, const Ice::Current&)
{
- const auto startT = TimeUtil::GetTime();
- ARMARX_ON_SCOPE_EXIT
- {
- ARMARX_INFO << "Loading " << recordingID << " " << batchIndex << " took "
- << (TimeUtil::GetTime() - startT).toSecondsDouble() << "s";
- };
viz::data::RecordingBatch result;
result.header.index = -1;
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h
index 02ebc981525bdc26ce277867950e79bbed39d17a..f01a68fba62bd7f9fb961dcb182480774ec848de 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.h
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.h
@@ -22,8 +22,6 @@
#pragma once
-#include "Coin/Visualizer.h"
-
#include <RobotAPI/interface/ArViz.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
@@ -85,7 +83,13 @@ namespace armarx
void updateLayers(viz::data::LayerUpdateSeq const& updates, const Ice::Current&) override;
// StorageInterface interface
+ viz::data::CommitResult commitAndReceiveInteractions(viz::data::CommitInput const& input, const Ice::Current&) override;
+
viz::data::LayerUpdates pullUpdatesSince(Ice::Long revision, const Ice::Current&) override;
+ viz::data::LayerUpdates pullUpdatesSinceAndSendInteractions(
+ Ice::Long revision,
+ viz::data::InteractionFeedbackSeq const& interactions,
+ const Ice::Current&) override;
std::string startRecording(std::string const& prefix, const Ice::Current&) override;
void stopRecording(const Ice::Current&) override;
viz::data::RecordingSeq getAllRecordings(const Ice::Current&) override;
@@ -104,10 +108,14 @@ namespace armarx
std::mutex historyMutex;
std::vector<viz::data::TimestampedLayerUpdate> currentState;
- // We ignore the history for now
std::vector<viz::data::TimestampedLayerUpdate> history;
long revision = 0;
+ // We store all interactions in here
+ // But we curate them, so that only the last interaction with an element is reported
+ // in case the component never retrieves the interactions
+ std::vector<viz::data::InteractionFeedback> interactionBuffer;
+
std::mutex recordingMutex;
std::filesystem::path recordingPath;
std::vector<viz::data::TimestampedLayerUpdate> recordingInitialState;
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 9a5a3adcbfedd3a756b8231b61db7591ad66047b..de69ba9afe9cacec520233bcdaa0030c71749544 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -6,11 +6,11 @@ set(COMPONENT_LIBS
RobotAPIInterfaces
RobotAPIArmarXObjects
RobotStatechartHelpers # For RobotNameHelper, used by RobotHand
- boost_iostreams #compression
)
set(SOURCES
+ Client/Client.cpp
Client/Elements.cpp
Client/elements/Mesh.cpp
Client/elements/Robot.cpp
@@ -18,9 +18,53 @@ set(SOURCES
Client/elements/Line.cpp
Client/elements/Path.cpp
- Client/drawer/ArVizDrawerBase.cpp
Client/ScopedClient.cpp
+)
+
+set(HEADERS
+
+ IceConversions.h
+
+ # Client
+ Client/Layer.h
+ Client/Elements.h
+ Client/Client.h
+ Client/ScopedClient.h
+ Client/ClientCGALExtensions.h
+ Client/Color.h
+
+ Client/elements/Color.h
+ Client/elements/ElementOps.h
+ Client/elements/Mesh.h
+ Client/elements/MeshCGALExtensions.h
+ Client/elements/PointCloud.h
+ Client/elements/Robot.h
+ Client/elements/RobotHand.h
+ Client/elements/Line.h
+ Client/elements/Path.h
+
+ Client/elements/point_cloud_type_traits.hpp
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+add_library(RobotAPI::ArViz ALIAS ArViz)
+
+
+# ArViz Coin library
+
+armarx_component_set_name("ArVizCoin")
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPIArmarXObjects
+)
+
+set(SOURCES
+
Coin/ElementVisualizer.cpp
Coin/VisualizationRobot.cpp
@@ -29,6 +73,7 @@ set(SOURCES
Coin/Visualizer.cpp
Coin/RegisterVisualizationTypes.cpp
+ Coin/ExportVRML.cpp
Introspection/ElementJsonSerializers.cpp
Introspection/exceptions.cpp
@@ -42,8 +87,6 @@ set(SOURCES
set(HEADERS
- IceConversions.h
-
Coin/ElementVisualizer.h
# Inventor
@@ -66,28 +109,7 @@ set(HEADERS
Coin/VisualizationObject.h
Coin/Visualizer.h
-
- # Client
- Client/Layer.h
- Client/Elements.h
- Client/Client.h
- Client/ScopedClient.h
- Client/ClientCGALExtensions.h
- Client/Color.h
-
- Client/elements/Color.h
- Client/elements/ElementOps.h
- Client/elements/Mesh.h
- Client/elements/MeshCGALExtensions.h
- Client/elements/PointCloud.h
- Client/elements/Robot.h
- Client/elements/RobotHand.h
- Client/elements/Line.h
- Client/elements/Path.h
-
- Client/drawer/ArVizDrawerBase.h
-
- Client/elements/point_cloud_type_traits.hpp
+ Coin/ExportVRML.h
Introspection/ElementJsonSerializers.h
Introspection/exceptions.h
@@ -101,22 +123,22 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
-add_library(RobotAPI::ArViz ALIAS ArViz)
+add_library(RobotAPI::ArVizCoin ALIAS ArVizCoin)
armarx_component_set_name("ArVizStorage")
set(COMPONENT_LIBS
ArmarXCore
- ArViz
+ RobotAPIInterfaces
)
set(SOURCES
-ArVizStorage.cpp
-ArVizStorageMain.cpp
+ ArVizStorage.cpp
+ ArVizStorageMain.cpp
)
set(HEADERS
-ArVizStorage.h
+ ArVizStorage.h
)
armarx_add_component_executable("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/ArViz/Client/Client.cpp b/source/RobotAPI/components/ArViz/Client/Client.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e6bcf38cf777e924dab5be2af0e0c86890f9427
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/Client.cpp
@@ -0,0 +1,100 @@
+#include "Client.h"
+
+#include <ArmarXCore/core/Component.h>
+
+namespace armarx::viz
+{
+
+Client::Client(Component& component, std::string const& topicNameProperty, std::string const& storageNameProperty)
+{
+ componentName = component.getName();
+ component.getTopicFromProperty(topic, topicNameProperty);
+
+ // Optional dependency on ArVizStorage
+ std::string storageName;
+ if (component.hasProperty(storageNameProperty))
+ {
+ storageName = component.getProperty<std::string>(storageNameProperty);
+ }
+ else
+ {
+ storageName = "ArVizStorage";
+ }
+ component.getProxy(storage, storageName);
+}
+
+Client::Client(ManagedIceObject& obj,
+ std::string const& topicName,
+ std::string const& storageName)
+{
+ componentName = obj.getName();
+ obj.getTopic(topic, topicName);
+ obj.getProxy(storage, storageName);
+}
+
+Client Client::createFromTopic(std::string const& componentName, Topic::ProxyType const& topic)
+{
+ Client client;
+ client.componentName = componentName;
+ client.topic = topic;
+ return client;
+}
+
+Client Client::createFromProxies(std::string const& componentName,
+ Topic::ProxyType const& topic,
+ StorageAndTopicInterfacePrx const& storage)
+{
+ Client client;
+ client.componentName = componentName;
+ client.topic = topic;
+ client.storage = storage;
+ return client;
+}
+
+Client Client::createForGuiPlugin(Component& component,
+ std::string const& topicName,
+ std::string const& storageName)
+{
+ Client client;
+ std::string name = component.getName();
+ std::size_t dashPos = name.find('-');
+ if (dashPos != std::string::npos)
+ {
+ name = name.substr(0, dashPos);
+ }
+ client.componentName = name;
+ component.getTopic(client.topic, topicName);
+ component.getProxy(client.storage, storageName);
+ return client;
+}
+
+CommitResult Client::commit(const StagedCommit& commit)
+{
+ CommitResult result;
+ result.data_ = storage->commitAndReceiveInteractions(commit.data_);
+ return result;
+}
+
+CommitResultAsync Client::commitAsync(const StagedCommit& commit)
+{
+ CommitResultAsync result;
+ result.async = storage->begin_commitAndReceiveInteractions(commit.data_);
+ result.storage = storage;
+ return result;
+}
+
+void Client::commit(const std::vector<Layer>& layers)
+{
+ data::LayerUpdateSeq updates;
+ updates.reserve(layers.size());
+ for (Layer const& layer : layers)
+ {
+ updates.push_back(layer.data_);
+ }
+ // This commit call still uses the legacy topic API
+ topic->updateLayers(updates);
+}
+
+
+
+}
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index f946e02d4298910c72bb4d8a59bf9d64d42ac5ab..5500e774720824ad5aa72de8c76ab3f3db25d7de 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -5,159 +5,356 @@
#include "Layer.h"
-#include <ArmarXCore/core/Component.h>
-
-namespace armarx::viz
+namespace armarx
{
+ class Component;
+ class ManagedIceObject;
- class Client
+namespace viz
+{
+ /**
+ * A staged commit prepares multiple layers to be committed.
+ *
+ * Add all relevant layer updates via .add(layer).
+ * Add layers for which you want interaction feedback via .requestInteraction(layer).
+ * Then, commit via client.apply(stagedCommit).
+ *
+ * A staged commit can be reused for subsequent commits.
+ * Remember to call .reset() to clear the internal data structures.
+ *
+ * As long as you keep the staged commits separate, this method is thread-safe.
+ * So you can have two threads calling .commit(A) and .commit(B)
+ * simultaneously without any locking mechanism.
+ */
+ struct StagedCommit
{
- public:
- Client() = default;
- virtual ~Client() = default;
+ /**
+ * @brief Stage a layer to be committed later via client.apply(*this)
+ * @param layer The layer to be added to this staged commit.
+ */
+ void add(Layer const& layer)
+ {
+ data_.updates.push_back(layer.data_);
+ }
- Client(armarx::Component& component, std::string topicNameProperty = "ArVizTopicName")
+ void add(std::initializer_list<Layer> layers)
{
- componentName = component.getName();
- component.getTopicFromProperty(_topic, topicNameProperty);
+ data_.updates.reserve(data_.updates.size() + layers.size());
+ for (Layer const& layer : layers)
+ {
+ data_.updates.push_back(layer.data_);
+ }
}
- Client(ManagedIceObject& obj, const std::string& topicName = "ArVizTopic")
+ /**
+ * @brief Request interaction feedback for a particular layer.
+ * @param layer The layer you want to get interaction feedback for.
+ */
+ void requestInteraction(Layer const& layer)
{
- componentName = obj.getName();
- obj.getTopic(_topic, topicName);
+ data_.interactionComponent = layer.data_.component;
+ data_.interactionLayers.push_back(layer.data_.name);
}
- static Client createFromTopic(const std::string arvizNamespace, armarx::viz::Topic::ProxyType topic)
+ /**
+ * @brief Reset all staged layers and interaction requests.
+ */
+ void reset()
{
- Client client;
- client.componentName = arvizNamespace;
- client._topic = topic;
- return client;
+ data_.updates.clear();
+ data_.interactionComponent.clear();
+ data_.interactionLayers.clear();
}
- static Client createForGuiPlugin(armarx::Component& component, std::string const& topicName = "ArVizTopic")
+ viz::data::CommitInput data_;
+ };
+
+ enum class InteractionFeedbackType
+ {
+ /** Nothing happened. */
+ None,
+
+ /** An element was selected. */
+ Select,
+ /** An element was deselected. */
+ Deselect,
+
+ /** A context menu entry was chosen. */
+ ContextMenuChosen,
+
+ /** The element was transformed (translated or rotated). */
+ Transform,
+ };
+
+ inline const char* toString(InteractionFeedbackType type)
+ {
+ switch (type)
{
- Client client;
- std::string name = component.getName();
- std::size_t dashPos = name.find('-');
- if (dashPos != std::string::npos)
+ case InteractionFeedbackType::None:
+ return "None";
+ case InteractionFeedbackType::Select:
+ return "Select";
+ case InteractionFeedbackType::Deselect:
+ return "Deselect";
+ case InteractionFeedbackType::ContextMenuChosen:
+ return "ContextMenuChosen";
+ case InteractionFeedbackType::Transform:
+ return "Transform";
+ default:
+ return "<Unknown>";
+ }
+ }
+
+ inline Eigen::Matrix4f toEigen(data::GlobalPose const& pose)
+ {
+ Eigen::Quaternionf ori(pose.qw, pose.qx, pose.qy, pose.qz);
+
+ Eigen::Matrix4f result;
+ result.block<3, 3>(0, 0) = ori.toRotationMatrix();
+ result.block<3, 1>(0, 3) = Eigen::Vector3f(pose.x, pose.y, pose.z);
+ result.block<1, 4>(3, 0) = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f);
+ return result;
+ }
+
+ struct InteractionFeedback
+ {
+ InteractionFeedbackType type() const
+ {
+ // Maks out all the flags in the higher bits
+ int type = data_.type & 0x7;
+
+ switch (type)
{
- name = name.substr(0, dashPos);
+ case data::InteractionFeedbackType::NONE:
+ return InteractionFeedbackType::None;
+
+ case data::InteractionFeedbackType::SELECT:
+ return InteractionFeedbackType::Select;
+
+ case data::InteractionFeedbackType::DESELECT:
+ return InteractionFeedbackType::Deselect;
+
+ case data::InteractionFeedbackType::CONTEXT_MENU_CHOSEN:
+ return InteractionFeedbackType::ContextMenuChosen;
+
+ case data::InteractionFeedbackType::TRANSFORM:
+ return InteractionFeedbackType::Transform;
+
+ default:
+ throw std::runtime_error("Unexpected InteractionFeedbackType");
}
- client.componentName = name;
- component.getTopic(client._topic, topicName);
- return client;
}
- // ////////////////////////////////////////////////////////////////// //
- //layer
- virtual Layer layer(std::string const& name) const
+ bool isTranslation() const
{
- return Layer(componentName, name);
+ return data_.type & data::InteractionFeedbackType::TRANSLATION_FLAG;
}
- template<class...Ts>
- Layer layerContaining(std::string const& name, Ts&& ...elems) const
+ bool isRotation() const
{
- auto l = layer(name);
- l.add(std::forward<Ts>(elems)...);
- return l;
+ return data_.type & data::InteractionFeedbackType::ROTATION_FLAG;
}
- // ////////////////////////////////////////////////////////////////// //
- //enqueue
- void enqueueLayer(const Layer& l)
+
+ bool isAxisX() const
{
- updates.emplace_back(l.data_);
+ return data_.type & data::InteractionFeedbackType::AXIS_X_FLAG;
}
- void enqueueLayer(const Layer* l)
+
+ bool isAxisY() const
{
- updates.emplace_back(l->data_);
+ return data_.type & data::InteractionFeedbackType::AXIS_Y_FLAG;
}
- void enqueueLayer(std::initializer_list<Layer> const& layers)
+
+ bool isAxisZ() const
{
- for (const auto& l : layers)
- {
- enqueueLayer(l);
- }
+ return data_.type & data::InteractionFeedbackType::AXIS_Z_FLAG;
}
- void enqueueLayer(const std::vector<const Layer*>& layers)
+
+ bool isTransformBegin() const
{
- for (const auto& l : layers)
- {
- enqueueLayer(l);
- }
+ return data_.type & data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG;
}
- void enqueueLayer(const std::vector<Layer>& layers)
+
+ bool isTransformDuring() const
{
- for (const auto& l : layers)
- {
- enqueueLayer(l);
- }
+ return data_.type & data::InteractionFeedbackType::TRANSFORM_DURING_FLAG;
}
- template<class...Ts>
- void enqueueLayerContaining(std::string const& name, Ts&& ...elems)
+
+ bool isTransformEnd() const
{
- enqueueLayer(layerContaining(name, std::forward<Ts>(elems)...));
+ return data_.type & data::InteractionFeedbackType::TRANSFORM_END_FLAG;
}
- void enqueueDeleteLayer(std::string const& name)
+
+ std::string const& layer() const
{
- auto l = layer(name);
- l.data_.action = data::LayerAction::Layer_DELETE;
- enqueueLayer(std::move(l));
+ return data_.layer;
}
- // ////////////////////////////////////////////////////////////////// //
- //commit
- void commit(std::initializer_list<Layer> const& layers)
+ std::string const& element() const
{
- enqueueLayer(layers);
- commit();
+ return data_.element;
}
- void commit(const std::vector<const Layer*>& layers)
+ long revision() const
{
- enqueueLayer(layers);
- commit();
+ return data_.revision;
}
- void commit(const std::vector<Layer>& layers)
+ int chosenContextMenuEntry() const
{
- enqueueLayer(layers);
- commit();
+ return data_.chosenContextMenuEntry;
}
- template<class...Ts>
- void
- commit(Ts&& ...ts)
+ Eigen::Matrix4f originalPose() const
{
- (enqueueLayer(std::forward<Ts>(ts)), ...);
- _topic->updateLayers(updates);
- updates.clear();
+ return toEigen(data_.originalPose);
}
- template<class...Ts>
- void commitLayerContaining(std::string const& name, Ts&& ...elems)
+ Eigen::Matrix4f chosenPose() const
{
- commit(layerContaining(name, std::forward<Ts>(elems)...));
+ return toEigen(data_.chosenPose);
}
- void commitDeleteLayer(std::string const& name)
+ data::InteractionFeedback data_;
+ };
+
+ struct InteractionFeedbackRange
+ {
+ InteractionFeedback const* begin() const
{
- enqueueDeleteLayer(name);
- commit();
+ return begin_;
}
- const armarx::viz::TopicPrx& topic() const
+ InteractionFeedback const* end() const
{
- return _topic;
+ return end_;
}
+
+ std::size_t size() const
+ {
+ return end_ - begin_;
+ }
+
+ bool empty() const
+ {
+ return begin_ == end_;
+ }
+
+ InteractionFeedback const* begin_;
+ InteractionFeedback const* end_;
+ };
+
+ struct CommitResult
+ {
+ long revision() const
+ {
+ return data_.revision;
+ }
+
+ InteractionFeedbackRange interactions() const
+ {
+ InteractionFeedback* begin = (InteractionFeedback*) data_.interactions.data();
+ InteractionFeedback* end = begin + data_.interactions.size();
+ return InteractionFeedbackRange{begin, end};
+ }
+
+ data::CommitResult data_;
+ };
+
+ struct CommitResultAsync
+ {
+ bool isAvailable() const
+ {
+ return async && async->isCompleted();
+ }
+
+ CommitResult waitAndGet() const
+ {
+ CommitResult result;
+ result.data_ = storage->end_commitAndReceiveInteractions(async);
+ return result;
+ }
+
+ Ice::AsyncResultPtr async;
+ armarx::viz::StorageInterfacePrx storage;
+ };
+
+ struct Client
+ {
+ Client() = default;
+
+ Client(armarx::Component& component,
+ std::string const& topicNameProperty = "ArVizTopicName",
+ std::string const& storageNameProperty = "ArVizStorageName");
+
+ Client(ManagedIceObject& obj,
+ std::string const& topicName = "ArVizTopic",
+ std::string const& storageName = "ArVizStorage");
+
+ static Client createFromTopic(std::string const& componentName,
+ armarx::viz::Topic::ProxyType const& topic);
+
+ static Client createFromProxies(std::string const& componentName,
+ armarx::viz::Topic::ProxyType const& topic,
+ armarx::viz::StorageAndTopicInterfacePrx const& storage);
+
+ static Client createForGuiPlugin(armarx::Component& component,
+ std::string const& topicName = "ArVizTopic",
+ std::string const& storageName = "ArVizStorage");
+
+ Layer layer(std::string const& name) const
+ {
+ return Layer(componentName, name);
+ }
+
+ StagedCommit stage()
+ {
+ return StagedCommit();
+ }
+
+ CommitResult commit(StagedCommit const& commit);
+
+ CommitResultAsync commitAsync(StagedCommit const& commit);
+
+ void commit(Layer const& layer)
+ {
+ std::vector<Layer> layers;
+ layers.push_back(layer);
+ commit(layers);
+ }
+
+ void commit(std::vector<Layer> const& layers);
+
+ void commitLayerContaining(std::string const& name)
+ {
+ std::vector<viz::Layer> layers;
+ layers.push_back(this->layer(name));
+ commit(layers);
+ }
+
+ template <typename ElementT>
+ void commitLayerContaining(std::string const& name, ElementT const& element)
+ {
+ std::vector<viz::Layer> layers;
+ viz::Layer& newLayer = layers.emplace_back(this->layer(name));
+ newLayer.add(element);
+ commit(layers);
+ }
+
+ void commitDeleteLayer(std::string const& name)
+ {
+ std::vector<viz::Layer> layers;
+ viz::Layer& layerToDelete = layers.emplace_back(this->layer(name));
+ layerToDelete.markForDeletion();
+ commit(layers);
+ }
+
private:
std::string componentName;
- armarx::viz::TopicPrx _topic;
-
- data::LayerUpdateSeq updates;
+ armarx::viz::StorageInterfacePrx storage;
+ armarx::viz::TopicPrx topic;
};
}
+}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index daec8bc655e0f6431dc531f0401478838e7f053b..f479f4ef5456fc7a88d0f3a0fa87d2e77e9aab66 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -4,10 +4,43 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <SimoxUtility/math/normal/normal_to_mat4.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/pose/transform.h>
+
namespace armarx::viz
{
+ struct Convert
+ {
+ static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
+ {
+ dir = dir.normalized();
+ Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
+ Eigen::Vector3f cross = naturalDir.cross(dir);
+ float dot = naturalDir.dot(dir);
+ float angle = std::acos(dot);
+ if (cross.squaredNorm() < 1.0e-12)
+ {
+ // Directions are almost colinear ==> Do no rotation
+ cross = Eigen::Vector3f::UnitX();
+ if (dot < 0)
+ {
+ angle = M_PI;
+ }
+ else
+ {
+ angle = 0.0f;
+ }
+ }
+ Eigen::Vector3f axis = cross.normalized();
+ Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
+
+ return ori;
+ }
+ };
+
const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
@@ -33,6 +66,110 @@ namespace armarx::viz
}
return *this;
}
+
+ Box& Box::set(const simox::OrientedBoxBase<float>& b)
+ {
+ size(b.dimensions());
+ return pose(b.transformation_centered());
+ }
+
+ Box& Box::set(const simox::OrientedBoxBase<double>& b)
+ {
+ size(b.dimensions<float>());
+ return pose(b.transformation_centered<float>());
+ }
+
+ Cylinder& Cylinder::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ {
+ position((to + from) / 2);
+ orientation(Convert::directionToQuaternion((to - from).normalized()));
+ height((to - from).norm());
+
+ return *this;
+ }
+
+ Cylinder& Cylinder::direction(Eigen::Vector3f direction)
+ {
+ orientation(Convert::directionToQuaternion(direction));
+
+ return *this;
+ }
+
+ Arrow& Arrow::direction(Eigen::Vector3f dir)
+ {
+ return orientation(Convert::directionToQuaternion(dir));
+ }
+
+ ArrowCircle& ArrowCircle::normal(Eigen::Vector3f dir)
+ {
+ Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitZ();
+ Eigen::Vector3f cross = naturalDir.cross(dir);
+ float angle = std::acos(naturalDir.dot(dir));
+ if (cross.squaredNorm() < 1.0e-12f)
+ {
+ // Directions are almost colinear ==> Do no rotation
+ cross = Eigen::Vector3f::UnitX();
+ angle = 0.0f;
+ }
+ Eigen::Vector3f axis = cross.normalized();
+ Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
+
+ return orientation(ori);
+ }
+
+ Polygon& Polygon::points(const std::vector<Eigen::Vector3f>& ps)
+ {
+ auto& points = data_->points;
+ points.clear();
+ points.reserve(ps.size());
+ for (auto& p : ps)
+ {
+ points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+ }
+
+ return *this;
+ }
+
+ Polygon& Polygon::plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
+ {
+ const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
+ Eigen::Vector3f::UnitZ(), plane.normal());
+ return this->plane(plane.projection(at), ori, size);
+ }
+
+ Polygon& Polygon::plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
+ {
+ const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
+ const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
+
+ addPoint(center + x + y);
+ addPoint(center - x + y);
+ addPoint(center - x - y);
+ addPoint(center + x - y);
+
+ return *this;
+ }
+
+ Polygon& Polygon::circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation)
+ {
+ const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
+ ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
+
+ const float angle = 2 * M_PI / tessellation;
+ const Eigen::Matrix3f rot = simox::math::rpy_to_mat3f(0, 0, angle);
+
+ Eigen::Vector3f lastLocalPoint = Eigen::Vector3f::UnitX() * radius;
+ addPoint(simox::math::transform_position(pose, lastLocalPoint));
+ while (--tessellation)
+ {
+ const Eigen::Vector3f localPoint = rot * lastLocalPoint;
+ addPoint(simox::math::transform_position(pose, localPoint));
+ lastLocalPoint = localPoint;
+ }
+ return *this;
+ }
+
+
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 7b65e66ff7b273110366efa1da21ea7eff3412d5..af08e951c9c20b0bd05baad85b14679627cc2e7e 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -5,7 +5,7 @@
-#include "Color.h"
+#include "elements/Color.h"
#include "elements/ElementOps.h"
#include "elements/Mesh.h"
#include "elements/PointCloud.h"
@@ -14,11 +14,6 @@
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <SimoxUtility/math/normal/normal_to_mat4.h>
-#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
-#include <SimoxUtility/math/pose/transform.h>
#include <SimoxUtility/shapes/OrientedBoxBase.h>
#include <Eigen/Geometry>
@@ -51,41 +46,8 @@ namespace armarx::viz
{
using data::ColoredPoint;
- struct Convert
+ struct Box : ElementOps<Box, data::ElementBox>
{
- static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
- {
- dir = dir.normalized();
- Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
- Eigen::Vector3f cross = naturalDir.cross(dir);
- float dot = naturalDir.dot(dir);
- float angle = std::acos(dot);
- if (cross.squaredNorm() < 1.0e-12)
- {
- // Directions are almost colinear ==> Do no rotation
- cross = Eigen::Vector3f::UnitX();
- if (dot < 0)
- {
- angle = M_PI;
- }
- else
- {
- angle = 0.0f;
- }
- }
- Eigen::Vector3f axis = cross.normalized();
- Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
-
- return ori;
- }
- };
-
-
- // Move the ice datatypes into their own namespace
-
- class Box : public ElementOps<Box, data::ElementBox>
- {
- public:
using ElementOps::ElementOps;
Box& size(Eigen::Vector3f const& s)
@@ -102,44 +64,13 @@ namespace armarx::viz
return size(Eigen::Vector3f(s, s, s));
}
- Box& set(const simox::OrientedBoxBase<float>& b)
- {
- size(b.dimensions());
- return pose(b.transformation_centered());
- }
- Box& set(const simox::OrientedBoxBase<double>& b)
- {
- size(b.dimensions<float>());
- return pose(b.transformation_centered<float>());
- }
-
- template<class T>
- Box(const std::string& name, const simox::OrientedBoxBase<T>& b)
- : ElementOps(name)
- {
- set(b);
- }
-
- template<class T>
- Box(
- const simox::OrientedBoxBase<T>& b,
- const std::string& name = std::to_string(std::time(0))
- )
- : Box(name, b)
- {}
-
- Box(std::string const& id)
- : ElementOps(id)
- {
- pose(Eigen::Matrix4f::Identity());
- }
-
+ Box& set(const simox::OrientedBoxBase<float>& b);
+ Box& set(const simox::OrientedBoxBase<double>& b);
};
- class Cylinder : public ElementOps<Cylinder, data::ElementCylinder>
+ struct Cylinder : ElementOps<Cylinder, data::ElementCylinder>
{
- public:
using ElementOps::ElementOps;
Cylinder& radius(float r)
@@ -156,20 +87,14 @@ namespace armarx::viz
return *this;
}
- Cylinder& fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
- {
- position((to + from) / 2);
- orientation(Convert::directionToQuaternion((to - from).normalized()));
- height((to - from).norm());
+ Cylinder& fromTo(Eigen::Vector3f from, Eigen::Vector3f to);
- return *this;
- }
+ Cylinder& direction(Eigen::Vector3f direction);
};
- class Cylindroid : public ElementOps<Cylindroid, data::ElementCylindroid>
+ struct Cylindroid : ElementOps<Cylindroid, data::ElementCylindroid>
{
- public:
Cylindroid(const std::string& name) :
ElementOps(name)
{
@@ -204,9 +129,8 @@ namespace armarx::viz
};
- class Sphere : public ElementOps<Sphere, data::ElementSphere>
+ struct Sphere : ElementOps<Sphere, data::ElementSphere>
{
- public:
using ElementOps::ElementOps;
Sphere& radius(float r)
@@ -218,9 +142,8 @@ namespace armarx::viz
};
- class Ellipsoid : public ElementOps<Ellipsoid, data::ElementEllipsoid>
+ struct Ellipsoid : ElementOps<Ellipsoid, data::ElementEllipsoid>
{
- public:
Ellipsoid(const std::string& name) :
ElementOps(name)
{
@@ -251,16 +174,14 @@ namespace armarx::viz
};
- class Pose : public ElementOps<Pose, data::ElementPose>
+ struct Pose : ElementOps<Pose, data::ElementPose>
{
- public:
using ElementOps::ElementOps;
};
- class Text : public ElementOps<Text, data::ElementText>
+ struct Text : ElementOps<Text, data::ElementText>
{
- public:
using ElementOps::ElementOps;
Text& text(std::string const& t)
@@ -272,15 +193,11 @@ namespace armarx::viz
};
- class Arrow : public ElementOps<Arrow, data::ElementArrow>
+ struct Arrow : ElementOps<Arrow, data::ElementArrow>
{
- public:
using ElementOps::ElementOps;
- Arrow& direction(Eigen::Vector3f dir)
- {
- return orientation(Convert::directionToQuaternion(dir));
- }
+ Arrow& direction(Eigen::Vector3f dir);
Arrow& length(float l)
{
@@ -307,27 +224,11 @@ namespace armarx::viz
};
- class ArrowCircle : public ElementOps<ArrowCircle, data::ElementArrowCircle>
+ struct ArrowCircle : ElementOps<ArrowCircle, data::ElementArrowCircle>
{
- public:
using ElementOps::ElementOps;
- ArrowCircle& normal(Eigen::Vector3f dir)
- {
- Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitZ();
- Eigen::Vector3f cross = naturalDir.cross(dir);
- float angle = std::acos(naturalDir.dot(dir));
- if (cross.squaredNorm() < 1.0e-12f)
- {
- // Directions are almost colinear ==> Do no rotation
- cross = Eigen::Vector3f::UnitX();
- angle = 0.0f;
- }
- Eigen::Vector3f axis = cross.normalized();
- Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
-
- return orientation(ori);
- }
+ ArrowCircle& normal(Eigen::Vector3f dir);
ArrowCircle& radius(float r)
{
@@ -336,7 +237,6 @@ namespace armarx::viz
return *this;
}
-
ArrowCircle& width(float w)
{
data_->width = w;
@@ -353,9 +253,8 @@ namespace armarx::viz
};
- class Polygon : public ElementOps<Polygon, data::ElementPolygon>
+ struct Polygon : ElementOps<Polygon, data::ElementPolygon>
{
- public:
using ElementOps::ElementOps;
Polygon& clear()
@@ -371,10 +270,9 @@ namespace armarx::viz
return *this;
}
- template<class...Ts>
- Polygon& lineColor(Ts&& ...ts)
+ Polygon& lineColor(int r, int g, int b)
{
- return lineColor({std::forward<Ts>(ts)...});
+ return lineColor(viz::Color(r, g, b));
}
Polygon& lineColorGlasbeyLUT(std::size_t id, int alpha = 255)
@@ -389,18 +287,7 @@ namespace armarx::viz
return *this;
}
- Polygon& points(std::vector<Eigen::Vector3f> const& ps)
- {
- auto& points = data_->points;
- points.clear();
- points.reserve(ps.size());
- for (auto& p : ps)
- {
- points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
- }
-
- return *this;
- }
+ Polygon& points(std::vector<Eigen::Vector3f> const& ps);
Polygon& addPoint(Eigen::Vector3f p)
{
@@ -415,12 +302,7 @@ namespace armarx::viz
* @param at Center of rectangle, is projected onto plane.
* @param size Extents of rectangle.
*/
- Polygon& plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
- {
- const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
- Eigen::Vector3f::UnitZ(), plane.normal());
- return this->plane(plane.projection(at), ori, size);
- }
+ Polygon& plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size);
/**
* @brief Add points representing the XY-plane of the given coordinate system as rectangle.
@@ -428,47 +310,17 @@ namespace armarx::viz
* @param orientation The orientation of the coordinate system.
* @param size The XY-size of the rectangle.
*/
- Polygon& plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
- {
- const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
- const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
-
- addPoint(center + x + y);
- addPoint(center - x + y);
- addPoint(center - x - y);
- addPoint(center + x - y);
-
- return *this;
- }
+ Polygon& plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size);
- Polygon& circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation = 64)
- {
- const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
- ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
-
- const float angle = 2 * M_PI / tessellation;
- const Eigen::Matrix3f rot = simox::math::rpy_to_mat3f(0, 0, angle);
-
- Eigen::Vector3f lastLocalPoint = Eigen::Vector3f::UnitX() * radius;
- addPoint(simox::math::transform_position(pose, lastLocalPoint));
- while (--tessellation)
- {
- const Eigen::Vector3f localPoint = rot * lastLocalPoint;
- addPoint(simox::math::transform_position(pose, localPoint));
- lastLocalPoint = localPoint;
- }
- return *this;
- }
+ Polygon& circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation = 64);
};
- class Object : public ElementOps<Object, data::ElementObject>
+ struct Object : ElementOps<Object, data::ElementObject>
{
- private:
static const std::string DefaultObjectsPackage;
static const std::string DefaultRelativeObjectsDirectory;
- public:
using ElementOps::ElementOps;
Object& file(std::string const& project, std::string const& filename)
diff --git a/source/RobotAPI/components/ArViz/Client/Layer.h b/source/RobotAPI/components/ArViz/Client/Layer.h
index 27e056d78eb767637ab9b7a96951c01e3035e902..ef9b0fef855eb89f9151717694a1a2c33517d108 100644
--- a/source/RobotAPI/components/ArViz/Client/Layer.h
+++ b/source/RobotAPI/components/ArViz/Client/Layer.h
@@ -31,19 +31,18 @@ namespace armarx::viz
data_.elements.push_back(element.data_);
}
- template <typename ElementT>
- void add(std::vector<ElementT> const& elements)
- {
- for (const auto& e : elements)
- {
- add(e);
- }
- }
-
- template<class...Ts>
- std::enable_if_t < sizeof...(Ts) != 1 > add(Ts&& ...elems)
+// template <typename ElementT>
+// void add(std::vector<ElementT> const& elements)
+// {
+// for (ElementT const& e : elements)
+// {
+// add(e);
+// }
+// }
+
+ void markForDeletion()
{
- (add(std::forward<Ts>(elems)), ...);
+ data_.action = data::LayerAction::Layer_DELETE;
}
std::size_t size() const noexcept
diff --git a/source/RobotAPI/components/ArViz/Client/ScopedClient.h b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
index 4a799efda714e8850f23e3cac7ecaad995c28119..28969ed9fed88eb4ec3fc36ad271ac496dd9b9e7 100644
--- a/source/RobotAPI/components/ArViz/Client/ScopedClient.h
+++ b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
@@ -45,11 +45,11 @@ namespace armarx::viz
using Client::Client;
ScopedClient(const Client& client);
- Layer layer(std::string const& name) const override;
+ Layer layer(std::string const& name) const;
virtual ~ScopedClient();
private:
mutable std::set<std::string> layers;
};
-} // namespace armarx::viz
\ No newline at end of file
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
deleted file mode 100644
index 9d2c8e7ed36b6a494556ecdd55e652aea93b7e77..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "ArVizDrawerBase.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-#include "RobotAPI/components/ArViz/Client/Layer.h"
-
-namespace armarx
-{
-
- ArVizDrawerBase::ArVizDrawerBase(armarx::viz::Client& arviz) : arvizClient(arviz) {}
-
- ArVizDrawerBase::~ArVizDrawerBase() = default;
-
- viz::ScopedClient& ArVizDrawerBase::arviz()
- {
- return arvizClient;
- }
-
- const viz::ScopedClient& ArVizDrawerBase::arviz() const
- {
- return arvizClient;
- }
-
- void ArVizDrawerBase::commit(const viz::Layer& layer)
- {
- std::lock_guard guard{layerMtx};
- arvizClient.commit(layer);
- }
-
- void ArVizDrawerBase::commit(const std::vector<viz::Layer>& layers)
- {
- std::lock_guard guard{layerMtx};
- arvizClient.commit(layers);
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
deleted file mode 100644
index e302acc5c27e025aac9c103793f7a5ed726d76d9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * 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/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <mutex>
-#include <vector>
-
-#include "RobotAPI/components/ArViz/Client/ScopedClient.h"
-
-namespace armarx
-{
- // forward declaration
- namespace viz
- {
- class Client;
- struct Layer;
- } // namespace viz
-
- /**
- * @brief The ArVizDrawerBase class. Use this class to draw arbitrary objects.
- *
- * Instead of implementing arviz drawing in the component itself, this class helps to
- * decouple drawing from "component / processing logic".
- *
- */
- class ArVizDrawerBase
- {
- public:
- ArVizDrawerBase(armarx::viz::Client& arviz);
- virtual ~ArVizDrawerBase();
-
- protected:
- viz::ScopedClient& arviz();
- const viz::ScopedClient& arviz() const;
-
- void commit(const viz::Layer& layer);
- void commit(const std::vector<viz::Layer>& layers);
-
- private:
- viz::ScopedClient arvizClient;
-
- std::mutex layerMtx;
- };
-
-} // namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Color.h b/source/RobotAPI/components/ArViz/Client/elements/Color.h
index 4abfe40dd7daa9d2d39af686f9a5e116a797751c..272e25b44cfb6a2452d3accef676d970654fe2c0 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Color.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Color.h
@@ -1,10 +1,10 @@
#pragma once
-#include <Eigen/Core>
+#include <RobotAPI/interface/ArViz/Elements.h>
#include <SimoxUtility/color/Color.h>
-#include <RobotAPI/interface/ArViz/Elements.h>
+#include <Eigen/Core>
namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index b763c4301703bd83fe3af4b06a8fa9d96e4b6b1d..346130bbad0df4b67dfe4c15584b0c67200cfd60 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -1,24 +1,90 @@
#pragma once
-#pragma once
+#include "Color.h"
+
+#include <RobotAPI/interface/ArViz/Elements.h>
#include <SimoxUtility/color/GlasbeyLUT.h>
#include <SimoxUtility/math/convert/rpy_to_quat.h>
-#include <RobotAPI/interface/ArViz/Elements.h>
-
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <string>
-#include "Color.h"
-
namespace armarx::viz
{
using data::ColoredPoint;
+ struct InteractionDescription
+ {
+ using Self = InteractionDescription;
+
+ Self& none()
+ {
+ data_.enableFlags = 0;
+ return *this;
+ }
+
+ Self& selection()
+ {
+ data_.enableFlags |= data::InteractionEnableFlags::SELECT;
+ return *this;
+ }
+
+ Self& contextMenu(std::vector<std::string> const& options)
+ {
+ data_.enableFlags |= data::InteractionEnableFlags::CONTEXT_MENU;
+ data_.contextMenuOptions = options;
+ // Context menu (right click) implies selection
+ return selection();
+ }
+
+ Self& translation(bool x = true, bool y = true, bool z = true)
+ {
+ data_.enableFlags |= (x ? data::InteractionEnableFlags::TRANSLATION_X : 0);
+ data_.enableFlags |= (y ? data::InteractionEnableFlags::TRANSLATION_Y : 0);
+ data_.enableFlags |= (z ? data::InteractionEnableFlags::TRANSLATION_Z : 0);
+ // Translation implies selection
+ return selection();
+ }
+
+ Self& rotation(bool x = true, bool y = true, bool z = true)
+ {
+ data_.enableFlags |= (x ? data::InteractionEnableFlags::ROTATION_X : 0);
+ data_.enableFlags |= (y ? data::InteractionEnableFlags::ROTATION_Y : 0);
+ data_.enableFlags |= (z ? data::InteractionEnableFlags::ROTATION_Z : 0);
+ // Rotation implies selection
+ return selection();
+ }
+
+ Self& globalAxes(bool global = true)
+ {
+ if (global)
+ {
+ data_.enableFlags |= data::InteractionEnableFlags::GLOBAL_AXES;
+ }
+ else
+ {
+ data_.enableFlags &= ~data::InteractionEnableFlags::GLOBAL_AXES;
+ }
+ return *this;
+ }
+
+ Self& fullTransform()
+ {
+ return translation().rotation();
+ }
+
+ data::InteractionDescription data_;
+ };
+
+ inline InteractionDescription interaction()
+ {
+ return InteractionDescription();
+ }
+
// Move the ice datatypes into their own namespace
template <typename DerivedT, typename ElementT>
class ElementOps
@@ -171,6 +237,12 @@ namespace armarx::viz
}
}
+ DerivedT& enable(InteractionDescription const& interactionDescription)
+ {
+ data_->interaction = interactionDescription.data_;
+ return *static_cast<DerivedT*>(this);
+ }
+
IceInternal::Handle<ElementT> data_;
};
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
index af58bde80e165d9ccded34548f2d57a58bab5f70..595db28934519220c59c5e48ed97607a509a0f29 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
@@ -29,6 +29,7 @@ namespace armarx::viz::coin
SoUnits* units;
SoTransform* transform;
SoMaterial* material;
+ // TODO: Transform to flag system
bool wasUpdated = true;
bool visible = true;
};
diff --git a/source/RobotAPI/components/ArViz/Coin/ExportVRML.cpp b/source/RobotAPI/components/ArViz/Coin/ExportVRML.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8bdc64d2f1aa2690996fee29d550e458c2a3db73
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/ExportVRML.cpp
@@ -0,0 +1,99 @@
+#include "ExportVRML.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/actions/SoToVRML2Action.h>
+#include <Inventor/nodes/SoFile.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
+
+namespace armarx::viz::coin
+{
+
+static SoGroup* convertSoFileChildren(SoGroup* orig)
+{
+ if (!orig)
+ {
+ return new SoGroup;
+ }
+
+ SoGroup* storeResult;
+
+ if (orig->getTypeId() == SoSeparator::getClassTypeId())
+ {
+ storeResult = new SoSeparator;
+ }
+ else
+ {
+ storeResult = new SoGroup;
+ }
+
+ storeResult->ref();
+
+ if (orig->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
+ {
+ // process group node
+ for (int i = 0; i < orig->getNumChildren(); i++)
+ {
+ SoNode* n1 = orig->getChild(i);
+
+ if (n1->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
+ {
+ // convert group
+ SoGroup* n2 = (SoGroup*)n1;
+ SoGroup* gr1 = convertSoFileChildren(n2);
+ storeResult->addChild(gr1);
+ }
+ else if (n1->getTypeId() == SoFile::getClassTypeId())
+ {
+ // really load file!!
+ SoFile* fn = (SoFile*)n1;
+ SoGroup* fileChildren;
+ fileChildren = fn->copyChildren();
+ storeResult->addChild(fileChildren);
+ }
+ else
+ {
+ // just copy child node
+ storeResult->addChild(n1);
+ }
+ }
+ }
+
+ storeResult->unrefNoDelete();
+ return storeResult;
+}
+
+void exportToVRML(SoNode* node, std::string const& exportFilePath)
+{
+ SoOutput* so = new SoOutput();
+ if (!so->openFile(exportFilePath.c_str()))
+ {
+ ARMARX_ERROR << "Could not open file " << exportFilePath << " for writing.";
+ return;
+ }
+
+ so->setHeaderString("#VRML V2.0 utf8");
+
+ SoGroup* n = new SoGroup;
+ n->ref();
+ n->addChild(node);
+ SoGroup* newVisu = convertSoFileChildren(n);
+ newVisu->ref();
+
+ SoToVRML2Action tovrml2;
+ tovrml2.apply(newVisu);
+ SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph();
+ newroot->ref();
+ SoWriteAction wra(so);
+ wra.apply(newroot);
+ newroot->unref();
+
+ so->closeFile();
+
+ newVisu->unref();
+ n->unref();
+}
+
+}
diff --git a/source/RobotAPI/components/ArViz/Coin/ExportVRML.h b/source/RobotAPI/components/ArViz/Coin/ExportVRML.h
new file mode 100644
index 0000000000000000000000000000000000000000..85758493f8008e98dc4a4389a6de0b2c64b456dd
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/ExportVRML.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <Inventor/nodes/SoNode.h>
+
+#include <string>
+
+namespace armarx::viz::coin
+{
+
+ void exportToVRML(SoNode* node, std::string const& exportFilePath);
+
+}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index c76af807a1c9457e205de2dbf95d5540f89dfeac..0b5826a8975afca18a2b4181238b20e2d3a0fb18 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -1,24 +1,20 @@
#include "Visualizer.h"
-#include "VisualizationRobot.h"
-#include "VisualizationObject.h"
+#include "ExportVRML.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
-
-#include <Inventor/nodes/SoUnits.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/actions/SoToVRML2Action.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-
-
-#include <thread>
+#include <Inventor/SoPath.h>
namespace armarx::viz
{
+namespace coin
+{
+ void clearRobotCache();
+ void clearObjectCache();
+}
+
struct CoinVisualizerWrapper : IceUtil::Shared
{
class CoinVisualizer* this_;
@@ -34,6 +30,18 @@ namespace armarx::viz
}
};
+ static void selectionCallback(void* data, SoPath* path)
+ {
+ CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
+ this_->onSelectEvent(path, data::InteractionFeedbackType::SELECT);
+ }
+
+ static void deselectionCallback(void* data, SoPath* path)
+ {
+ CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
+ this_->onSelectEvent(path, data::InteractionFeedbackType::DESELECT);
+ }
+
static const char* toString(CoinVisualizerState state)
{
switch (state)
@@ -50,12 +58,6 @@ namespace armarx::viz
return "UNKNOWN";
}
- // static void updateVisualizationCB(void* data, SoSensor* sensor)
- // {
- // auto* visu = static_cast<CoinVisualizer*>(data);
- // visu->update();
- // }
-
struct TimedBlock
{
TimedBlock(const char* function)
@@ -83,10 +85,22 @@ namespace armarx::viz
callbackData = new CoinVisualizerWrapper;
callbackData->this_ = this;
- callback = newCallback_StorageInterface_pullUpdatesSince(callbackData,
+ callback = newCallback_StorageInterface_pullUpdatesSinceAndSendInteractions(callbackData,
&CoinVisualizerWrapper::onUpdateSuccess,
&CoinVisualizerWrapper::onUpdateFailure);
+
root = new SoSeparator;
+
+ // The SoSelection node enable selection of nodes via mouse click / ray casting
+ selection = new SoSelection;
+ selection->addSelectionCallback(&selectionCallback, this);
+ selection->addDeselectionCallback(&deselectionCallback, this);
+ selection->setUserData(this);
+
+ selection->addChild(root);
+
+ // Preallocate some space for layers
+ layers.data.reserve(32);
}
CoinVisualizer::~CoinVisualizer()
@@ -132,6 +146,31 @@ namespace armarx::viz
timing.layerName = update.name;
CoinLayerID layerID(update.component, update.name);
+ CoinLayer& layer = findOrAddLayer(layerID);
+
+ IceUtil::Time time_addLayer = IceUtil::Time::now();
+ timing.addLayer = time_addLayer - time_start;
+
+ addOrUpdateElements(&layer, update);
+
+ IceUtil::Time time_updates = IceUtil::Time::now();
+ timing.updateElements = time_updates - time_addLayer;
+
+ removeElementsIfNotUpdated(&layer);
+
+ IceUtil::Time time_remove = IceUtil::Time::now();
+ timing.removeElements = time_remove - time_updates;
+
+ emitLayerUpdated(layerID, layer);
+
+ IceUtil::Time time_end = IceUtil::Time::now();
+ timing.total = time_end - time_start;
+
+ return timing;
+ }
+
+ CoinLayer& CoinVisualizer::findOrAddLayer(CoinLayerID const& layerID)
+ {
auto layerIt = layers.lowerBound(layerID);
if (layerIt == layers.data.end() || layerIt->id != layerID)
@@ -142,15 +181,16 @@ namespace armarx::viz
root->addChild(coinNode);
layerIt = layers.data.insert(layerIt, CoinLayer(layerID, coinNode));
+ layerIt->elements.reserve(64);
}
- IceUtil::Time time_addLayer = IceUtil::Time::now();
- timing.addLayer = time_addLayer - time_start;
+ return *layerIt;
+ }
- // Add or update the elements in the update
- CoinLayer& layer = *layerIt;
- layer.elements.reserve(update.elements.size());
- for (auto& updatedElementPtr : update.elements)
+ void CoinVisualizer::addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update)
+ {
+ layer->elements.reserve(update.elements.size());
+ for (viz::data::ElementPtr const& updatedElementPtr : update.elements)
{
if (!updatedElementPtr)
{
@@ -179,9 +219,9 @@ namespace armarx::viz
}
coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
- auto oldElementIter = layer.lowerBound(updatedElement.id);
+ auto oldElementIter = layer->lowerBound(updatedElement.id);
CoinLayerElement* oldElement = nullptr;
- if (oldElementIter != layer.elements.end() && oldElementIter->data->id == updatedElement.id)
+ if (oldElementIter != layer->elements.end() && oldElementIter->data->id == updatedElement.id)
{
// Element already exists
CoinLayerElement* oldElement = &*oldElementIter;
@@ -193,20 +233,81 @@ namespace armarx::viz
if (updated)
{
+ // 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");
+ }
+
oldElement->data = updatedElementPtr;
continue;
}
else
{
// Types are different, so we delete the old element and create a new one
- layer.node->removeChild(oldElementVisu.separator);
+ layer->node->removeChild(oldElementVisu.separator);
}
}
auto elementVisu = visualizer->create(updatedElement);
if (elementVisu->separator)
{
- layer.node->addChild(elementVisu->separator);
+ // Has the new element interactions?
+ 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");
+ }
+
+ layer->node->addChild(elementVisu->separator);
if (oldElement)
{
oldElement->data = updatedElementPtr;
@@ -215,7 +316,7 @@ namespace armarx::viz
else
{
// Need to add a new element
- layer.elements.insert(oldElementIter, CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
+ layer->elements.insert(oldElementIter, CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
}
}
else
@@ -226,12 +327,11 @@ namespace armarx::viz
<< "You need to register a visualizer for each type in ArViz/Coin/Visualizer.cpp";
}
}
+ }
- IceUtil::Time time_updates = IceUtil::Time::now();
- timing.updateElements = time_updates - time_addLayer;
-
- // Remove the elements which were not contained in the update
- for (auto iter = layer.elements.begin(); iter != layer.elements.end();)
+ void CoinVisualizer::removeElementsIfNotUpdated(CoinLayer* layer)
+ {
+ for (auto iter = layer->elements.begin(); iter != layer->elements.end();)
{
coin::ElementVisualization& elementVisu = *iter->visu;
if (elementVisu.wasUpdated)
@@ -241,20 +341,24 @@ namespace armarx::viz
}
else
{
- layer.node->removeChild(elementVisu.separator);
- iter = layer.elements.erase(iter);
+ void* userData = elementVisu.separator->getUserData();
+ if (userData)
+ {
+ // Remove interaction entry if element has been removed
+ auto removedInteraction = std::find_if(elementInteractions.begin(), elementInteractions.end(),
+ [userData](std::unique_ptr<ElementInteractionData> const& entry)
+ {
+ return entry.get() == userData;
+ });
+ elementInteractions.erase(removedInteraction);
+
+ elementVisu.separator->setUserData(nullptr);
+ elementVisu.separator->setName("");
+ }
+ layer->node->removeChild(elementVisu.separator);
+ iter = layer->elements.erase(iter);
}
}
-
- IceUtil::Time time_remove = IceUtil::Time::now();
- timing.removeElements = time_remove - time_updates;
-
- emitLayerUpdated(layerID, layer);
-
- IceUtil::Time time_end = IceUtil::Time::now();
- timing.total = time_end - time_start;
-
- return timing;
}
void CoinVisualizer::update()
@@ -269,7 +373,11 @@ namespace armarx::viz
{
std::unique_lock<std::mutex> lock(stateMutex);
storage = stateStorage;
+ selection->deselectAll();
root->removeAllChildren();
+ interactionFeedbackBuffer.clear();
+ elementInteractions.clear();
+ selectedElement = nullptr;
layers.data.clear();
pulledUpdates.revision = 0;
pulledUpdates.updates.clear();
@@ -300,11 +408,14 @@ namespace armarx::viz
IceUtil::Time time_start = IceUtil::Time::now();
CoinVisualizer_UpdateTiming timing;
- // We should restart the pull for updates so it can run in parallel
data::LayerUpdates currentUpdates = pulledUpdates;
updateResult = CoinVisualizerUpdateResult::WAITING;
timing.waitStart = time_start;
- storage->begin_pullUpdatesSince(currentUpdates.revision, callback);
+ storage->begin_pullUpdatesSinceAndSendInteractions(
+ currentUpdates.revision, interactionFeedbackBuffer, callback);
+
+ // Clear interaction feedback buffer after it has been sent
+ interactionFeedbackBuffer.clear();
auto layerIDsBefore = getLayerIDs();
@@ -396,6 +507,12 @@ namespace armarx::viz
// Layer is currently visible
if (!visible)
{
+ // If we hide a layer wit a selected element, Coin will crash during rendering
+ // So we check if the selected element is in the layer to be hidden
+ if (selectedElement && selectedElement->layer == id)
+ {
+ selection->deselectAll();
+ }
root->removeChild(childIndex);
}
}
@@ -437,36 +554,98 @@ namespace armarx::viz
}
}
- void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
+ void CoinVisualizer::selectElement(int index)
{
+ if (index >= (int)elementInteractions.size())
+ {
+ return;
+ }
+ ElementInteractionData const* id = elementInteractions[index].get();
- SoOutput* so = new SoOutput();
- if (!so->openFile(exportFilePath.c_str()))
+ CoinLayer* layer = layers.findLayer(id->layer);
+ if (layer == nullptr)
{
- ARMARX_ERROR << "Could not open file " << exportFilePath << " for writing." << std::endl;
+ 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;
+ }
+
+ selection->deselectAll();
+ selection->select(element->visu->separator);
+ }
+
+ static ElementInteractionData* findInteractionDataOnPath(SoPath* path)
+ {
+ // Search for user data in the path
+ // We stored ElementInteractionData into the user data of the parent SoSeparator
+ void* userData = nullptr;
+ int pathLength = path->getLength();
+ for (int i = 0; i < pathLength; ++i)
+ {
+ SoNode* node = path->getNode(i);
+ const char* name = node->getName().getString();
+ if (strcmp(name, "InteractiveNode") == 0)
+ {
+ userData = node->getUserData();
+ if (userData != nullptr)
+ {
+ break;
+ }
+ }
+ }
- so->setHeaderString("#VRML V2.0 utf8");
+ return static_cast<ElementInteractionData*>(userData);
+ }
+
+ void CoinVisualizer::onSelectEvent(SoPath* path, int eventType)
+ {
+ if (state != CoinVisualizerState::RUNNING)
+ {
+ return;
+ }
- SoGroup* n = new SoGroup;
- n->ref();
- n->addChild(root);
- SoGroup* newVisu = VirtualRobot::CoinVisualizationFactory::convertSoFileChildren(n);
- newVisu->ref();
+ ElementInteractionData* id = findInteractionDataOnPath(path);
+ if (id == nullptr)
+ {
+ if (eventType == data::InteractionFeedbackType::SELECT)
+ {
+ // An object was selected that does not have any interactions enabled
+ // We deselect all other elements in this case to avoid highlighting
+ // non-interactable elements
+ selection->deselectAll();
+ }
+ return;
+ }
- SoToVRML2Action tovrml2;
- tovrml2.apply(newVisu);
- SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph();
- newroot->ref();
- SoWriteAction wra(so);
- wra.apply(newroot);
- newroot->unref();
+ if (eventType == data::InteractionFeedbackType::SELECT)
+ {
+ selectedElement = id;
+ }
+ else
+ {
+ selectedElement = nullptr;
+ }
- so->closeFile();
+ viz::data::InteractionFeedback& feedback = interactionFeedbackBuffer.emplace_back();
+ feedback.type = eventType;
+ feedback.component = id->layer.first;
+ feedback.layer = id->layer.second;
+ feedback.element = id->element;
+ feedback.revision = pulledUpdates.revision;
+ }
- newVisu->unref();
- n->unref();
+ void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
+ {
+ coin::exportToVRML(root, exportFilePath);
}
}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 195172e9c812c4fef9bd391853748592e113b5d8..adda36b9ae47404a3f59062c649dad681cd0502f 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -4,7 +4,9 @@
#include <RobotAPI/interface/ArViz/Component.h>
+#include <Inventor/nodes/SoSelection.h>
#include <Inventor/nodes/SoSeparator.h>
+
#include <IceUtil/Shared.h>
#include <functional>
@@ -184,6 +186,13 @@ namespace armarx::viz
struct CoinVisualizerWrapper;
+ struct ElementInteractionData
+ {
+ CoinLayerID layer;
+ std::string element;
+ viz::data::InteractionDescription interaction;
+ };
+
class CoinVisualizer
{
public:
@@ -219,14 +228,28 @@ namespace armarx::viz
CoinVisualizer_UpdateTiming getTiming();
+ CoinLayer& findOrAddLayer(CoinLayerID const& layerID);
+ void addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update);
+ void removeElementsIfNotUpdated(CoinLayer* layer);
+
std::vector<CoinLayerID> getLayerIDs();
void emitLayersChanged(std::vector<CoinLayerID> const& layerIDs);
void emitLayerUpdated(CoinLayerID const& layerID, CoinLayer const& layer);
+ void selectElement(int index);
+ void onSelectEvent(SoPath* path, 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.
+ std::vector<std::unique_ptr<ElementInteractionData>> elementInteractions;
+ std::vector<viz::data::InteractionFeedback> interactionFeedbackBuffer;
+ // The currently selected element. Maybe nullptr if no element is selected.
+ ElementInteractionData* selectedElement = nullptr;
+
void onUpdateSuccess(data::LayerUpdates const& updates);
void onUpdateFailure(Ice::Exception const& ex);
IceUtil::Handle<CoinVisualizerWrapper> callbackData;
- armarx::viz::Callback_StorageInterface_pullUpdatesSincePtr callback;
+ armarx::viz::Callback_StorageInterface_pullUpdatesSinceAndSendInteractionsPtr callback;
std::mutex storageMutex;
viz::StorageInterfacePrx storage;
@@ -236,6 +259,7 @@ namespace armarx::viz
std::vector<std::type_index> elementVisualizersTypes;
std::vector<std::unique_ptr<coin::ElementVisualizer>> elementVisualizers;
+ SoSelection* selection = nullptr;
SoSeparator* root = nullptr;
std::atomic<CoinVisualizerUpdateResult> updateResult{CoinVisualizerUpdateResult::SUCCESS};
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 4d2be3924a99696bb013dc9cab0ed2c8201b1923..d772854de1792982ecf4015f7b61fd412ab6fb09 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -337,6 +337,7 @@ namespace armarx
viz::PointCloud pc = viz::PointCloud("points")
.position(Eigen::Vector3f(2000.0f, 0.0f, 400.0f))
.transparency(0.0f);
+ pc.enable(viz::interaction().selection());
viz::ColoredPoint p;
p.color = viz::Color::fromRGBA(255, 255, 0, 255);
@@ -500,6 +501,34 @@ namespace armarx
}
+ void fillInteractionLayer(viz::Layer& layer)
+ {
+ // Make box selectable
+ 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));
+ // Enable some interaction possibilities
+ box.enable(viz::interaction()
+ .selection()
+ .contextMenu({"First Option", "Second Option", "Third Option"}));
+
+ layer.add(box);
+
+ viz::Cylinder cyl = viz::Cylinder("cylinder")
+ .position(Eigen::Vector3f(1000.0f, 0.0f, 2000.0f))
+ .direction(Eigen::Vector3f::UnitZ())
+ .height(200.0f)
+ .radius(50.0f)
+ .color(viz::Color::fromRGBA(255, 165, 0));
+ // Enable some interaction possibilities
+ cyl.enable(viz::interaction()
+ .selection()
+ .contextMenu({"Cyl Option 1", "Cyl Option 2"}));
+
+ layer.add(cyl);
+ }
+
void ArVizExample::run()
{
@@ -524,27 +553,37 @@ namespace armarx
viz::Layer objectsLayer = arviz.layer("Objects");
viz::Layer disAppearingLayer = arviz.layer("DisAppearing");
viz::Layer robotHandsLayer = arviz.layer("RobotHands");
+ viz::Layer interactionLayer = arviz.layer("Interaction");
+ viz::StagedCommit stage = arviz.stage();
// These layers are not updated in the loop.
{
viz::Layer permanentLayer = arviz.layer("Permanent");
fillPermanentLayer(permanentLayer);
- arviz.commit(permanentLayer);
+ stage.add(permanentLayer);
}
bool manyElements = getProperty<bool>("layers.ManyElements");
if (manyElements)
{
viz::Layer manyElementsLayer = arviz.layer("ManyElements");
fillManyElementsLayer(manyElementsLayer, 0);
- arviz.commit(manyElementsLayer);
+ stage.add(manyElementsLayer);
}
{
viz::Layer colorMapsLayer = arviz.layer("ColorMaps");
fillColorMapsLayer(colorMapsLayer, 0);
- arviz.commit(colorMapsLayer);
+ stage.add(colorMapsLayer);
}
+ fillInteractionLayer(interactionLayer);
+ stage.add(interactionLayer);
+
+ // Apply the staged commits in a single network call
+ viz::CommitResult result = arviz.commit(stage);
+ ARMARX_INFO << "Permanent layers committed in revision: "
+ << result.revision();
+
CycleUtil c(20);
while (!task->isStopped())
@@ -568,7 +607,39 @@ namespace armarx
fillRobotHandsLayer(robotHandsLayer);
}
- arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer, disAppearingLayer, robotHandsLayer});
+ // We can reuse a staged commit
+ 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});
+ // We can request interaction feedback for specific layers
+ stage.requestInteraction(interactionLayer);
+
+ // This sends the layer updates and receives interaction feedback in a single network call
+ result = arviz.commit(stage);
+ // Be careful: The interactions are stored in the CommitResult
+ // So the range is only valid as long as result is in scope and not overriden.
+ viz::InteractionFeedbackRange interactions = result.interactions();
+ if (interactions.size() > 0)
+ {
+ ARMARX_INFO << "We got some interactions: " << interactions.size();
+ for (viz::InteractionFeedback const& interaction: interactions)
+ {
+ if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen)
+ {
+ ARMARX_INFO << "[" << interaction.layer()
+ << "/" << interaction.element()
+ << "] Chosen context menu: "
+ << interaction.chosenContextMenuEntry();
+ }
+ else
+ {
+ ARMARX_INFO << "[" << interaction.layer()
+ << "/" << interaction.element()
+ << "] " << toString(interaction.type());
+ }
+ }
+ }
c.waitForCycleDuration();
}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index c5c4c52b6d430a4715f83893346ec7fa1acb1706..72a4ee76f56373b6e03697b76a9c45e07a3816dd 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -124,7 +124,7 @@ namespace armarx
class DebugDrawerHelper : public armarx::detail::DebugDrawerHelper::FrameView
{
using FrameView = armarx::detail::DebugDrawerHelper::FrameView;
- friend class FrameView;
+ friend class armarx::detail::DebugDrawerHelper::FrameView;
public:
struct Defaults
{
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt b/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
index 678b67dd3df935ebf6d7cb2bb530117f6430de9f..beb8a9d5e291f1c8faab89834417994baaeac0b9 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
@@ -6,6 +6,7 @@ set(COMPONENT_LIBS
${PROJECT_NAME}Interfaces
RobotAPIComponentPlugins
+ ArViz
)
set(SOURCES
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
index 4ac857086a55e5ec19b709ca00dcd24f2b23a574..f3417d30644acd9540aa8c1f888a77c49290a225 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
@@ -21,13 +21,13 @@
*/
#include "DebugDrawerToArViz.h"
-
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/color/interpolation.h>
+#include "BlackWhitelistUpdate.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include "BlackWhitelistUpdate.h"
+#include <SimoxUtility/color/interpolation.h>
+#include <SimoxUtility/math/pose/pose.h>
#define FUNCTION_NOT_IMPLEMENTED_MESSAGE \
@@ -180,7 +180,7 @@ namespace armarx
setLayerElement(layer, viz::Polygon(ss.str()).addPoint(toEigen(p1)).addPoint(toEigen(p2))
.lineColor(color).lineWidth(lineSet.lineWidth).color(simox::Color::black(0)));
}
- arviz.commit(layer);
+ arviz.commit({layer});
}
void DebugDrawerToArViz::setBoxVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
@@ -390,7 +390,7 @@ namespace armarx
viz::Robot& robot = it->second;
robot.pose(toEigen(globalPose));
}
- arviz.commit(getLayer(layer));
+ arviz.commit({getLayer(layer)});
}
void DebugDrawerToArViz::updateRobotConfig(const std::string& layer, const std::string& name, const NameValueMap& configuration, const Ice::Current&)
@@ -405,7 +405,7 @@ namespace armarx
viz::Robot& robot = it->second;
robot.joints(configuration);
}
- arviz.commit(getLayer(layer));
+ arviz.commit({getLayer(layer)});
}
void DebugDrawerToArViz::updateRobotColor(const std::string& layer, const std::string& name, const DrawColor& color, const Ice::Current&)
@@ -420,7 +420,7 @@ namespace armarx
viz::Robot& robot = it->second;
robot.overrideColor(toViz(color));
}
- arviz.commit(getLayer(layer));
+ arviz.commit({getLayer(layer)});
}
void DebugDrawerToArViz::updateRobotNodeColor(const std::string& layer, const std::string& name, const std::string& robotNodeName, const DrawColor& color, const Ice::Current&)
@@ -675,12 +675,12 @@ namespace armarx
{
std::scoped_lock lock(mutex);
- std::vector<const viz::Layer*> commit;
+ std::vector<viz::Layer> commit;
commit.reserve(layers.size());
for (auto& [name, layer] : layers)
{
layer.clear();
- commit.push_back(&layer);
+ commit.push_back(layer);
}
arviz.commit(commit);
}
@@ -690,7 +690,7 @@ namespace armarx
viz::Layer layer = getLayer(layerName);
layer.clear();
- arviz.commit(layer);
+ arviz.commit({layer});
}
void DebugDrawerToArViz::clearDebugLayer(const Ice::Current&)
{
@@ -802,7 +802,7 @@ namespace armarx
{
viz::Layer& layer = getLayer(layerName);
removeLayerElement(layer, name);
- arviz.commit(layer);
+ arviz.commit({layer});
}
}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
index f4ce170c84fb0ce7507a8e87bf2901b53d3b7d89..38e66882d00f87ee17e55e76699b943be51d66c6 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
@@ -187,7 +187,7 @@ namespace armarx
{
viz::Layer& layer = getLayer(layerName);
setLayerElement(layer, element);
- arviz.commit(layer);
+ arviz.commit({layer});
}
void removeLayerElement(viz::Layer& layer, const std::string& name);
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index 6ca25b01ef9eb704a2f29140e54e6e602c8b32b6..61497d62d399adf6f1c89a0a0de1c544a902ec51 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -269,7 +269,7 @@ namespace armarx
while (current_distance < distance_to_goal)
{
- for (const auto man_obstacle : m_managed_obstacles)
+ for (const auto& man_obstacle : m_managed_obstacles)
{
Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
Eigen::Vector2f sample_left = sample + (orthogonal_normalized * 250);
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
index 5dd70f211733cc61469a0d8b2be38f242b3307ff..a730c63ab96362386781388d65f426217b328721 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
@@ -30,6 +30,7 @@
#include <shared_mutex>
#include <string>
#include <vector>
+#include <memory>
// Eigen
#include <Eigen/Geometry>
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
index 997be9e96b87d88593d27fd7120f332a21912b05..906fc21433a7e5f6f6aec6898da7d55c6667e2cd 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
@@ -147,7 +147,7 @@ namespace armarx
{
ARMARX_CHECK_NOT_NULL(_driver);
- for (const std::pair<std::string, float>& pair : targetJointAngles)
+ for (const auto& pair : targetJointAngles)
{
if (pair.first == "Fingers")
{
diff --git a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
index dcf81778c35b15b22b99fdcde688e18d7f5af801..8649d60af88061cfdb973a5fdbe94f7cbaed9c4b 100644
--- a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
+++ b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
@@ -6,6 +6,7 @@ set(COMPONENT_LIBS
# RobotAPICore # for DebugDrawerTopic
RobotAPIInterfaces
ArmarXGuiComponentPlugins
+ ArViz
natik
diffik
)
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
index b6d5c668ecd0ad66bf47036d1b300efa9e814803..43f32295717ff031d2ac9b6f54184c21b8c60e73 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -89,7 +89,7 @@ namespace armarx
.fileByObjectFinder(objectPose.objectID)
.alpha(objectPose.confidence));
}
- arviz.commit(layer);
+ arviz.commit({layer});
}
cycle.waitForCycleDuration();
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index a6cf94b3a961e53c179c14132e5d3aaa3a11ba5b..7e83132236f7478cf9570747a6c0e83add0e1fab 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -3,6 +3,7 @@
#include <memory>
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 8b3f0299ee9732265b757d6c5b375a08e72d554b..e780bbfe388f6e484673bbd44d5cc7c3bf5f0570 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -185,7 +185,7 @@ GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq all;
- for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
+ for (const auto& pair : candidates)
{
all.insert(all.end(), pair.second.begin(), pair.second.end());
}
@@ -215,7 +215,7 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateF
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq matching;
- for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
+ for (const auto& pair : candidates)
{
for (const grasping::GraspCandidatePtr& candidate : pair.second)
{
@@ -268,7 +268,7 @@ BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const
{
std::unique_lock lock(dataMutex);
BimanualGraspCandidateSeq all;
- for (const std::pair<std::string, grasping::BimanualGraspCandidateSeq>& pair : bimanualCandidates)
+ for (const auto& pair : bimanualCandidates)
{
all.insert(all.end(), pair.second.begin(), pair.second.end());
}
@@ -311,7 +311,7 @@ void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
StringSeq GraspCandidateObserver::getAvailableProviderNames()
{
StringSeq names;
- for (const std::pair<std::string, ProviderInfoPtr>& pair : providers)
+ for (const auto& pair : providers)
{
names.push_back(pair.first);
}
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
index fc4a18f313bf5320bc3307448b3dba645b8ffc41..8b4239814b0a3a7cdc6f1abd54a9da6f889fe122 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
@@ -6,5 +6,6 @@ armarx_add_component(
RobotAPICore
RobotAPIComponentPlugins
RobotUnit
+ ArViz
)
armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
index 524b650bf5a3767a92a457ab97b26ffcdd903a40..44687f4467d65481b8500e10871112863c892be2 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
@@ -6,5 +6,6 @@ armarx_add_component(
RobotAPICore
RobotAPIComponentPlugins
RobotUnit
+ ArViz
)
armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
index daacae417637d89a1a4d23a79a6a4668d196b69d..1a545e067effa40daed3e05b5c34dda4c8f851a9 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
@@ -200,7 +200,8 @@ namespace armarx
void GazeController::removeTargetAfter(long durationMilliSeconds, const Ice::Current&)
{
- std::async(std::launch::async, [](long durationMilliSeconds, GazeController * self)
+ // TODO: This probably does not run async, as a future associated with std::async waits on destruction
+ auto void_f = std::async(std::launch::async, [](long durationMilliSeconds, GazeController * self) -> void
{
std::this_thread::sleep_for(std::chrono::milliseconds(durationMilliSeconds));
self->removeTarget();
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index 7ed79a0b5f833864eedf06900548b37fa1e6b780..f23c8a04945858b9efdb071f1c5215b719073be0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -72,7 +72,7 @@ namespace armarx
void setPositionOrientation(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const Ice::Current&) override;
void setPose(const Eigen::Matrix4f& mat, const Ice::Current&) override;
- void setImpedanceParameters(const std::string&, const Ice::FloatSeq&, const Ice::Current&);
+ void setImpedanceParameters(const std::string&, const Ice::FloatSeq&, const Ice::Current&) override;
void setNullspaceConfig(const Eigen::VectorXf& joint, const Eigen::VectorXf& knull, const Eigen::VectorXf& dnull, const Ice::Current&) override;
void setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg, const Ice::Current&) override;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 6f6adf1092c040bf546d03092c458129d49e0327..ce63e07571da1dfd17d42b99e0c446a72aa83532 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -300,5 +300,11 @@ namespace armarx::RobotUnitModule
std::size_t rtLoggingTimestepMs {0};
/// @brief The time an entry shold remain in the backlog.
IceUtil::Time rtLoggingBacklogRetentionTime;
+
+ friend void WriteTo(const auto& dentr,
+ const Logging::DataStreamingEntry::OutVal& out,
+ const auto& val,
+ std::size_t fidx,
+ auto& data);
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index e65e6506a5312346f4409f68a09880c3aa77b939..a9d2e8de69625147071a2e2aaa650d43de40cdb9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -293,7 +293,7 @@ namespace armarx::RobotUnitModule
//check for all nodes 0
{
bool allJoints0 = true;
- for (const VirtualRobot::RobotNodePtr node : selfCollisionAvoidanceRobotNodes)
+ for (const auto& node : selfCollisionAvoidanceRobotNodes)
{
if (0 != node->getJointValue())
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
index 3a98c100b79bef43a55b1dd12475a41c8016c5b5..8539b52e860fc21b785fad49e6d1a0c342c92ae1 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
@@ -30,7 +30,7 @@
#include "HeterogenousContinuousContainerMacros.h"
-#if __GNUC__< 5
+#if __GNUC__< 5 && !defined(__clang__)
namespace std
{
inline void* align(size_t alignment, size_t bytes, void*& bufferPlace, size_t& bufferSpace) noexcept
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 649d40ccf31ae409e140f651c40010c7fd22ad8b..40250b2b1a7d0dc472806715fe6c4daf9b532f17 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -435,7 +435,7 @@ namespace armarx
LaserScanStep step;
step.angle = i * scanInfo.stepSize;
step.distance = distVal[i];
- step.intensity = intensityVal[i];
+ //step.intensity = intensityVal[i];
scanData.push_back(step);
}
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
index f5bb28f2f794200a6af845cfb107587149929d35..c43cbd71a23ae7ca803f59c6d6844e8d869a2a85 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
@@ -155,7 +155,7 @@
<number>2</number>
</property>
<attribute name="headerVisible">
- <bool>true</bool>
+ <bool>false</bool>
</attribute>
<column>
<property name="text">
@@ -212,7 +212,7 @@
</item>
</layout>
</widget>
- <widget class="QWidget" name="recordingTab">
+ <widget class="QWidget" name="tabRecording">
<attribute name="title">
<string>Recording && Replay</string>
</attribute>
@@ -613,6 +613,59 @@
</item>
</layout>
</widget>
+ <widget class="QWidget" name="tabInteraction">
+ <attribute name="title">
+ <string>Interaction</string>
+ </attribute>
+ <layout class="QVBoxLayout" name="verticalLayout_7">
+ <item>
+ <widget class="QGroupBox" name="groupBoxSelectedElement">
+ <property name="title">
+ <string>Selected Element</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_8">
+ <item>
+ <widget class="QLabel" name="labelSelectedElement">
+ <property name="text">
+ <string><None></string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="deselectButton">
+ <property name="text">
+ <string>Deselect / Clear Selection</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBoxContextMenu">
+ <property name="title">
+ <string>Context Menu</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_9"/>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBoxInteractiveElements">
+ <property name="title">
+ <string>Interactive Elements (Double click to select in 3D Viewer)</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_10">
+ <item>
+ <widget class="QListWidget" name="listInteractiveElements"/>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
</layout>
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index c3aad744b56229d4ae2f01b716e092abfa5f76f2..7aa05e3a197d4054ea04eaaa8ebc0f68304c50cc 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -31,8 +31,6 @@
#include <QTimer>
#include <QFileDialog>
-#define ENABLE_INTROSPECTION 1
-
namespace armarx
{
@@ -61,9 +59,6 @@ namespace armarx
updateTimer = new QTimer(this);
connect(updateTimer, &QTimer::timeout, this, QOverload<>::of(&This::onUpdate));
- timingObserverTimer = new QTimer(this);
- connect(timingObserverTimer, &QTimer::timeout, this, QOverload<>::of(&This::onTimingObserverUpdate));
-
replayTimer = new QTimer(this);
connect(replayTimer, &QTimer::timeout, this, QOverload<>::of(&This::onReplayTimerTick));
@@ -91,6 +86,9 @@ namespace armarx
connect(widget.exportToVRMLButton, &QPushButton::clicked, this, &This::exportToVRML);
+ connect(widget.deselectButton, &QPushButton::clicked, this, &This::onDeselectElement);
+ connect(widget.listInteractiveElements, &QListWidget::itemDoubleClicked, this, &This::onInteractiveElementSelected);
+
connect(this, &This::connectGui, this, &This::onConnectGui, Qt::QueuedConnection);
connect(this, &This::disconnectGui, this, &This::onDisconnectGui, Qt::QueuedConnection);
@@ -98,7 +96,6 @@ namespace armarx
connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
-#if ENABLE_INTROSPECTION
connect(widget.layerInfoTreeGroupBox, &QGroupBox::toggled, &layerInfoTree, &LayerInfoTree::setEnabled);
connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged),
&layerInfoTree, &LayerInfoTree::setMaxElementCountDefault);
@@ -108,8 +105,6 @@ namespace armarx
layerInfoTree.setEnabled(widget.layerInfoTreeGroupBox->isChecked());
layerInfoTree.registerVisualizerCallbacks(visualizer);
-#endif
-
// We need a callback from the visualizer, when the layers have changed
// So we can update the tree accordingly
@@ -178,14 +173,13 @@ namespace armarx
currentRecordingSelected = false;
changeMode(ArVizWidgetMode::Live);
- timingObserverTimer->start(33);
updateTimer->start(33);
}
void ArVizWidgetController::onDisconnectGui()
{
- timingObserverTimer->stop();
visualizer.stop();
+ updateTimer->stop();
changeMode(ArVizWidgetMode::NotConnected);
}
@@ -321,7 +315,6 @@ namespace armarx
void ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
{
-#if ENABLE_INTROSPECTION
(void) previous;
if (!current->parent())
@@ -342,8 +335,6 @@ namespace armarx
{
layerInfoTree.setSelectedLayer(id, &visualizer.layers);
}
-
-#endif
}
void ArVizWidgetController::onCollapseAll(bool)
@@ -435,9 +426,135 @@ namespace armarx
layerTreeChanged(nullptr, 0);
}
+ void ArVizWidgetController::onDeselectElement()
+ {
+ // We just deselect all elements.
+ // Maybe we need to be more specific for strange use cases (?)
+ visualizer.selection->deselectAll();
+ }
+
+ void ArVizWidgetController::onContextMenuClicked()
+ {
+ viz::ElementInteractionData* selected = visualizer.selectedElement;
+ if (selected == nullptr)
+ {
+ ARMARX_WARNING << "Selected element is null, but a context menu option was clicked!";
+ return;
+ }
+
+ QPushButton* clickedButton = static_cast<QPushButton*>(sender());
+
+ QLayout* layout = widget.groupBoxContextMenu->layout();
+ int count = layout->count();
+ for (int i = 0; i < count; ++i)
+ {
+ QPushButton* button = static_cast<QPushButton*>(layout->itemAt(i)->widget());
+ if (button == clickedButton)
+ {
+ viz::data::InteractionFeedback& interaction = visualizer.interactionFeedbackBuffer.emplace_back();
+ interaction.component = selected->layer.first;
+ interaction.layer = selected->layer.second;
+ interaction.element = selected->element;
+ interaction.type = viz::data::InteractionFeedbackType::CONTEXT_MENU_CHOSEN;
+ interaction.chosenContextMenuEntry = i;
+ return;
+ }
+ }
+ }
+
+ void ArVizWidgetController::onInteractiveElementSelected(QListWidgetItem* item)
+ {
+ int index = widget.listInteractiveElements->row(item);
+ if (index < 0)
+ {
+ return;
+ }
+
+ visualizer.selectElement(index);
+ }
+
+ static QString toQString(viz::ElementInteractionData const& inter)
+ {
+ std::string id = inter.layer.first + "/"
+ + inter.layer.second + "/"
+ + inter.element;
+ return QString::fromStdString(id);
+ }
+
void ArVizWidgetController::onUpdate()
{
visualizer.update();
+
+ // Show the currently selected element
+ QString selectedElementName("<None>");
+ QLayout* contextMenuLayout = widget.groupBoxContextMenu->layout();
+ if (visualizer.selectedElement)
+ {
+ selectedElementName = toQString(*visualizer.selectedElement);
+
+ // Show context menu options if enabled
+ int currentCount = contextMenuLayout->count();
+ viz::data::InteractionDescription const& desc = visualizer.selectedElement->interaction;
+ std::vector<std::string> const& options = desc.contextMenuOptions;
+ int newCount = 0;
+ if (desc.enableFlags & viz::data::InteractionEnableFlags::CONTEXT_MENU)
+ {
+ newCount = (int)options.size();
+ }
+ if (newCount != currentCount)
+ {
+ // Remove all items
+ QLayoutItem* item;
+ while ((item = contextMenuLayout->takeAt( 0 )) != nullptr)
+ {
+ delete item->widget();
+ delete item;
+ }
+
+ for (std::string const& option : options)
+ {
+ QPushButton* button = new QPushButton(
+ QString::fromStdString(option),
+ widget.groupBoxContextMenu);
+ connect(button, &QPushButton::clicked, this, &ArVizWidgetController::onContextMenuClicked);
+ contextMenuLayout->addWidget(button);
+ }
+ }
+ else
+ {
+ for (int i = 0; i < currentCount; ++i)
+ {
+ QLayoutItem* item = contextMenuLayout->itemAt(i);
+ QPushButton* button = static_cast<QPushButton*>(item->widget());
+ button->setText(QString::fromStdString(options[i]));
+ }
+ }
+ }
+ else
+ {
+ QLayoutItem* item;
+ while ((item = contextMenuLayout->takeAt( 0 )) != nullptr)
+ {
+ delete item->widget();
+ delete item;
+ }
+ }
+ widget.labelSelectedElement->setText(selectedElementName);
+
+ // Show the interactive elements
+ int currentCount = widget.listInteractiveElements->count();
+ int newCount = (int)visualizer.elementInteractions.size();
+ if (newCount != currentCount)
+ {
+ widget.listInteractiveElements->clear();
+ for (auto& interaction : visualizer.elementInteractions)
+ {
+ QString elementID = toQString(*interaction);
+ widget.listInteractiveElements->addItem(elementID);
+ }
+ }
+
+ onTimingObserverUpdate();
}
void ArVizWidgetController::onTimingObserverUpdate()
@@ -1004,7 +1121,7 @@ namespace armarx
SoNode* ArVizWidgetController::getScene()
{
- return visualizer.root;
+ return visualizer.selection;
}
static const std::string CONFIG_KEY_STORAGE = "Storage";
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index cd089f6c83760e161f4095aaf4b132093100f78f..cb10e285bee2e0b69497a1032703ca2aa5b5462e 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -138,6 +138,10 @@ namespace armarx
void showAllLayers(bool visible);
void showFilteredLayers(bool visible);
+ void onDeselectElement();
+ void onContextMenuClicked();
+ void onInteractiveElementSelected(QListWidgetItem* item);
+
void onUpdate();
void onTimingObserverUpdate();
@@ -174,7 +178,6 @@ namespace armarx
QPointer<SimpleConfigDialog> configDialog;
QTimer* updateTimer;
- QTimer* timingObserverTimer;
viz::CoinVisualizer_UpdateTiming lastTiming;
StringVariantBaseMap timingMap;
diff --git a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
index 69a25ce1ef369fa25a15ae984f426ef3ccf148a1..1df2874b254aa6eec3688ff645d2a1430df1e85f 100644
--- a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
@@ -28,7 +28,7 @@ set(GUI_UIS ArVizWidget.ui)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
set(COMPONENT_LIBS
- ArViz
+ RobotAPI::ArVizCoin
SimpleConfigDialog
)
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
index defcb3362d6c038761389b87215cc149c5a061d9..090a2c7e47eeb7647b955dc69b166d9aa97b2c05 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
@@ -84,7 +84,7 @@ namespace armarx
elem->addTo(layer);
}
}
- getArvizClient().commit(layer);
+ getArvizClient().commit({layer});
}
}
diff --git a/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp b/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp
index 27cb74dadea8b121431466d39e4633d94832a3d6..1e8b97c14c4d086336373c19dfcd59aff13e4ca9 100644
--- a/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp
@@ -296,10 +296,14 @@ namespace armarx
side_enabled_r[ba::neg_z_neg_y] = _ui.checkBoxGraspRNegZNegY->isChecked();
}
- auto layer_l = getArvizClient().layer("grasps_l");
- auto layer_r = getArvizClient().layer("grasps_r");
- auto layer_hand_vec = getArvizClient().layer("hand_vec");
- getArvizClient().enqueueLayerContaining("box", viz::Box{"box"}.set(box).transformPose(_robot->getGlobalPose()));
+ viz::Layer layer_l = arviz.layer("grasps_l");
+ viz::Layer layer_r = arviz.layer("grasps_r");
+ viz::Layer layer_hand_vec = arviz.layer("hand_vec");
+ viz::Layer layer_box = arviz.layer("box");
+ layer_box.add(viz::Box{"box"}
+ .set(box)
+ .transformPose(_robot->getGlobalPose())
+ );
gc_draw.draw(b2gc.side("Left"), layer_hand_vec);
gc_draw.draw(b2gc.side("Right"), layer_hand_vec);
@@ -318,7 +322,7 @@ namespace armarx
b2gc.center_grasps(box, "Left", lim, consume_l, side_enabled_l);
b2gc.center_grasps(box, "Right", lim, consume_r, side_enabled_r);
- getArvizClient().commit(layer_l, layer_r, layer_hand_vec);
+ arviz.commit({layer_l, layer_r, layer_hand_vec, layer_box});
_gc_topic->reportGraspCandidates(getName(), graps);
}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index c5f588adf750b8b2eda12375c798b7d80c741dfa..68fbc7165c7d318cf68990f779c074d2f9e12cd4 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -357,7 +357,7 @@ namespace armarx
QAction* attachAgentAction = new QAction(QString::fromStdString(frame), tree);
// attachAgentAction->setStatusTip(tr("Attach object rigidly to a robot node"));
connect(attachAgentAction, &QAction::triggered,
- [ = ]()
+ [ =, this ]()
{
this->attachObjectToRobotNode(providerName, objectID, agentFrames.agent, frame);
});
@@ -368,7 +368,7 @@ namespace armarx
QAction* detachAction = new QAction(tr("Detach from to robot node"), tree);
detachAction->setEnabled(!item->text(OBJECTS_COLUMN_ATTACHMENT).isEmpty());
- connect(detachAction, &QAction::triggered, [ = ]()
+ connect(detachAction, &QAction::triggered, [ =, this ]()
{
this->detachObjectFromRobotNode(providerName, objectID);
});
diff --git a/source/RobotAPI/interface/ArViz/Component.ice b/source/RobotAPI/interface/ArViz/Component.ice
index 6a855c9344eea0f901fbc33e03dcececed41f1e4..33fd7ca5728456079298f4ca34c5aee074d88a62 100644
--- a/source/RobotAPI/interface/ArViz/Component.ice
+++ b/source/RobotAPI/interface/ArViz/Component.ice
@@ -14,7 +14,10 @@ sequence <Element> ElementSeq;
enum LayerAction
{
+ // Create a new layer or update an existing layer
Layer_CREATE_OR_UPDATE,
+
+ // Delete an existing layer
Layer_DELETE,
};
@@ -23,6 +26,8 @@ struct LayerUpdate
string component;
string name;
LayerAction action = Layer_CREATE_OR_UPDATE;
+
+ // Elements are only needed for Layer_CREATE_OR_UPDATE
ElementSeq elements;
};
@@ -41,6 +46,23 @@ struct LayerUpdates
long revision = 0;
};
+struct CommitInput
+{
+ LayerUpdateSeq updates;
+
+ string interactionComponent;
+ Ice::StringSeq interactionLayers;
+};
+
+struct CommitResult
+{
+ // The revision number corresponding to the applied commit
+ long revision = 0;
+
+ // Interactions for the requested layers (see Layer_RECEIVE_INTERACTIONS)
+ InteractionFeedbackSeq interactions;
+};
+
struct RecordingHeader
{
string prefix;
@@ -88,8 +110,13 @@ sequence<Recording> RecordingSeq;
interface StorageInterface
{
+ data::CommitResult commitAndReceiveInteractions(data::CommitInput input);
+
data::LayerUpdates pullUpdatesSince(long revision);
+ data::LayerUpdates pullUpdatesSinceAndSendInteractions(
+ long revision, data::InteractionFeedbackSeq interactions);
+
string startRecording(string prefix);
void stopRecording();
@@ -101,6 +128,8 @@ interface StorageInterface
interface Topic
{
+ // FIXME: The old interface used this topic.
+ // But this call has been superceeded by commitAndReceiveInteractions
void updateLayers(data::LayerUpdateSeq updates);
};
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index bebbb3f414be2184f51fdb8518cdcd368da33ebc..ca27a0e1075e1896e786dd76d74f340a0e7a9404 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -2,7 +2,6 @@
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
-#include <RobotAPI/interface/core/PoseBase.ice>
module armarx
@@ -30,6 +29,84 @@ module data
byte b = 100;
};
+ module InteractionEnableFlags
+ {
+ const int NONE = 0;
+
+ // Make an object selectable
+ const int SELECT = 1;
+ // Enable the context menu for an object
+ const int CONTEXT_MENU = 2;
+
+ // Enable translation along the three axes
+ const int TRANSLATION_X = 4;
+ const int TRANSLATION_Y = 8;
+ const int TRANSLATION_Z = 16;
+
+ // 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;
+ };
+
+ struct InteractionDescription
+ {
+ int enableFlags = 0;
+ Ice::StringSeq contextMenuOptions;
+ };
+
+ module InteractionFeedbackType
+ {
+ const int NONE = 0;
+
+ const int SELECT = 1;
+ const int DESELECT = 2;
+
+ const int CONTEXT_MENU_CHOSEN = 3;
+
+ 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;
+ };
+
+ struct InteractionFeedback
+ {
+ // The type of interaction that happened (in the lower 4 bits)
+ // The higher bits are used for flags specifying more details of the interaction
+ int type = 0;
+
+ // The element with which the interaction took place
+ string component;
+ string layer;
+ string element;
+ // The revision in which the interaction took place
+ long revision = 0;
+
+ // 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;
+ };
+
+ sequence<InteractionFeedback> InteractionFeedbackSeq;
+
module ElementFlags
{
const int NONE = 0;
@@ -40,6 +117,8 @@ module data
class Element
{
string id;
+ InteractionDescription interaction;
+
GlobalPose pose;
float scale = 1.0f;
Color color;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 92a5760ae47f099c3408606903efb25db007f764..4b61a29ecf41a7af19336ccdc81d30c7dc32189a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -75,19 +75,19 @@ namespace armarx
}
// Search for object in datasets.
const std::vector<std::string>& datasets = getDatasets();
- for (const std::string& dataset : datasets)
+ for (const std::string& ds : datasets)
{
- if (fs::is_directory(_rootDirAbs() / dataset / name))
+ if (fs::is_directory(_rootDirAbs() / ds / name))
{
- return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, ds, name);
}
}
std::stringstream ss;
ss << "Did not find object '" << name << "' in any of these datasets:\n";
- for (const path& dataset : datasets)
+ for (const auto& ds : datasets)
{
- ss << "- " << dataset << "\n";
+ ss << "- " << ds << "\n";
}
ss << "Objects root directory: " << _rootDirAbs();
ARMARX_VERBOSE << ss.str();
diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index cce3efe6d385799d6496d39d0d0ca4362177403e..076fd258ef969776ab00016874a8813de38e3c67 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
@@ -8,6 +8,7 @@ armarx_add_library(
RobotStatechartHelpers # Contains RobotNameHelper
RobotAPI::armem
RobotAPI::ArmarXObjects
+ RobotAPI::ArViz
SOURCES box_to_grasp_candidates.cpp
grasp_candidate_drawer.cpp
GraspCandidateHelper.cpp
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
index 859389a12506d1a73734a9111dcae884164ea4d0..7c63d33b767f10ace65279b9be98e9426e5d8644 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
@@ -5,37 +5,62 @@
namespace armarx::plugins
{
+ static const std::string ARVIZ_TOPIC_PROPERTY_NAME = "ArVizTopicName";
+ static const std::string ARVIZ_TOPIC_PROPERTY_DEFAULT = "ArVizTopic";
+
+ static const std::string ARVIZ_STORAGE_PROPERTY_NAME = "ArVizStorageName";
+ static const std::string ARVIZ_STORAGE_PROPERTY_DEFAULT = "ArVizStorage";
+
+
std::string ArVizComponentPlugin::getTopicName()
{
return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(PROPERTY_NAME)) :
- std::string{PROPERTY_DEFAULT};
+ parent<Component>().getProperty<std::string>(makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME)) :
+ ARVIZ_TOPIC_PROPERTY_DEFAULT;
+ }
+
+ std::string ArVizComponentPlugin::getStorageName()
+ {
+ return parentDerives<Component>() ?
+ parent<Component>().getProperty<std::string>(makePropertyName(ARVIZ_STORAGE_PROPERTY_NAME)) :
+ ARVIZ_STORAGE_PROPERTY_DEFAULT;
}
void ArVizComponentPlugin::preOnInitComponent()
{
parent().offeringTopic(getTopicName());
+ parent().usingProxy(getStorageName());
}
void ArVizComponentPlugin::preOnConnectComponent()
{
- parent<ArVizComponentPluginUser>().arviz = createClient();
+ if (parentDerives<ArVizComponentPluginUser>())
+ {
+ parent<ArVizComponentPluginUser>().arviz = createClient();
+ }
}
void ArVizComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(makePropertyName(PROPERTY_NAME)))
+ if (!properties->hasDefinition(makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME)))
{
properties->defineOptionalProperty<std::string>(
- makePropertyName(PROPERTY_NAME),
- PROPERTY_DEFAULT,
+ makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME),
+ ARVIZ_TOPIC_PROPERTY_DEFAULT,
"Name of the ArViz topic");
}
+ if (!properties->hasDefinition(makePropertyName(ARVIZ_STORAGE_PROPERTY_NAME)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(ARVIZ_STORAGE_PROPERTY_NAME),
+ ARVIZ_STORAGE_PROPERTY_DEFAULT,
+ "Name of the ArViz storage");
+ }
}
viz::Client ArVizComponentPlugin::createClient()
{
- return viz::Client(parent(), getTopicName());
+ return viz::Client(parent(), getTopicName(), getStorageName());
}
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
index 6ce746eddb21cf676150d615cb2d9d57f2211d13..beb9ee13d8cbfc994cc8377ad43c7b11e851bcfc 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
@@ -21,9 +21,7 @@ namespace armarx::plugins
std::string getTopicName();
- private:
- static constexpr const char* PROPERTY_NAME = "ArVizTopicName";
- static constexpr const char* PROPERTY_DEFAULT = "ArVizTopic";
+ std::string getStorageName();
};
}
@@ -46,10 +44,6 @@ namespace armarx
armarx::viz::Client& getArvizClient()
{
- if (!arviz.topic())
- {
- arviz = createArVizClient();
- }
return arviz;
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index ea00db45e6987b2df3fd021abf71cbd95d1e6a37..100088eb3f98b6b37ce669b7830d60b20587f59a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -9,6 +9,7 @@ set(LIBS
diffik
RobotStatechartHelpers
RobotUnitDataStreamingReceiver
+ ArViz
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index d256ee202c02d8ebf204455afa241731557815c8..57a714eacc92f1243dcd31d30ea08b1d48648dc0 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -48,7 +48,7 @@ namespace armarx
return;
}
str << ind << n->name << ", profile = " << n->profile << ", value " << n->value << '\n';
- for (const auto c : n->children)
+ for (const auto& c : n->children)
{
writeRobotInfoNode(c, str, indent + 1);
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 65fbc12c2b037ab7b1f7e9101d651e2592e168aa..1c40eac0d8c07c476129e0e852f4e29f57d20144 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -103,7 +103,7 @@ namespace armarx
struct RobotArm
{
friend class RobotNameHelper;
- friend class Arm;
+ friend struct Arm;
public:
std::string getSide() const;
VirtualRobot::RobotNodeSetPtr getKinematicChain() const;
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
index 75096ffed0c58eb6b98245f21ded0d2a81e23109..ecf7f69cdc41df7319c29a9fac28aa0362724a8d 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
@@ -109,7 +109,7 @@ namespace armarx
JsonObjectPtr SimpleJsonLoggerEntry::ToObj(const std::map<std::string, float>& value)
{
JsonObjectPtr obj(new JsonObject);
- for (const std::pair<std::string, float>& pair : value)
+ for (const auto& pair : value)
{
obj->add(pair.first, JsonValue::Create(pair.second));
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
index cb0088285ca161ddc9283f3a024b65a86809fb1b..f0be74213236e8cc13ea2eb0716bcdff98065ff8 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
@@ -43,7 +43,7 @@ namespace armarx::armem::server::obj::clazz
ARMARX_INFO << "Did not find floor class '" << properties.entityName << "'.";
}
}
- arviz.commit(layer);
+ arviz.commit({layer});
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 8b1ae942a95b73119eea072b7f33a539a05fbbdf..7c032c40a0a42914a9810a64b41e5d6b958feef7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -167,7 +167,7 @@ namespace armarx::armem::server::obj::clazz
}
}
- arviz.commit(layerObject, layerOrigin, layerAABB, layerOOBB);
+ arviz.commit({layerObject, layerOrigin, layerAABB, layerOOBB});
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
index 5a731720c49fdb33749edab4ca4c9792eedc0599..b6b0acb6da46d593f2da2677e9e7258ce6d7e0c2 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
@@ -2,6 +2,7 @@
#include <string>
#include <vector>
+#include <optional>
#include <IceUtil/Time.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
index 7482b13b6ab19f1ca5ef38087e1417a8b1479d56..0461a50fcb25567f4df2315e86f44dba2f5e343d 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
@@ -23,6 +23,7 @@
// STD/STL
#include <memory>
#include <string>
+#include <optional>
// ArmarX
#include <RobotAPI/interface/aron.h>
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
index e0f0620a491c120090be226712b5e1e8e9d55411..471b145503c02021a92cee6ba8e1e1599a22d6c5 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
@@ -23,6 +23,7 @@
// STD/STL
#include <memory>
#include <string>
+#include <optional>
// ArmarX
#include <RobotAPI/interface/aron.h>
diff --git a/source/RobotAPI/libraries/aron/core/typereader/xml/Data.h b/source/RobotAPI/libraries/aron/core/typereader/xml/Data.h
index 2fdda0b29d766300ae6364bdacf3ab3a855614a3..34fc92ef71afbfa4bee417ebe4cb1d3f4c47cc39 100644
--- a/source/RobotAPI/libraries/aron/core/typereader/xml/Data.h
+++ b/source/RobotAPI/libraries/aron/core/typereader/xml/Data.h
@@ -26,6 +26,7 @@
// STD/STL
#include <memory>
#include <map>
+#include <optional>
// ArmarX
#include <SimoxUtility/xml.h>
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
index fa5ea9fe7c8ba2e5dab6ea7a8020c47d1e605070..67391514fcba1a51f62ab4488aa1d5f4d54a4a1e 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
@@ -34,6 +34,7 @@ namespace armarx
FramedOrientedPoint::FramedOrientedPoint(const FramedOrientedPoint& source) :
IceUtil::Shared(source),
+ armarx::Serializable(source),
OrientedPointBase(source),
FramedOrientedPointBase(source),
OrientedPoint(source)
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index c5e4d774bf02fdb78bb84c6bb4253bdec6713679..3399214dc4957654f5e8cdefa76a99bdee29640f 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -50,6 +50,7 @@ namespace armarx
FramedDirection::FramedDirection(const FramedDirection& source) :
IceUtil::Shared(source),
+ armarx::Serializable(source),
Vector3Base(source),
FramedDirectionBase(source),
Vector3(source)
@@ -282,6 +283,8 @@ namespace armarx
FramedPose::FramedPose(const FramedPose& pose) :
IceUtil::Shared(pose),
+ armarx::Serializable(pose),
+ armarx::VariantDataClass(pose),
PoseBase(pose),
FramedPoseBase(pose),
Pose(pose)
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 601dc48bbb5df213edf8abbad0cf19bb487ae0b6..27a04fccb69d3b8bf89d91a4e33c45d0d0b36885 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -156,6 +156,7 @@ namespace armarx
//FramedPosition(const Vector3BasePtr pos, const std::string &frame ); // this doesnt work for unknown reasons
FramedPosition(const FramedPosition& other):
Shared(other),
+ armarx::Serializable(other),
Vector3Base(other.x, other.y, other.z),
FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
Vector3(other.x, other.y, other.z)
diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp
index 10b93907b5873f521378987329895c1a7033a960..4a4d285d97a3001b2d5d3eb1ab504604809806c6 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/core/LinkedPose.cpp
@@ -48,6 +48,8 @@ namespace armarx
LinkedPose::LinkedPose(const LinkedPose& other) :
IceUtil::Shared(other),
+ armarx::Serializable(other),
+ armarx::VariantDataClass(other),
PoseBase(other),
FramedPoseBase(other),
LinkedPoseBase(other),
@@ -214,6 +216,7 @@ namespace armarx
LinkedDirection::LinkedDirection(const LinkedDirection& source) :
IceUtil::Shared(source),
+ armarx::Serializable(source),
Vector3Base(source),
FramedDirectionBase(source),
LinkedDirectionBase(source),
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 0bca83408ff4831cd028c664aed9c20f7e056c1f..39968afc088d5ed82cad8461cfd89cb96a75e321 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -277,6 +277,8 @@ namespace armarx
Pose::Pose(const Pose& source) :
IceUtil::Shared(source),
+ armarx::Serializable(source),
+ armarx::VariantDataClass(source),
PoseBase(source)
{
orientation = QuaternionBasePtr::dynamicCast(source.orientation->clone());
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index f8068131cc9a3d0b939ff06242505195310f830a..1c75dcd1cf70470595a5f2f0f242e1106c287229 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -176,6 +176,8 @@ namespace armarx
Trajectory::Trajectory(const Trajectory& source) :
IceUtil::Shared(source),
+ armarx::Serializable(source),
+ armarx::VariantDataClass(source),
TrajectoryBase(source)
{
CopyData(source, *this);