diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index decbf95c39a16482ee3bc6975396812397c3d37e..434310e1d26d935d5bd8b5c15444960518a41a28 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -22,19 +22,23 @@
#include "ArVizStorage.h"
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/util/IceBlobToObject.h>
-#include <ArmarXCore/core/util/ObjectToIceBlob.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <algorithm>
+#include <optional>
#include <SimoxUtility/json/json.hpp>
-#include <iomanip>
-#include <optional>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/time.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/util/IceBlobToObject.h>
+#include <ArmarXCore/core/util/ObjectToIceBlob.h>
namespace armarx
{
- static std::filesystem::path getAbsolutePath(const std::filesystem::path& path)
+ static std::filesystem::path
+ getAbsolutePath(const std::filesystem::path& path)
{
if (path.is_absolute())
{
@@ -56,53 +60,53 @@ namespace armarx
}
}
-
- std::string ArVizStorage::getDefaultName() const
+ std::string
+ ArVizStorage::getDefaultName() const
{
return "ArVizStorage";
}
-
- armarx::PropertyDefinitionsPtr ArVizStorage::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ArVizStorage::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr defs(
+ new ComponentPropertyDefinitions(getConfigIdentifier()));
- defs->optional(topicName, "TopicName",
- "Layer updates are sent over this topic.");
+ defs->optional(
+ properties_.topicName, "TopicName", "Layer updates are sent over this topic.");
- defs->optional(maxHistorySize, "MaxHistorySize",
+ defs->optional(properties_.maxHistorySize,
+ "MaxHistorySize",
"How many layer updates are saved in the history until they are compressed")
- .setMin(0);
+ .setMin(0);
- defs->defineOptionalProperty<std::string>(
- "HistoryPath", "RobotAPI/ArVizStorage",
- "Destination path where the history is serialized to");
+ defs->optional(properties_.historyPath,
+ "HistoryPath",
+ "Destination path where the history is serialized to");
return defs;
}
-
- void ArVizStorage::onInitComponent()
+ void
+ ArVizStorage::onInitComponent()
{
- std::filesystem::path historyPathProp = getProperty<std::string>("HistoryPath").getValue();
- historyPath = getAbsolutePath(historyPathProp);
- if (!std::filesystem::exists(historyPath))
+ properties_.historyPath = getAbsolutePath(properties_.historyPath);
+ if (!std::filesystem::exists(properties_.historyPath))
{
- ARMARX_INFO << "Creating history path: " << historyPath;
+ ARMARX_INFO << "Creating history path: " << properties_.historyPath;
std::error_code error;
- std::filesystem::create_directory(historyPath, error);
+ std::filesystem::create_directory(properties_.historyPath, error);
if (error)
{
- ARMARX_WARNING << "Could not create directory for history: \n"
- << error.message();
+ ARMARX_WARNING << "Could not create directory for history: \n" << error.message();
}
}
- usingTopic(topicName);
+ usingTopic(properties_.topicName);
}
-
- void ArVizStorage::onConnectComponent()
+ void
+ ArVizStorage::onConnectComponent()
{
revision = 0;
currentState.clear();
@@ -113,8 +117,8 @@ namespace armarx
recordingMetaData.id = "";
}
-
- void ArVizStorage::onDisconnectComponent()
+ void
+ ArVizStorage::onDisconnectComponent()
{
if (recordingTask)
{
@@ -123,20 +127,51 @@ namespace armarx
}
}
-
- void ArVizStorage::onExitComponent()
+ void
+ ArVizStorage::onExitComponent()
{
}
-
- void ArVizStorage::updateLayers(viz::data::LayerUpdateSeq const& updates, const Ice::Current&)
+ void
+ ArVizStorage::updateLayers(viz::data::LayerUpdateSeq const& updates, const Ice::Current&)
{
std::unique_lock<std::mutex> lock(historyMutex);
revision += 1;
+
IceUtil::Time now = IceUtil::Time::now();
long nowInMicroSeconds = now.toMicroSeconds();
+ if (not updates.empty())
+ {
+ const std::string& componentName = updates.front().component;
+
+ auto& history = updateHistoryForComponents[componentName];
+ history.push_back(armarx::Clock::Now());
+
+ const auto maxHistoryDur = armarx::Duration::SecondsDouble(1);
+
+ const DateTime referenceNow = Clock::Now();
+ const auto isOutdated = [&referenceNow,
+ &maxHistoryDur](const DateTime& timestamp) -> bool
+ { return (referenceNow - timestamp) > maxHistoryDur; };
+
+ // trim history
+ history.erase(std::remove_if(history.begin(), history.end(), isOutdated),
+ history.end());
+
+ ARMARX_VERBOSE << deactivateSpam(1, componentName) << componentName << ": "
+ << history.size() / maxHistoryDur.toSecondsDouble() << " Hz";
+
+ if (history.size() > properties_.componentWarnFrequency)
+ {
+ ARMARX_WARNING << "Component `" << componentName << "`"
+ << "sends data at a too high rate ("
+ << history.size() / maxHistoryDur.toSecondsDouble() << ")";
+ }
+ }
+
+
for (auto& update : updates)
{
if (update.component.empty())
@@ -157,8 +192,7 @@ namespace armarx
bool found = false;
for (auto& layer : currentState)
{
- if (layer.update.component == update.component
- && layer.update.name == update.name)
+ if (layer.update.component == update.component && layer.update.name == update.name)
{
layer = historyEntry;
found = true;
@@ -172,7 +206,7 @@ namespace armarx
}
long currentHistorySize = history.size();
- if (currentHistorySize >= maxHistorySize)
+ if (currentHistorySize >= properties_.maxHistorySize)
{
{
std::unique_lock<std::mutex> lock(recordingMutex);
@@ -190,7 +224,9 @@ namespace armarx
}
}
- viz::data::CommitResult ArVizStorage::commitAndReceiveInteractions(viz::data::CommitInput const& input, const Ice::Current&)
+ viz::data::CommitResult
+ ArVizStorage::commitAndReceiveInteractions(viz::data::CommitInput const& input,
+ const Ice::Current&)
{
viz::data::CommitResult result;
@@ -215,8 +251,8 @@ namespace armarx
bool found = false;
for (viz::data::TimestampedLayerUpdate& layer : currentState)
{
- if (layer.update.component == update.component
- && layer.update.name == update.name)
+ if (layer.update.component == update.component &&
+ layer.update.name == update.name)
{
layer = historyEntry;
found = true;
@@ -231,7 +267,7 @@ namespace armarx
// Trim the history if max size has been exceeded
long currentHistorySize = history.size();
- if (currentHistorySize >= maxHistorySize)
+ if (currentHistorySize >= properties_.maxHistorySize)
{
{
std::unique_lock<std::mutex> lock(recordingMutex);
@@ -252,21 +288,23 @@ namespace armarx
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;
- });
+ 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);
@@ -276,9 +314,8 @@ namespace armarx
return result;
}
-
-
- viz::data::LayerUpdates ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
+ viz::data::LayerUpdates
+ ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
{
viz::data::LayerUpdates result;
@@ -297,12 +334,16 @@ namespace armarx
return result;
}
- static const int ALL_TRANSFORM_FLAGS = viz::data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG
- | viz::data::InteractionFeedbackType::TRANSFORM_DURING_FLAG
- | viz::data::InteractionFeedbackType::TRANSFORM_END_FLAG;
+ static const int ALL_TRANSFORM_FLAGS =
+ viz::data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG |
+ viz::data::InteractionFeedbackType::TRANSFORM_DURING_FLAG |
+ viz::data::InteractionFeedbackType::TRANSFORM_END_FLAG;
- viz::data::LayerUpdates ArVizStorage::pullUpdatesSinceAndSendInteractions(
- Ice::Long revision, viz::data::InteractionFeedbackSeq const& interactions, const Ice::Current& c)
+ viz::data::LayerUpdates
+ ArVizStorage::pullUpdatesSinceAndSendInteractions(
+ Ice::Long revision,
+ viz::data::InteractionFeedbackSeq const& interactions,
+ const Ice::Current& c)
{
viz::data::LayerUpdates result;
@@ -312,9 +353,8 @@ namespace armarx
{
for (viz::data::InteractionFeedback& entry : interactionBuffer)
{
- if (entry.component == interaction.component
- && entry.layer == interaction.layer
- && entry.element == interaction.element)
+ if (entry.component == interaction.component && entry.layer == interaction.layer &&
+ entry.element == interaction.element)
{
int previousTransformFlags = entry.type & ALL_TRANSFORM_FLAGS;
int interactionTransformFlags = interaction.type & ALL_TRANSFORM_FLAGS;
@@ -333,7 +373,7 @@ namespace armarx
// Interaction did not exist, add it to the buffer
interactionBuffer.push_back(interaction);
- for_end: ;
+ for_end:;
}
result.updates.reserve(currentState.size());
@@ -349,7 +389,8 @@ namespace armarx
return result;
}
- void ArVizStorage::record()
+ void
+ ArVizStorage::record()
{
while (!recordingTask->isStopped())
{
@@ -365,12 +406,13 @@ namespace armarx
recordingBuffer.clear();
}
}
-}
+} // namespace armarx
namespace armarx::viz::data
{
- void to_json(nlohmann::json& j, RecordingBatchHeader const& batch)
+ void
+ to_json(nlohmann::json& j, RecordingBatchHeader const& batch)
{
j["index"] = batch.index;
j["firstRevision"] = batch.firstRevision;
@@ -379,16 +421,18 @@ namespace armarx::viz::data
j["lastTimestampInMicroSeconds"] = batch.lastTimestampInMicroSeconds;
}
- void from_json(nlohmann::json const& j, RecordingBatchHeader& batch)
+ void
+ from_json(nlohmann::json const& j, RecordingBatchHeader& batch)
{
- batch.index = j["index"] ;
+ batch.index = j["index"];
batch.firstRevision = j["firstRevision"];
batch.lastRevision = j["lastRevision"];
batch.firstTimestampInMicroSeconds = j["firstTimestampInMicroSeconds"];
batch.lastTimestampInMicroSeconds = j["lastTimestampInMicroSeconds"];
}
- void to_json(nlohmann::json& j, Recording const& recording)
+ void
+ to_json(nlohmann::json& j, Recording const& recording)
{
j["id"] = recording.id;
j["firstRevision"] = recording.firstRevision;
@@ -398,7 +442,8 @@ namespace armarx::viz::data
j["batchHeaders"] = recording.batchHeaders;
}
- void from_json(nlohmann::json const& j, Recording& recording)
+ void
+ from_json(nlohmann::json const& j, Recording& recording)
{
recording.id = j["id"];
recording.firstRevision = j["firstRevision"];
@@ -408,10 +453,10 @@ namespace armarx::viz::data
j["batchHeaders"].get_to(recording.batchHeaders);
}
-}
+} // namespace armarx::viz::data
-static bool writeCompleteFile(std::string const& filename,
- const void* data, std::size_t size)
+static bool
+writeCompleteFile(std::string const& filename, const void* data, std::size_t size)
{
FILE* file = fopen(filename.c_str(), "wb");
if (!file)
@@ -427,12 +472,13 @@ static bool writeCompleteFile(std::string const& filename,
return true;
}
-static std::string readCompleteFile(std::filesystem::path const& path)
+static std::string
+readCompleteFile(std::filesystem::path const& path)
{
FILE* f = fopen(path.string().c_str(), "rb");
fseek(f, 0, SEEK_END);
long fsize = ftell(f);
- fseek(f, 0, SEEK_SET); /* same as rewind(f); */
+ fseek(f, 0, SEEK_SET); /* same as rewind(f); */
std::string result(fsize, '\0');
std::size_t read = fread(result.data(), 1, fsize, f);
@@ -442,7 +488,8 @@ static std::string readCompleteFile(std::filesystem::path const& path)
return result;
}
-static std::optional<armarx::viz::data::Recording> readRecordingInfo(std::filesystem::path const& recordingDirectory)
+static std::optional<armarx::viz::data::Recording>
+readRecordingInfo(std::filesystem::path const& recordingDirectory)
{
std::optional<::armarx::viz::data::Recording> result;
@@ -472,12 +519,14 @@ static std::optional<armarx::viz::data::Recording> readRecordingInfo(std::filesy
}
}
-static std::string batchFileName(armarx::viz::data::RecordingBatchHeader const& batchHeader)
+static std::string
+batchFileName(armarx::viz::data::RecordingBatchHeader const& batchHeader)
{
return std::to_string(batchHeader.firstRevision) + ".bin";
}
-void armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
+void
+armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
{
if (batch.updates.empty())
{
@@ -492,7 +541,7 @@ void armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
batch.header.lastRevision = last.revision;
batch.header.firstTimestampInMicroSeconds = first.timestampInMicroseconds;
batch.header.lastTimestampInMicroSeconds = last.timestampInMicroseconds;
- if (firstBatch)
+ if (firstBatch)
{
batch.initialState = currentState;
firstBatch = false;
@@ -523,7 +572,8 @@ void armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
}
recordingMetaData.lastTimestampInMicroSeconds = last.timestampInMicroseconds;
- armarx::viz::data::RecordingBatchHeader& newBatch = recordingMetaData.batchHeaders.emplace_back();
+ armarx::viz::data::RecordingBatchHeader& newBatch =
+ recordingMetaData.batchHeaders.emplace_back();
newBatch.index = recordingMetaData.batchHeaders.size() - 1;
newBatch.firstRevision = first.revision;
newBatch.lastRevision = last.revision;
@@ -543,26 +593,26 @@ void armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
ARMARX_INFO << "Recorded ArViz batch to: " << filePath;
}
-std::string armarx::ArVizStorage::startRecording(std::string const& newRecordingPrefix, const Ice::Current&)
+std::string
+armarx::ArVizStorage::startRecording(std::string const& newRecordingPrefix, const Ice::Current&)
{
{
std::unique_lock<std::mutex> lock(recordingMutex);
if (recordingMetaData.id.size() > 0)
{
- ARMARX_WARNING << "Could not start recording with prefix " << newRecordingPrefix
- << "\nbecause there is already a recording running for the recording ID: "
- << recordingMetaData.id;
+ ARMARX_WARNING
+ << "Could not start recording with prefix " << newRecordingPrefix
+ << "\nbecause there is already a recording running for the recording ID: "
+ << recordingMetaData.id;
return recordingMetaData.id;
}
IceUtil::Time now = IceUtil::Time::now();
std::ostringstream id;
- id << newRecordingPrefix
- << '_'
- << now.toString("%Y-%m-%d_%H-%M-%S");
+ id << newRecordingPrefix << '_' << now.toString("%Y-%m-%d_%H-%M-%S");
std::string newRecordingID = id.str();
- recordingPath = historyPath / newRecordingID;
+ recordingPath = properties_.historyPath / newRecordingID;
if (!std::filesystem::exists(recordingPath))
{
ARMARX_INFO << "Creating directory for recording with ID '" << newRecordingID
@@ -594,7 +644,8 @@ std::string armarx::ArVizStorage::startRecording(std::string const& newRecording
return "";
}
-void armarx::ArVizStorage::stopRecording(const Ice::Current&)
+void
+armarx::ArVizStorage::stopRecording(const Ice::Current&)
{
if (!recordingTask)
{
@@ -616,12 +667,14 @@ void armarx::ArVizStorage::stopRecording(const Ice::Current&)
recordingMetaData.firstTimestampInMicroSeconds = -1;
}
-armarx::viz::data::RecordingsInfo armarx::ArVizStorage::getAllRecordings(const Ice::Current&)
+armarx::viz::data::RecordingsInfo
+armarx::ArVizStorage::getAllRecordings(const Ice::Current&)
{
viz::data::RecordingsInfo recordingsInfo;
viz::data::RecordingSeq result;
- for (std::filesystem::directory_entry const& entry : std::filesystem::directory_iterator(historyPath))
+ for (std::filesystem::directory_entry const& entry :
+ std::filesystem::directory_iterator(properties_.historyPath))
{
ARMARX_DEBUG << "Checking: " << entry.path();
@@ -638,17 +691,20 @@ armarx::viz::data::RecordingsInfo armarx::ArVizStorage::getAllRecordings(const I
}
recordingsInfo.recordings = result;
- recordingsInfo.recordingsPath = historyPath;
+ recordingsInfo.recordingsPath = properties_.historyPath;
return recordingsInfo;
}
-armarx::viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID, Ice::Long batchIndex, const Ice::Current&)
+armarx::viz::data::RecordingBatch
+armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID,
+ Ice::Long batchIndex,
+ const Ice::Current&)
{
viz::data::RecordingBatch result;
result.header.index = -1;
- std::filesystem::path recordingPath = historyPath / recordingID;
+ std::filesystem::path recordingPath = properties_.historyPath / recordingID;
std::optional<viz::data::Recording> recording = readRecordingInfo(recordingPath);
if (!recording)
{
@@ -669,8 +725,7 @@ armarx::viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::s
if (!std::filesystem::exists(batchFile))
{
ARMARX_WARNING << "Could not find batch file for recording '" << recordingID
- << "' with index " << batchIndex
- << "\nPath: " << batchFile;
+ << "' with index " << batchIndex << "\nPath: " << batchFile;
return result;
}
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h
index 36d4d85564f5817d042b7607c11295cd527e2d69..60305bfd987fe1c75e04f86ab0665095a788447f 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.h
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.h
@@ -22,15 +22,16 @@
#pragma once
-#include <RobotAPI/interface/ArViz.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-
-#include <mutex>
#include <atomic>
#include <condition_variable>
#include <filesystem>
+#include <mutex>
+#include "ArmarXCore/core/time/DateTime.h"
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
+#include <RobotAPI/interface/ArViz.h>
namespace armarx
{
@@ -53,12 +54,11 @@ namespace armarx
* @brief Stores visualization elements drawn by ArViz clients.
*
*/
- class ArVizStorage
- : virtual public armarx::Component
- , virtual public armarx::viz::StorageAndTopicInterface
+ class ArVizStorage :
+ virtual public armarx::Component,
+ virtual public armarx::viz::StorageAndTopicInterface
{
public:
-
/// armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -83,17 +83,19 @@ 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::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;
+ 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::RecordingsInfo getAllRecordings(const Ice::Current&) override;
- viz::data::RecordingBatch getRecordingBatch(const std::string&, Ice::Long, const Ice::Current&) override;
+ viz::data::RecordingBatch
+ getRecordingBatch(const std::string&, Ice::Long, const Ice::Current&) override;
private:
void record();
@@ -101,9 +103,15 @@ namespace armarx
void recordBatch(viz::data::RecordingBatch& batch);
private:
- std::string topicName = "ArVizTopic";
- int maxHistorySize = 1000;
- std::filesystem::path historyPath;
+ struct Properties
+ {
+ std::string topicName = "ArVizTopic";
+ int maxHistorySize = 1000;
+ std::filesystem::path historyPath = "RobotAPI/ArVizStorage";
+
+ float componentWarnFrequency = 25;
+
+ } properties_;
std::mutex historyMutex;
@@ -124,5 +132,8 @@ namespace armarx
viz::data::Recording recordingMetaData;
std::condition_variable recordingCondition;
RunningTask<ArVizStorage>::pointer_type recordingTask;
+
+
+ std::map<std::string, std::vector<armarx::DateTime>> updateHistoryForComponents;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 12dc68399fc92b3d32816dd212d75fc4b0cdab8c..71dad203ab70fab4b411ad891b55b781b9fc7e39 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -1,12 +1,11 @@
#include "VisualizationRobot.h"
-#include <fstream>
-#include <regex>
-
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -114,17 +113,23 @@ namespace armarx::viz::coin
else
{
// 2) We do not have unused instances in the pool ==> Clone one
- ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", "
+ ARMARX_INFO << "Cloning robot from cache " << VAROUT(project) << ", "
<< VAROUT(filename);
- if (instancePool.robots.size() > 0)
+ if (!instancePool.robots.empty())
{
VirtualRobot::RobotPtr const& robotToClone =
instancePool.robots.front();
float scaling = 1.0f;
bool preventCloningMeshesIfScalingIs1 = true;
- result.robot = robotToClone->clone(
+
+ {
+ armarx::core::time::ScopedStopWatch sw([](const armarx::Duration& duration){
+ ARMARX_DEBUG << "Cloning took " << duration.toSecondsDouble() << " seconds.";
+ });
+ result.robot = robotToClone->clone(
nullptr, scaling, preventCloningMeshesIfScalingIs1);
+ }
// Insert the cloned robot into the instance pool
instancePool.robots.push_back(result.robot);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index e577eb166ebc77bbbb66dc4ecfe47b9728b2a2be..f6c3146f7877990053cc372670f9fb5800c047bc 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -72,6 +72,20 @@ namespace armarx::objpose
return std::nullopt;
}
+
+ std::optional<ObjectPose>
+ ObjectPoseClient::fetchObjectPoseFromProvider(const ObjectID& objectID, const std::string& providerName) const
+ {
+ const auto objectPoses = fetchObjectPosesFromProvider(providerName);
+ const auto *object = findObjectPoseByID(objectPoses, objectID);
+
+ if(object != nullptr)
+ {
+ return *object;
+ }
+
+ return std::nullopt;
+ }
ObjectPoseSeq
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index 8a1cb31f23f30b37724177aaff7ba30e77ec5604..f2bfb04410bd1d0ba0a3cec4bc9eed14edd26dfb 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -76,7 +76,19 @@ namespace armarx::objpose
* @return The object's pose, if known.
*/
std::optional<ObjectPose>
- fetchObjectPose(const ObjectID& objectID) const;
+ fetchObjectPose(const ObjectID& objectID) const;
+
+ /**
+ * @brief Fetch the pose of a single object and a single provider.
+ *
+ * This is a network call. If you need multiple object poses, use
+ * `fetchObjectPoses()` instead.
+ *
+ * @param objectID The object's ID.
+ * @return The object's pose, if known.
+ */
+ std::optional<ObjectPose>
+ fetchObjectPoseFromProvider(const ObjectID& objectID, const std::string& providerName) const;
/**
* @brief Fetch object poses from a specific provider.
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index 7ab4f089abe260f55ff832fb7777d4c16f8b2dc3..800a22e47d9afaa7cf564bda93ea20b68234d574 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -34,9 +34,11 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
@@ -44,6 +46,7 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+
namespace armarx
{
@@ -138,6 +141,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromFile(const std::string& filename)
{
+ ARMARX_IMPORTANT << filename;
+
ARMARX_TRACE;
const auto ext = std::filesystem::path(filename).extension();
if (ext == ".xml")
@@ -159,15 +164,37 @@ namespace armarx
inline void
from_json(const nlohmann::json& j, GraspTrajectoryKeypoint& kp)
{
- const std::optional<std::string> shape =
- j.contains("shape") ? std::optional<std::string>(j.at("shape").get<std::string>())
- : std::nullopt;
+ ARMARX_TRACE;
+ std::optional<std::string> shape;
- const std::map<std::string, float> targetHandValues =
- j.contains("HandValues") ? j.at("HandValues").get<std::map<std::string, float>>()
- : std::map<std::string, float>{};
+ if (j.contains("shape"))
+ {
+ if (not j.at("shape").is_null())
+ {
+ shape = j.at("shape");
+ }
+ }
+
+ ARMARX_TRACE;
+ std::optional<std::map<std::string, float>> targetHandValues;
- kp = GraspTrajectoryKeypoint(j.at("Pose"), targetHandValues, shape);
+ if (j.contains("fingerValues"))
+ {
+ if (not j.at("fingerValues").is_null())
+ {
+ targetHandValues = j.at("fingerValues");
+ }
+ }
+
+ // VERY hacky!
+ const Eigen::Matrix<float, 4, 4, Eigen::RowMajor> aronTf = j.at("pose");
+ const Eigen::Matrix4f tf = aronTf.transpose();
+
+ ARMARX_IMPORTANT << tf;
+
+ ARMARX_TRACE;
+ kp = GraspTrajectoryKeypoint(
+ tf, targetHandValues.value_or(std::map<std::string, float>{}), shape);
}
GraspTrajectoryPtr
@@ -179,9 +206,27 @@ namespace armarx
const nlohmann::json j = nlohmann::json::parse(ifs);
std::vector<GraspTrajectoryKeypoint> traj;
- traj = j;
+ traj = j.at("keypoints");
- return std::make_shared<GraspTrajectory>(traj);
+ const armarx::Duration duration =
+ armarx::Duration::MicroSeconds(j.at("duration").at("microSeconds").get<std::int64_t>());
+
+ // at the moment, we assume that all keypoints are sampled equidistant
+
+ ARMARX_CHECK_NOT_EMPTY(traj);
+ const float dtSeconds = duration.toSecondsDouble() / (traj.size() - 1);
+
+ for (auto& kp : traj)
+ {
+ kp.dt = dtSeconds;
+ }
+
+ const std::string frame = j.at("frame").at("frame");
+
+ auto graspTrajectory = std::make_shared<GraspTrajectory>(traj);
+ graspTrajectory->setFrame(frame);
+
+ return graspTrajectory;
}
GraspTrajectoryPtr
@@ -557,7 +602,8 @@ namespace armarx
traj->addKeypoint(
::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
kp->handJointsTarget,
- kp->dt);
+ kp->dt,
+ kp->shape);
}
return traj;
}
@@ -566,12 +612,13 @@ namespace armarx
GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform) const
{
ARMARX_TRACE;
- GraspTrajectoryPtr traj(
- new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ GraspTrajectoryPtr traj(new GraspTrajectory(
+ transform * getStartPose(), getKeypoint(0)->handJointsTarget, getKeypoint(0)->shape));
for (size_t i = 1; i < keypoints.size(); i++)
{
const KeypointPtr& kp = keypoints.at(i);
- traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
+ traj->addKeypoint(
+ transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt, kp->shape);
}
return traj;
}
@@ -620,7 +667,7 @@ namespace armarx
const GraspTrajectory::KeypointPtr& kp = getKeypoint(i);
Eigen::Matrix4f target_pose = kp->getTargetPose();
target_pose.block<3, 3>(0, 0) = flip_yz * target_pose.block<3, 3>(0, 0) * flip_yz;
- output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt);
+ output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt, kp->shape);
}
return output_trajectory;
}
@@ -721,4 +768,16 @@ namespace armarx
feedForwardOriVelocity(0, 0, 0)
{
}
+
+ void
+ GraspTrajectory::setFrame(const std::string& frame)
+ {
+ frame_ = frame;
+ }
+
+ const std::optional<std::string>&
+ GraspTrajectory::getFrame() const
+ {
+ return frame_;
+ }
} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
index 718642c2ae5028e4cbf7cbee4cabf6fcc4373497..15786c619c4833f5e77e23148cc04dfa8294e9c6 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -175,11 +175,16 @@ namespace armarx
static GraspTrajectoryPtr ReadFromJSON(const std::string& filename);
static GraspTrajectoryPtr ReadFromString(const std::string& xml);
+ void setFrame(const std::string& frame);
+ const std::optional<std::string>& getFrame() const;
+
private:
void updateKeypointMap();
private:
std::vector<KeypointPtr> keypoints;
std::map<float, size_t> keypointMap;
+
+ std::optional<std::string> frame_;
};
} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index e9b06f66e1e297296a2f47c39cdf6474c0015795..727c1ff1941fa92001c3946e1b3b8b290166a174 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -5,7 +5,6 @@
#include "../error.h"
-
namespace armarx::armem::client
{
@@ -13,7 +12,10 @@ namespace armarx::armem::client
{
}
- data::AddSegmentResult Writer::addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName, bool clearWhenExists)
+ data::AddSegmentResult
+ Writer::addSegment(const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ bool clearWhenExists) const
{
data::AddSegmentInput input;
input.coreSegmentName = coreSegmentName;
@@ -22,24 +24,30 @@ namespace armarx::armem::client
return addSegment(input);
}
- data::AddSegmentResult Writer::addSegment(const MemoryID& providerSegmentID, bool clearWhenExists)
+ data::AddSegmentResult
+ Writer::addSegment(const MemoryID& providerSegmentID, bool clearWhenExists) const
{
- return addSegment(providerSegmentID.coreSegmentName, providerSegmentID.providerSegmentName, clearWhenExists);
+ return addSegment(providerSegmentID.coreSegmentName,
+ providerSegmentID.providerSegmentName,
+ clearWhenExists);
}
- data::AddSegmentResult Writer::addSegment(const std::pair<std::string, std::string>& names, bool clearWhenExists)
+ data::AddSegmentResult
+ Writer::addSegment(const std::pair<std::string, std::string>& names, bool clearWhenExists) const
{
return addSegment(names.first, names.second, clearWhenExists);
}
- data::AddSegmentResult Writer::addSegment(const data::AddSegmentInput& input)
+ data::AddSegmentResult
+ Writer::addSegment(const data::AddSegmentInput& input) const
{
data::AddSegmentsResult results = addSegments({input});
ARMARX_CHECK_EQUAL(results.size(), 1);
return results.at(0);
}
- data::AddSegmentsResult Writer::addSegments(const data::AddSegmentsInput& inputs)
+ data::AddSegmentsResult
+ Writer::addSegments(const data::AddSegmentsInput& inputs) const
{
ARMARX_CHECK_NOT_NULL(memory);
data::AddSegmentsResult results = memory->addSegments(inputs);
@@ -47,8 +55,8 @@ namespace armarx::armem::client
return results;
}
-
- CommitResult Writer::commit(const Commit& commit)
+ CommitResult
+ Writer::commit(const Commit& commit) const
{
ARMARX_CHECK_NOT_NULL(memory);
@@ -63,15 +71,15 @@ namespace armarx::armem::client
return result;
}
-
- data::CommitResult Writer::commit(const data::Commit& _commit)
+ data::CommitResult
+ Writer::commit(const data::Commit& _commit) const
{
data::Commit commit = _commit;
return this->_commit(commit);
}
-
- EntityUpdateResult Writer::commit(const EntityUpdate& update)
+ EntityUpdateResult
+ Writer::commit(const EntityUpdate& update) const
{
armem::Commit commit;
commit.updates.push_back(update);
@@ -81,10 +89,10 @@ namespace armarx::armem::client
return result.results.at(0);
}
- EntityUpdateResult Writer::commit(
- const MemoryID& entityID,
- const std::vector<aron::data::DictPtr>& instancesData,
- Time referencedTime)
+ EntityUpdateResult
+ Writer::commit(const MemoryID& entityID,
+ const std::vector<aron::data::DictPtr>& instancesData,
+ Time referencedTime) const
{
EntityUpdate update;
update.entityID = entityID;
@@ -99,7 +107,8 @@ namespace armarx::armem::client
this->memory = memory;
}
- data::CommitResult Writer::_commit(data::Commit& commit)
+ data::CommitResult
+ Writer::_commit(data::Commit& commit) const
{
ARMARX_CHECK_NOT_NULL(memory);
@@ -119,11 +128,11 @@ namespace armarx::armem::client
}
data::CommitResult result;
- auto handleError = [&commit, &result](const std::string & what)
+ auto handleError = [&commit, &result](const std::string& what)
{
for (const auto& _ : commit.updates)
{
- (void) _;
+ (void)_;
data::EntityUpdateResult& r = result.results.emplace_back();
r.success = false;
r.errorMessage = "Memory component not registered.\n" + std::string(what);
@@ -149,18 +158,20 @@ namespace armarx::armem::client
return result;
}
-}
+} // namespace armarx::armem::client
-
-std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegmentInput& rhs)
+std::ostream&
+armarx::armem::data::operator<<(std::ostream& os, const AddSegmentInput& rhs)
{
return os << "AddSegmentInput: "
- << "\n- core segment: \t'" << rhs.coreSegmentName << "'"
- << "\n- provider segment: \t'" << rhs.providerSegmentName << "'"
- << "\n- clear when exists:\t" << rhs.clearWhenExists << ""
- << "\n";
+ << "\n- core segment: \t'" << rhs.coreSegmentName << "'"
+ << "\n- provider segment: \t'" << rhs.providerSegmentName << "'"
+ << "\n- clear when exists:\t" << rhs.clearWhenExists << ""
+ << "\n";
}
-std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegmentsInput& rhs)
+
+std::ostream&
+armarx::armem::data::operator<<(std::ostream& os, const AddSegmentsInput& rhs)
{
for (size_t i = 0; i < rhs.size(); ++i)
{
@@ -168,15 +179,18 @@ std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegment
}
return os;
}
-std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegmentResult& rhs)
+
+std::ostream&
+armarx::armem::data::operator<<(std::ostream& os, const AddSegmentResult& rhs)
{
return os << "AddSegmentResult:"
- << "\n- success: \t" << rhs.success
- << "\n- segment ID: \t'" << rhs.segmentID << "'"
- << "\n- error message: \t" << rhs.errorMessage
- << "\n";
+ << "\n- success: \t" << rhs.success << "\n- segment ID: \t'" << rhs.segmentID
+ << "'"
+ << "\n- error message: \t" << rhs.errorMessage << "\n";
}
-std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegmentsResult& rhs)
+
+std::ostream&
+armarx::armem::data::operator<<(std::ostream& os, const AddSegmentsResult& rhs)
{
for (size_t i = 0; i < rhs.size(); ++i)
{
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 61eeef93be3164d79d09f6c5aa3371cb362b58b2..65d405a3736e62e39971e08928bd098c0e469636 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -32,28 +32,28 @@ namespace armarx::armem::client
data::AddSegmentResult addSegment(const std::string& coreSegmentName,
const std::string& providerSegmentName,
- bool clearWhenExists = false);
+ bool clearWhenExists = false) const;
data::AddSegmentResult addSegment(const MemoryID& providerSegmentID,
- bool clearWhenExists = false);
+ bool clearWhenExists = false) const;
data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names,
- bool clearWhenExists = false);
- data::AddSegmentResult addSegment(const data::AddSegmentInput& input);
- data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input);
+ bool clearWhenExists = false) const;
+ data::AddSegmentResult addSegment(const data::AddSegmentInput& input) const;
+ data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input) const;
/**
* @brief Writes a `Commit` to the memory.
*/
- CommitResult commit(const Commit& commit);
+ CommitResult commit(const Commit& commit) const;
/// Commit a single entity update.
- EntityUpdateResult commit(const EntityUpdate& update);
+ EntityUpdateResult commit(const EntityUpdate& update) const;
/// Commit a single entity update.
EntityUpdateResult commit(const MemoryID& entityID,
const std::vector<aron::data::DictPtr>& instancesData,
- Time referencedTime);
+ Time referencedTime) const;
// with bare-ice types
- data::CommitResult commit(const data::Commit& commit);
+ data::CommitResult commit(const data::Commit& commit) const;
void setWritingMemory(server::WritingMemoryInterfacePrx memory);
@@ -65,7 +65,7 @@ namespace armarx::armem::client
private:
/// Sets `timeSent` on all entity updates and performs the commit,
- data::CommitResult _commit(data::Commit& commit);
+ data::CommitResult _commit(data::Commit& commit) const;
public:
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
index ab0c5aa6d30a0bc0b377981c22f65d0d3b8c3ade..4bdcaac5bde3e0920c8e59f72cc06abd49875d55 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
@@ -15,8 +15,9 @@ namespace armarx::armem::articulated_object
const std::optional<std::string>& providerName,
const std::string& instanceName = "");
- bool synchronizeArticulatedObject(VirtualRobot::Robot& object,
- const armem::Time& timestamp,
- const std::optional<std::string>& providerName);
+ [[nodiscard]] bool
+ synchronizeArticulatedObject(VirtualRobot::Robot& object,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName);
};
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 16c83498cd59f65418d00f649031b750d4d44969..f21151f1982a28f852bc2b79215709cb0520e4f1 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -26,7 +26,6 @@
#include "utils.h"
-namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
@@ -100,7 +99,7 @@ namespace armarx::armem::articulated_object
std::optional<ArticulatedObject>
Reader::get(const std::string& name,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName)
+ const std::optional<std::string>& providerName) const
{
ARMARX_TRACE;
@@ -125,7 +124,7 @@ namespace armarx::armem::articulated_object
Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp,
const std::string& instanceName,
- const std::optional<std::string>& providerName)
+ const std::optional<std::string>& providerName) const
{
ARMARX_TRACE;
@@ -142,7 +141,7 @@ namespace armarx::armem::articulated_object
bool
Reader::synchronize(ArticulatedObject& obj,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName)
+ const std::optional<std::string>& providerName) const
{
ARMARX_TRACE;
@@ -164,7 +163,7 @@ namespace armarx::armem::articulated_object
std::vector<robot::RobotDescription>
Reader::queryDescriptions(const armem::Time& timestamp,
- const std::optional<std::string>& providerName)
+ const std::optional<std::string>& providerName) const
{
ARMARX_TRACE;
// Query all entities from provider.
@@ -209,7 +208,7 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotDescription>
Reader::queryDescription(const std::string& name,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName)
+ const std::optional<std::string>& providerName) const
{
ARMARX_TRACE;
@@ -256,7 +255,7 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotState>
Reader::queryState(const std::string& instanceName,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName)
+ const std::optional<std::string>& providerName) const
{
ARMARX_TRACE;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index 0eb5bd104f4de49f3b7dfa6cc14ce232a99933fc..50c342c0cfc87614505df230ecdc3fce9cd8b43b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -26,13 +26,17 @@
#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include "ArmarXCore/core/application/properties/forward_declarations.h"
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
#include "interfaces.h"
+// #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+// #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
namespace armarx::armem::articulated_object
{
@@ -40,7 +44,7 @@ namespace armarx::armem::articulated_object
{
public:
Reader() = default;
- virtual ~Reader() = default;
+ ~Reader() override = default;
void
registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -49,30 +53,30 @@ namespace armarx::armem::articulated_object
void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- bool synchronize(ArticulatedObject& obj,
+ [[nodiscard]] bool synchronize(ArticulatedObject& obj,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName) override;
+ const std::optional<std::string>& providerName) const override;
std::optional<ArticulatedObject>
get(const std::string& name,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName) override;
+ const std::optional<std::string>& providerName) const override;
ArticulatedObject get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp,
const std::string& instanceName,
- const std::optional<std::string>& providerName) override;
+ const std::optional<std::string>& providerName) const override;
std::optional<robot::RobotState> queryState(const std::string& instanceName,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName);
+ const std::optional<std::string>& providerName) const;
std::optional<robot::RobotDescription>
queryDescription(const std::string& name,
const armem::Time& timestamp,
- const std::optional<std::string>& providerName);
+ const std::optional<std::string>& providerName) const;
std::vector<robot::RobotDescription>
queryDescriptions(const armem::Time& timestamp,
- const std::optional<std::string>& providerName);
+ const std::optional<std::string>& providerName) const;
std::string getProviderName() const;
void setProviderName(const std::string& providerName);
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index c507fd90594bb6b300080f737d6263a5025417dc..4df7921025ba582a4a913a0fa88906959137b3bb 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -103,7 +103,7 @@ namespace armarx::armem::articulated_object
}
std::optional<armem::MemoryID>
- Writer::storeOrGetClass(const ArticulatedObject& obj)
+ Writer::storeOrGetClass(const ArticulatedObject& obj) const
{
ARMARX_TRACE;
@@ -129,7 +129,7 @@ namespace armarx::armem::articulated_object
}
std::optional<armem::MemoryID>
- Writer::storeClass(const ArticulatedObject& obj)
+ Writer::storeClass(const ArticulatedObject& obj) const
{
std::lock_guard g{memoryWriterMutex};
@@ -190,7 +190,7 @@ namespace armarx::armem::articulated_object
}
bool
- Writer::storeInstance(const ArticulatedObject& obj)
+ Writer::storeInstance(const ArticulatedObject& obj) const
{
std::lock_guard g{memoryWriterMutex};
@@ -259,7 +259,7 @@ namespace armarx::armem::articulated_object
}
bool
- Writer::store(const ArticulatedObject& obj)
+ Writer::store(const ArticulatedObject& obj) const
{
const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 6245ef374edc289afa452900ee97e769ed4eae8b..bf1c7d9459654c422f27c26b8cab9c9e6ae9b20f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -39,16 +39,16 @@ namespace armarx::armem::articulated_object
{
public:
Writer() = default;
- virtual ~Writer() = default;
+ ~Writer() override = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- bool store(const ArticulatedObject& obj) override;
+ bool store(const ArticulatedObject& obj) const override;
- bool storeInstance(const ArticulatedObject& obj);
- std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj);
+ bool storeInstance(const ArticulatedObject& obj) const;
+ std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj) const;
// const std::string& getPropertyPrefix() const override;
@@ -57,7 +57,7 @@ namespace armarx::armem::articulated_object
private:
- std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
+ std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj) const;
void updateKnownObjects(const armem::MemoryID& subscriptionID,
const std::vector<armem::MemoryID>& snapshotIDs);
@@ -85,13 +85,13 @@ namespace armarx::armem::articulated_object
const std::string propertyPrefix = "mem.obj.articulated.";
armem::client::Writer memoryWriter;
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
armem::client::Reader memoryReader;
- std::mutex memoryReaderMutex;
+ mutable std::mutex memoryReaderMutex;
// key: name of object: RobotDescription::name
- std::unordered_map<std::string, MemoryID> knownObjects;
+ mutable std::unordered_map<std::string, MemoryID> knownObjects;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index 6031eba4bfd1288d6567b946a0dd359018482792..899cb1e79ac7ada7991e064b2a1ae1c20310749a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -11,19 +11,26 @@ namespace armarx::armem::articulated_object
public:
virtual ~ReaderInterface() = default;
- virtual bool synchronize(ArticulatedObject& obj, const armem::Time& timestamp, const std::optional<std::string>& providerName) = 0;
-
- virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp, const std::string& instanceName, const std::optional<std::string>& providerName) = 0;
- virtual std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName) = 0;
+ virtual bool synchronize(ArticulatedObject& obj,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName) const = 0;
+
+ virtual ArticulatedObject get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp,
+ const std::string& instanceName,
+ const std::optional<std::string>& providerName) const = 0;
+ virtual std::optional<ArticulatedObject>
+ get(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName) const = 0;
};
-
class WriterInterface
{
public:
virtual ~WriterInterface() = default;
- virtual bool store(const ArticulatedObject& obj) = 0;
+ virtual bool store(const ArticulatedObject& obj) const = 0;
};
-} // namespace armarx::armem::articulated_object
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
index 0c6baa3b770e2136543be7885b1e856a7e93bb78..4c2d920267eb05f799f66e08f26d3ae48b1ee811 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
@@ -13,6 +13,7 @@ namespace armarx::armem::server::robot_state::proprioception
add<Armar6Converter>("Armar6");
add<Armar6Converter>("ArmarDE");
add<Armar6Converter>("Armar7");
+ add<Armar6Converter>("Frankie");
}