diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt index 7debb7e6f4aa280c4e28cc1a40bbc59af0bf25c3..43e38a886433e9ec8cc35bf23e02cb7a19a17c1c 100644 --- a/source/RobotAPI/components/CMakeLists.txt +++ b/source/RobotAPI/components/CMakeLists.txt @@ -32,3 +32,5 @@ add_subdirectory(RobotToArViz) add_subdirectory(StatechartExecutorExample) add_subdirectory(TopicTimingTest) add_subdirectory(ViewSelection) + +add_subdirectory(InteractiveMemoryEditor) diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/CMakeLists.txt b/source/RobotAPI/components/InteractiveMemoryEditor/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..b52a46ecbfc8ce2f5ab554c33e3a9380894f1e7a --- /dev/null +++ b/source/RobotAPI/components/InteractiveMemoryEditor/CMakeLists.txt @@ -0,0 +1,27 @@ +armarx_component_set_name("InteractiveMemoryEditor") + + +set(COMPONENT_LIBS + # ArmarXCore + ArmarXCoreComponentPlugins + # RobotAPI + RobotAPI::ArmarXObjects + RobotAPI::ArViz + RobotAPI::ComponentPlugins +) + +set(SOURCES + InteractiveMemoryEditor.cpp + InteractionObserver.cpp +) +set(HEADERS + InteractiveMemoryEditor.h + InteractionObserver.h +) + + +armarx_add_component("${SOURCES}" "${HEADERS}") + + +#generate the application +armarx_generate_and_add_component_executable() diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.cpp new file mode 100644 index 0000000000000000000000000000000000000000..806125859b963d1d7a4d550144f4e18fe25b4d88 --- /dev/null +++ b/source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.cpp @@ -0,0 +1,58 @@ +#include "InteractionObserver.h" + +#include <utility> + +armarx::InteractionObserver::Observation &armarx::InteractionObserver::Observation::onContextMenu(size_t index, std::function<void()> action) +{ + contextMenuActions[index] = std::move(action); + return *this; +} + +void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback const& interaction) +{ + if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen) + { + auto iterator = contextMenuActions.find(interaction.chosenContextMenuEntry()); + + if (iterator != contextMenuActions.end()) + { + auto [index, action] = *iterator; + action(); + } + } +} + +void armarx::InteractionObserver::clearObservedLayer(armarx::viz::Layer & layer) +{ + layer.clear(); + observedLayers.erase(layer.data_.name); +} + +void armarx::InteractionObserver::requestInteractions(armarx::viz::StagedCommit& stage) +{ + for (auto& [name, observedLayer] : observedLayers) + { + stage.requestInteraction(observedLayer.layer); + } +} + +void armarx::InteractionObserver::process(viz::InteractionFeedbackRange const& interactions) +{ + for (viz::InteractionFeedback const& interaction: interactions) + { + auto layerIterator = observedLayers.find(interaction.layer()); + + if (layerIterator != observedLayers.end()) + { + auto& [name, layer] = *layerIterator; + + auto observationIterator = layer.observations.find(interaction.element()); + + if (observationIterator != layer.observations.end()) + { + auto& [element, observation] = *observationIterator; + observation.process(interaction); + } + } + } +} diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.h b/source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.h new file mode 100644 index 0000000000000000000000000000000000000000..9230d1ede86f92a1fa84ac2f9e3a131dd4c143f6 --- /dev/null +++ b/source/RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.h @@ -0,0 +1,53 @@ +#pragma once + +#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h> + +namespace armarx +{ + class InteractionObserver + { + public: + class Observation + { + public: + Observation& onContextMenu(size_t index, std::function<void()> action); + + void process(viz::InteractionFeedback const& interaction); + + private: + std::map<size_t, std::function<void()>> contextMenuActions; + }; + + template <typename ElementT> + Observation& addObserved(viz::Layer & layer, ElementT const& element) + { + layer.add(element); + + auto iterator = observedLayers.find(layer.data_.name); + + if (iterator == observedLayers.end()) + { + std::tie(iterator, std::ignore) = observedLayers.emplace(layer.data_.name, layer); + } + + auto& [name, observedLayer] = *iterator; + return observedLayer.observations[element.data_->id]; + } + + void clearObservedLayer(viz::Layer & layer); + + void requestInteractions(viz::StagedCommit & stage); + void process(viz::InteractionFeedbackRange const& interactions); + + private: + struct ObservedLayer + { + explicit ObservedLayer(viz::Layer & layer) : layer(layer) {} + + std::reference_wrapper<viz::Layer> layer; + std::map<std::string, Observation> observations; + }; + + std::map<std::string, ObservedLayer> observedLayers; + }; +} // namespace armarx diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp new file mode 100644 index 0000000000000000000000000000000000000000..acfbe4e595acef54ebf7b43280d318b36685618e --- /dev/null +++ b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp @@ -0,0 +1,99 @@ +#include "InteractiveMemoryEditor.h" + +#include <ArmarXCore/core/time/CycleUtil.h> + +#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h> +#include <RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.h> + +namespace armarx +{ + armarx::PropertyDefinitionsPtr InteractiveMemoryEditor::createPropertyDefinitions() + { + armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier()); + + return defs; + } + + std::string InteractiveMemoryEditor::getDefaultName() const + { + return "InteractiveMemoryEditor"; + } + + void InteractiveMemoryEditor::onInitComponent() + { + } + + void InteractiveMemoryEditor::onConnectComponent() + { + setDebugObserverBatchModeEnabled(true); + + isPushRequired = false; + isPullRequired = true; + + objectVizTask = new SimpleRunningTask<>([this]() + { + this->objectVizTaskRun(); + }); + objectVizTask->start(); + } + + void InteractiveMemoryEditor::onDisconnectComponent() + { + } + + void InteractiveMemoryEditor::onExitComponent() + { + } + + + void InteractiveMemoryEditor::objectVizTaskRun() + { + CycleUtil cycle(50); + + objpose::ObjectPoseClient client = getClient(); + + viz::Layer memoryLayer = arviz.layer(MEMORY_LAYER_NAME); + + InteractionObserver observer; + + while (objectVizTask && !objectVizTask->isStopped()) + { + viz::StagedCommit stage = arviz.stage(); + + if (isPullRequired) + { + observer.clearObservedLayer(memoryLayer); + + const objpose::ObjectPoseSeq objectPoses = client.fetchObjectPoses(); + + for (const objpose::ObjectPose &objectPose: objectPoses) + { + observer.addObserved( + memoryLayer, + viz::Object(objectPose.objectID.str()) + .pose(objectPose.objectPoseGlobal) + .fileByObjectFinder(objectPose.objectID) + .alpha(objectPose.confidence) + .enable(viz::interaction() + .selection() + .transform() + .hideDuringTransform() + .contextMenu({"Push", "Pull"}))) + .onContextMenu(0, [this] { this->isPushRequired = true; }) + .onContextMenu(1, [this] { this->isPullRequired = true; }); + } + + stage.add(memoryLayer); + isPullRequired = false; + } + + observer.requestInteractions(stage); + + viz::CommitResult result = arviz.commit(stage); + + observer.process(result.interactions()); + + cycle.waitForCycleDuration(); + } + } +} // namespace armarx diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.h b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.h new file mode 100644 index 0000000000000000000000000000000000000000..fda378e384451d3cd08611dc2fd6a28450277f22 --- /dev/null +++ b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.h @@ -0,0 +1,45 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <ArmarXCore/core/services/tasks/TaskUtil.h> +#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h> + +#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h> + +#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h> + +namespace armarx +{ + class InteractiveMemoryEditor : + virtual public armarx::Component, + virtual public armarx::DebugObserverComponentPluginUser, + virtual public armarx::ArVizComponentPluginUser, + virtual public armarx::ObjectPoseClientPluginUser + { + public: + std::string getDefaultName() const override; + + + protected: + armarx::PropertyDefinitionsPtr createPropertyDefinitions() override; + + void onInitComponent() override; + + void onConnectComponent() override; + + void onDisconnectComponent() override; + + void onExitComponent() override; + + + private: + void objectVizTaskRun(); + + armarx::SimpleRunningTask<>::pointer_type objectVizTask; + + bool isPushRequired; + bool isPullRequired; + + static constexpr const char* MEMORY_LAYER_NAME = "Memory"; + }; +} // namespace armarx