diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp index 6299664899bd830a63b1b0ac245529f697f06b2b..2df6e08e9b7cde6a129f5459091ba1df578ea7b1 100644 --- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp +++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp @@ -150,7 +150,7 @@ namespace armarx { layer = historyEntry; found = true; - continue; + break; } } if (!found) @@ -178,6 +178,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::remove_if(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 true; + } + } + } + return false; + }); + + result.interactions.assign(foundInteractionsBegin, interactionsEnd); + interactionBuffer.erase(foundInteractionsBegin, interactionsEnd); + } + } + + return result; + } + viz::data::LayerUpdates ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&) diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h index a7ffb87098fd6ad1ca276f1ada3af3a161745a48..f01a68fba62bd7f9fb961dcb182480774ec848de 100644 --- a/source/RobotAPI/components/ArViz/ArVizStorage.h +++ b/source/RobotAPI/components/ArViz/ArVizStorage.h @@ -83,6 +83,8 @@ 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, diff --git a/source/RobotAPI/interface/ArViz/Component.ice b/source/RobotAPI/interface/ArViz/Component.ice index 8972819eaa75a48063ac67234dc2f854caaecc58..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,6 +110,8 @@ sequence<Recording> RecordingSeq; interface StorageInterface { + data::CommitResult commitAndReceiveInteractions(data::CommitInput input); + data::LayerUpdates pullUpdatesSince(long revision); data::LayerUpdates pullUpdatesSinceAndSendInteractions( @@ -104,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); };