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/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");
}