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/components/DSObstacleAvoidance/CMakeLists.txt b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
index 4eb4b16a3d8df576ee0a2a870aaced1f69a78816..37675a4f29325e9d3da5e55be23c6a52d66f5aa4 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
+++ b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
@@ -11,9 +11,6 @@ endif()
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
-if(Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIR})
-endif()
find_package(PythonLibs 3.6 QUIET)
armarx_build_if(PYTHONLIBS_FOUND "Python libs not available")
@@ -26,6 +23,7 @@ set(COMPONENT_LIBS
RobotAPIComponentPlugins
RobotAPIInterfaces
dynamic_obstacle_avoidance
+ Simox::SimoxUtility
)
set(SOURCES DSObstacleAvoidance.cpp)
diff --git a/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
index 7136f05db5538c47faa2ed7d875a69166172e6db..abcd773ce86a236dcd2f15bdf3e3fe1d22718017 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
+++ b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
@@ -3,15 +3,9 @@ armarx_component_set_name("DynamicObstacleManager")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-if(Eigen3_FOUND)
- include_directories(${Eigen3_INCLUDE_DIR})
-endif()
find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
armarx_build_if(Simox_FOUND "Simox ${ArmarX_Simox_VERSION} or later not available")
-if (Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
-endif()
find_package(OpenCV QUIET)
armarx_build_if(OpenCV_FOUND "OpenCV not available")
@@ -25,8 +19,10 @@ set(COMPONENT_LIBS
RobotAPICore
RobotAPIComponentPlugins
RobotAPIInterfaces
+ Eigen3::Eigen
${OpenCV_LIBS}
- ${Simox_LIBS}
+ Simox::VirtualRobot
+ # todo maybe add other simox libs
)
set(SOURCES
@@ -42,4 +38,3 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
armarx_generate_and_add_component_executable()
-
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index c944eda90774140a09c4b2376bcfa734b000817f..fabf44525736293fdbf5f2f84a2bdbef951e2954 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
namespace armarx::armem
{
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
index 23b426f37c9dc8942a621cde3b5746e6cdb52ae3..318ab3d38183f7cf93337082b5b0386c5451f609 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
@@ -38,7 +38,7 @@
#include <RobotAPI/libraries/armem_index/memory_ids.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
index 492c2c14214d5b1a31a8c0998843a5b799ab495b..94fbe1b178a3d538601976da47f28215d8ec30d0 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
@@ -134,7 +134,7 @@ namespace armarx::armem::robot_state
ARMARX_CHECK_EQUAL(localizationPredictionResults.size(), localizationEntityIDs.size());
std::stringstream errorMessages;
- namespace loc = armem::common::robot_state::localization;
+ namespace loc = armem::robot_state::localization;
std::optional<armem::wm::CoreSegment> coreSegment;
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
index a94f44f2160ec2ee4ab67618a8552ffdbdf8ed28..f9f41702e8cde482679fdddf8bd8a91c948b9c54 100644
--- a/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
@@ -8,8 +8,6 @@ set(COMPONENT_LIBS
RobotAPICore RobotAPIInterfaces armem_server
RobotAPIComponentPlugins # for ArViz and other plugins
armem_robot_state
- armem_robot
-
armem_laser_scans
${IVT_LIBRARIES}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index 2db1b8dae17b84c6d0ef98fea1129cebd97f9e75..e4b4d4de1139392d9a8e7cd868c58eacdd5a0314 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -21,7 +21,6 @@
*/
#include "PlatformUnitObserver.h"
-#include <Eigen/src/Geometry/Transform.h>
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include "PlatformUnit.h"
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 879422d20200f53cc0befe9b1c28f6b59220396a..59576d4fb0ec2687336aa41a99e13cca92616ee8 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -16,6 +16,7 @@ set(LIBS
Eigen3::Eigen
${manif_LIBRARIES}
+ armem_robot_state
)
set(LIB_FILES
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/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index aab6379138035bbabb26599742ec74372779fd66..2a3716d2bc3a93aa38764e202f644ae13ab2c3c6 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -27,7 +27,6 @@ add_subdirectory(armem_motions)
add_subdirectory(armem_mps)
add_subdirectory(armem_objects)
add_subdirectory(armem_reasoning)
-add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
add_subdirectory(armem_skills)
add_subdirectory(armem_system_state)
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/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index e150f1e044f225c73aeda5f330a50ffef1c51a16..f127ebd92200346de7421fdd493443971e984045 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -133,7 +133,7 @@ namespace armarx::armem
{
std::vector<std::string> allItems = this->getAllItems(true);
std::string newID = "";
- for(int i = 0; i < allItems.size(); i++){
+ for(std::size_t i = 0; i < allItems.size(); i++){
if(!allItems.at(i).empty()){
std::string toAppend = allItems.at(i);
if(allItems.at(i).find(delimiter) != std::string::npos){
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
index 2c140c94b5950afdb89c887c78198db57ba52457..00568c75b2d8e69eabd2d56d27a0b99e1744bcd6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
@@ -62,7 +62,7 @@ namespace armarx::armem::server::ltm
std::map<std::string, processor::SnapshotFilter::FilterStatistics> stats;
bool recordedOverall = false;
ARMARX_INFO << "Number of active filters: " << snapFilters.size();
- for (int i = 0; i < snapFilters.size(); i++)
+ for (std::size_t i = 0; i < snapFilters.size(); i++)
{
auto statistics = snapFilters.at(i)->getFilterStatistics();
auto recorded = statistics.accepted + statistics.rejected;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
index 67448db2dba914f0d7de8f392433e55f03725026..e5d3a7b1fac491353895ecfa44fbb9df089b4578 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
@@ -38,7 +38,7 @@ namespace armarx::armem::server::ltm::processor::filter
std::map<MemoryID, long> timestampLastCommitInMs;
std::double_t threshold;
FilterStatistics stats;
- int max_images = 2;
+ std::size_t max_images = 2;
aron::similarity::NDArraySimilarity::Type similarity_type;
aron::similarity::FloatSimilarity::Type float_similarity_type;
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index 6eafef5ed875232aa83ab237f058a3c29db08a60..ab4750e34435213bd5db7541012fe8e21a7bb2bb 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -13,9 +13,9 @@
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::grasping::known_grasps
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
index c36d567350b3050ed49e7d362dfcaf2f63f3a643..58e15d056ae74b3d79b48ef917822d20c9de206c 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
@@ -51,18 +51,29 @@ namespace armarx::armem::gui::instance
}
else
{
- return;
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
}
try
{
key = path.getLastElement();
+ if (!elementData)
+ {
+ // push NULL
+ jsonStack.push({key, nullptr});
+ }
+ else
+ {
+ // push new object as there is data
+ jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::object)});
+ }
}
catch (const aron::error::AronException& e)
{
// This happens when we start at the top-level object.
+ jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::object)});
}
- jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::object)});
}
void
@@ -111,10 +122,34 @@ namespace armarx::armem::gui::instance
}
void
- TreeTypedJSONConverter::visitListOnEnter(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitListOnEnter(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::array)});
+ std::string key = "";
+ aron::Path path;
+ if (elementData)
+ {
+ path = elementData->getPath();
+ }
+ else if(elementType)
+ {
+ path = elementType->getPath();
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
+
+ key = path.getLastElement();
+
+ if (!elementData)
+ {
+ jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::array)});
+ }
+ else
+ {
+ jsonStack.push({key, nullptr});
+ }
}
void
@@ -128,115 +163,263 @@ namespace armarx::armem::gui::instance
void
TreeTypedJSONConverter::visitMatrix(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- auto t = *aron::type::Matrix::DynamicCastAndCheck(elementType);
- nlohmann::json obj;
- if (nd.getType() == "float")
+ if (elementData)
{
- Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>> m(
- reinterpret_cast<float*>(nd.getData()), t.getRows(), t.getCols());
- Eigen::to_json(obj, m);
+ auto e = aron::data::NDArray::DynamicCast(elementData);
+ auto t = *aron::type::Matrix::DynamicCastAndCheck(elementType);
+
+ auto& nd = *e;
+ const std::string key = nd.getPath().getLastElement();
+
+ nlohmann::json obj;
+ if (nd.getType() == "float") // TODO: are these types correct?
+ {
+ Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>> m(
+ reinterpret_cast<float*>(nd.getData()), t.getRows(), t.getCols());
+ Eigen::to_json(obj, m);
+ }
+ else if (nd.getType() == "double")
+ {
+ Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> m(
+ reinterpret_cast<double*>(nd.getData()), t.getRows(), t.getCols());
+ Eigen::to_json(obj, m);
+ }
+ else
+ {
+ obj = handleGenericNDArray(nd);
+ }
+
+ insertIntoJSON(key, obj);
}
- else if (nd.getType() == "double")
+ else if(elementType)
{
- Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> m(
- reinterpret_cast<double*>(nd.getData()), t.getRows(), t.getCols());
- Eigen::to_json(obj, m);
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
}
else
{
- obj = handleGenericNDArray(nd);
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
}
- insertIntoJSON(key, obj);
}
void
- TreeTypedJSONConverter::visitNDArray(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitNDArray(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, handleGenericNDArray(nd));
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, handleGenericNDArray(nd));
+ }
+ else if(elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitQuaternion(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitQuaternion(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- nlohmann::json obj;
- Eigen::to_json(obj, aron::data::converter::AronEigenConverter::ConvertToQuaternionf(nd));
- insertIntoJSON(key, obj);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ nlohmann::json obj;
+ Eigen::to_json(obj, aron::data::converter::AronEigenConverter::ConvertToQuaternionf(nd));
+ insertIntoJSON(key, obj);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
+
}
void
- TreeTypedJSONConverter::visitImage(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitImage(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, handleGenericNDArray(nd));
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, handleGenericNDArray(nd));
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitPointCloud(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitPointCloud(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, handleGenericNDArray(nd));
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, handleGenericNDArray(nd));
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
TreeTypedJSONConverter::visitIntEnum(DataInput& elementData, TypeInput& elementType)
{
- /* Not handled by the TreeTypedDataVisitor either */
+ this->visitInt(elementData, elementType);
}
void
- TreeTypedJSONConverter::visitInt(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitInt(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- int i = *aron::data::Int::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, i);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ int i = *aron::data::Int::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, i);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitLong(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitLong(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- int64_t l = *aron::data::Long::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, l);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ int64_t l = *aron::data::Long::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, l);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitFloat(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitFloat(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- float f = *aron::data::Float::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, f);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ float f = *aron::data::Float::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, f);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitDouble(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitDouble(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- double d = *aron::data::Double::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, d);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ double d = *aron::data::Double::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, d);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitBool(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitBool(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- bool b = *aron::data::Bool::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, b);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ bool b = *aron::data::Bool::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, b);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitString(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitString(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- std::string s = *aron::data::String::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, s);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ std::string s = *aron::data::String::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, s);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
//void
@@ -266,6 +449,19 @@ namespace armarx::armem::gui::instance
}
}
+ void
+ TreeTypedJSONConverter::insertNULLIntoJSON(const std::string& key)
+ {
+ if (jsonStack.top().second.is_object())
+ {
+ jsonStack.top().second[key] = nullptr;
+ }
+ else
+ {
+ jsonStack.top().second.emplace_back(nullptr);
+ }
+ }
+
nlohmann::json
TreeTypedJSONConverter::handleGenericNDArray(const aron::data::NDArray& nd)
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
index e299f8f64974bc32a5af1883fc0a10e52ebdedec..a83a6a2352f88c0c2aed408e38677e1be3c35ecf 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
@@ -48,6 +48,7 @@ namespace armarx::armem::gui::instance
template <typename ElementType>
void insertIntoJSON(const std::string& key, const ElementType& data);
+ void insertNULLIntoJSON(const std::string& key);
static nlohmann::json handleGenericNDArray(const aron::data::NDArray& nd);
};
diff --git a/source/RobotAPI/libraries/armem_index/CMakeLists.txt b/source/RobotAPI/libraries/armem_index/CMakeLists.txt
index e586cd150e061a690b1d18b672028e17ac85a3fa..d9e28596e6a9155d9a470a3d1b66cf1fd87389ec 100644
--- a/source/RobotAPI/libraries/armem_index/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_index/CMakeLists.txt
@@ -13,7 +13,6 @@ armarx_add_library(
# RobotAPI
RobotAPI::ArViz
RobotAPI::armem
- # RobotAPI::armem_robot
HEADERS
forward_declarations.h
diff --git a/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
index 86a96909df32e14bbc90a95a9e2a600e86181c9f..e6060daf003fff505e848de0963bb5a430fa35e0 100644
--- a/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
@@ -19,9 +19,6 @@ armarx_add_library(
RobotAPI::armem_server
RobotAPI::armem_index
- # armem_robot_state
- # armem_robot
-
HEADERS
server.h
diff --git a/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
index f40102f3bb1c12ea9d6884e58d4115f25634737b..5f60985331d1be30b290aa61398d1df605f86107 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
@@ -13,7 +13,7 @@ armarx_add_library(
aroncommon
aronvectorconverter
armem_robot_state
- armem_robot
+ ArViz
# System / External
Eigen3::Eigen
HEADERS
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index bbb52cc2b5f473d71cbf1d7c73e39002368f71ad..70eab90df6a473558a51ff94fcdecd5446ba27f1 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -15,7 +15,8 @@ armarx_add_library(
RobotAPI::ComponentPlugins
RobotAPI::Core
RobotAPI::armem
- RobotAPI::armem_robot
+ RobotAPI::armem_robot_state
+# armem_robot_state_aron
HEADERS
aron_conversions.h
@@ -44,6 +45,7 @@ armarx_add_library(
client/articulated_object/ArticulatedObjectWriter.h
client/articulated_object/interfaces.h
client/articulated_object/utils.h
+ client/articulated_object/aron_conversions.h
client/attachment/Reader.h
client/attachment/Writer.h
@@ -63,6 +65,7 @@ armarx_add_library(
client/articulated_object/Writer.cpp
client/articulated_object/ArticulatedObjectReader.cpp
client/articulated_object/ArticulatedObjectWriter.cpp
+ client/articulated_object/aron_conversions.cpp
client/articulated_object/utils.cpp
client/attachment/Reader.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index 093284885bc00e2b50688234c76e94c79795db21..011ee22fafcf6324aa307dbfa5bfb48bf31bb9c5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -17,9 +17,9 @@
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
namespace armarx::armem::articulated_object
{
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/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 5e5b15f15d8135b7de89386c7ba95a7d65f29b9e..d4cf12feebd5e996353ff08fc174720981d96256 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -41,7 +41,7 @@ namespace armarx::armem::articulated_object
obj.getFilename())},
.instance = obj.getName(),
.config = {.timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
.jointMap = obj.getJointValues(),
.proprioception = std::nullopt},
.timestamp = timestamp};
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..24e2a29d828214237ed8b9114fad4bbffd645762 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -20,14 +20,12 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/constants.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include "utils.h"
-namespace fs = ::std::filesystem;
-
namespace armarx::armem::articulated_object
{
@@ -100,7 +98,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 +123,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 +140,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;
@@ -162,9 +160,9 @@ namespace armarx::armem::articulated_object
return true;
}
- std::vector<robot::RobotDescription>
+ std::vector<robot_state::description::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.
@@ -206,10 +204,10 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::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;
@@ -253,10 +251,10 @@ namespace armarx::armem::articulated_object
return getRobotDescription(qResult.memory);
}
- std::optional<robot::RobotState>
+ std::optional<robot_state::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;
@@ -302,7 +300,7 @@ namespace armarx::armem::articulated_object
return getArticulatedObjectState(qResult.memory);
}
- std::optional<robot::RobotState>
+ std::optional<robot_state::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
ARMARX_TRACE;
@@ -321,15 +319,16 @@ namespace armarx::armem::articulated_object
objpose::ObjectPose objectPose;
objpose::fromAron(aronObjectInstance.pose, objectPose);
- robot::RobotState robotState{.timestamp = objectPose.timestamp,
- .globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues,
- .proprioception = std::nullopt};
+ robot_state::RobotState robotState{.timestamp = objectPose.timestamp,
+ .globalPose =
+ Eigen::Isometry3f(objectPose.objectPoseGlobal),
+ .jointMap = objectPose.objectJointValues,
+ .proprioception = std::nullopt};
return robotState;
}
- std::optional<robot::RobotState>
+ std::optional<robot_state::RobotState>
Reader::getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const
{
ARMARX_TRACE;
@@ -351,7 +350,7 @@ namespace armarx::armem::articulated_object
}
}
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
ARMARX_TRACE;
@@ -372,7 +371,7 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- std::vector<robot::RobotDescription>
+ std::vector<robot_state::description::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
ARMARX_TRACE;
@@ -380,7 +379,7 @@ namespace armarx::armem::articulated_object
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(objects::constants::CoreClassSegmentName);
- std::vector<robot::RobotDescription> descriptions;
+ std::vector<robot_state::description::RobotDescription> descriptions;
coreSegment.forEachEntity(
[&descriptions](const wm::Entity& entity)
{
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..93abc3332a5f0ab32a1fe6a95560d40af35b33ee 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,40 +53,42 @@ namespace armarx::armem::articulated_object
void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- bool synchronize(ArticulatedObject& obj,
- const armem::Time& timestamp,
- const std::optional<std::string>& providerName) override;
+ [[nodiscard]] bool
+ synchronize(ArticulatedObject& obj,
+ const armem::Time& timestamp,
+ 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);
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::RobotState>
+ queryState(const std::string& instanceName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName) const;
+ std::optional<robot_state::description::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>
+ std::vector<robot_state::description::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);
protected:
- std::optional<robot::RobotState>
+ std::optional<robot_state::RobotState>
getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const;
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory) const;
- std::vector<robot::RobotDescription>
+ std::vector<robot_state::description::RobotDescription>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
private:
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..d061c692e3399b07279a6cd5420c295396d34e38 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -15,10 +15,11 @@
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include "utils.h"
@@ -103,7 +104,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 +130,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 +191,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 +260,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);
@@ -274,7 +275,7 @@ namespace armarx::armem::articulated_object
}
// TODO this is a duplicate
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
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..dcc49a2b2dc9e9e32a8edd06efa2026e0bb326c6 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);
@@ -67,7 +67,7 @@ namespace armarx::armem::articulated_object
// TODO duplicate
std::unordered_map<std::string, armem::MemoryID>
queryDescriptions(const armem::Time& timestamp);
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory) const;
std::unordered_map<std::string, armem::MemoryID>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
@@ -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/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98b84e3f6760181ee05e60091137bc4b31af8815
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.cpp
@@ -0,0 +1,55 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+namespace armarx::armem::robot_state::description
+{
+
+ void
+ fromAron(const arondto::ObjectClass& dto, RobotDescription& bo)
+ {
+ bo.name = aron::fromAron<armarx::ObjectID>(dto.id).str();
+ fromAron(dto.articulatedSimoxXmlPath, bo.xml);
+ }
+
+ void
+ toAron(arondto::ObjectClass& dto, const RobotDescription& bo)
+ {
+ toAron(dto.id, ObjectID(bo.name));
+ toAron(dto.articulatedSimoxXmlPath, bo.xml);
+ }
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+ void
+ fromAron(const arondto::ObjectInstance& dto, RobotState& bo)
+ {
+ fromAron(dto.pose, bo);
+ }
+
+ void
+ toAron(arondto::ObjectInstance& dto, const RobotState& bo)
+ {
+ toAron(dto.pose, bo);
+ }
+
+ void
+ fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
+ {
+ aron::fromAron(dto.timestamp, bo.timestamp);
+ aron::fromAron(dto.objectPoseGlobal, bo.globalPose);
+ aron::fromAron(dto.objectJointValues, bo.jointMap);
+ }
+
+ void
+ toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
+ {
+ aron::toAron(dto.timestamp, bo.timestamp);
+ aron::toAron(dto.objectPoseGlobal, bo.globalPose.matrix());
+ aron::toAron(dto.objectJointValues, bo.jointMap);
+ }
+
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..d0ce1aaa0c3c5f3d44bc251c239da22aead42384
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::robot_state::description
+{
+ void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
+ void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+
+ void fromAron(const arondto::ObjectInstance& dto, RobotState& bo);
+ void toAron(arondto::ObjectInstance& dto, const RobotState& bo);
+
+ void fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo);
+ void toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo);
+} // namespace armarx::armem::robot_state
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_objects/client/articulated_object/utils.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
index 11c389be63160c69d449244bf6d125f71e7bc225..4afc5a0196a9715d7cb7efd14d3b4c88e81ccf2b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
@@ -1,12 +1,13 @@
#include "utils.h"
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx::armem::articulated_object
{
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
convertRobotDescription(const armem::wm::EntityInstance& instance)
{
@@ -21,7 +22,7 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- robot::RobotDescription robotDescription;
+ robot_state::description::RobotDescription robotDescription;
fromAron(aronObjectInfo, robotDescription);
// check if robot description is valid
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
index 5bb773c96354b108185811137602bdc443c0f9a7..62ec0132d5200075ffea139890471a233d9a43ff 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
@@ -3,14 +3,13 @@
#include <optional>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::articulated_object
{
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
convertRobotDescription(const armem::wm::EntityInstance& instance);
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
index 938ba098681aca145efbeb131af9c7a6a1b6c054..c724a3fd78bff4674428dcd4e4d21ac69ffff9a9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -11,9 +11,9 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
index 9a59562ec531ce44644b4bda5ed2a8e9a0c6ac3e..37ad1d6cfe603b749c7d9956b66ff4dd22972f1a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
@@ -1,29 +1,33 @@
#include "Writer.h"
-#include <IceUtil/Time.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <mutex>
#include <optional>
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
namespace armarx::armem::attachment
{
Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
- {}
+ {
+ }
- void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -37,7 +41,8 @@ namespace armarx::armem::attachment
def->optional(properties.providerName, prefix + "ProviderName");
}
- void Writer::connect()
+ void
+ Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
@@ -54,12 +59,13 @@ namespace armarx::armem::attachment
}
}
-
- std::optional<armem::MemoryID> Writer::commit(const ObjectAttachment& attachment)
+ std::optional<armem::MemoryID>
+ Writer::commit(const ObjectAttachment& attachment)
{
std::lock_guard g{memoryWriterMutex};
- const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+ const auto result =
+ memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
if (not result.success)
{
@@ -72,8 +78,8 @@ namespace armarx::armem::attachment
const auto providerId = armem::MemoryID(result.segmentID);
const auto entityID =
providerId
- .withEntityName(attachment.object.entityName) // TODO check if meaningful
- .withTimestamp(timestamp);
+ .withEntityName(attachment.object.entityName) // TODO check if meaningful
+ .withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -82,7 +88,7 @@ namespace armarx::armem::attachment
toAron(aronAttachment, attachment);
update.instancesData = {aronAttachment.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -96,14 +102,15 @@ namespace armarx::armem::attachment
}
return updateResult.snapshotID;
-
}
- std::optional<armem::MemoryID> Writer::commit(const ArticulatedObjectAttachment& attachment)
+ std::optional<armem::MemoryID>
+ Writer::commit(const ArticulatedObjectAttachment& attachment)
{
std::lock_guard g{memoryWriterMutex};
- const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+ const auto result =
+ memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
if (not result.success)
{
@@ -116,8 +123,8 @@ namespace armarx::armem::attachment
const auto providerId = armem::MemoryID(result.segmentID);
const auto entityID =
providerId
- .withEntityName(attachment.object.id.entityName) // TODO check if meaningful
- .withTimestamp(timestamp);
+ .withEntityName(attachment.object.id.entityName) // TODO check if meaningful
+ .withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -126,7 +133,7 @@ namespace armarx::armem::attachment
toAron(aronAttachment, attachment);
update.instancesData = {aronAttachment.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -143,5 +150,4 @@ namespace armarx::armem::attachment
}
-
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
index 754f4f28041429cd1bb93c9bd3b962335c984530..067d53dc8e9bbd237d9d6d8db34f98105d18f8aa 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
@@ -22,12 +22,13 @@
#pragma once
#include <mutex>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+// #include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
#include <RobotAPI/libraries/armem_objects/types.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 399fce56d0cc22c10de24a8b1a4ab29eb6668581..131fdc655569a16b5d80488289ad2e0fa0d90983 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -14,9 +14,9 @@
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
index 80d1af5590886994c04a69adb025d2767b8f7ffb..243e6a4b4c12d9fcd5b1557b4ce354f7561c2ec0 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
@@ -13,9 +13,6 @@
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index 74e29b855363472523ae328b2698059a8f9d108f..a04d95ca88d86fd722373456cccccddbd82c09a7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
@@ -18,11 +18,10 @@ armarx_add_library(
RobotAPI::ComponentPlugins
RobotAPI::Core
RobotAPI::armem_server
- RobotAPI::armem_robot
+ RobotAPI::armem_robot_state
RobotAPI::armem_objects
armem_robot_state
- armem_robot
HEADERS
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index 59a9a3474a82235d8bf19a7ee3510084c35f96f2..03988d61d5d42ecb4c039bbf93ea028f6a1d298b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -17,7 +17,7 @@
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 865fa2ae35b1ce975b3be920214035674c3c9050..5f0487883fcff6ddb80b6eabf74126d1d491f345 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -3,6 +3,8 @@
#include <filesystem>
#include <sstream>
+#include <sys/inotify.h>
+
#include <Eigen/Dense>
#include <Eigen/Geometry>
@@ -14,6 +16,7 @@
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
@@ -35,8 +38,9 @@
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -138,6 +142,9 @@ namespace armarx::armem::server::obj::instance
prefix + "scene.12_SnapshotToLoad",
simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
+ defs->optional(p.autoReloadSceneSnapshotsOnFileChange,
+ prefix + "scene.13_autoReloadSceneSnapshotsOnFileChange");
+
decay.defineProperties(defs, prefix + "decay.");
}
@@ -165,6 +172,87 @@ namespace armarx::armem::server::obj::instance
}
robots.setTag(Logging::tag);
+
+ if (p.autoReloadSceneSnapshotsOnFileChange)
+ {
+
+ int inotifyFd;
+
+ // init inotify
+ {
+
+ inotifyFd = inotify_init();
+ if (inotifyFd == -1)
+ {
+ ARMARX_WARNING << "inotify_init failed";
+ }
+ }
+
+ std::map<int, std::string> wds;
+
+ // set up inotify watchers
+ for (const auto& scene : scenes)
+ {
+ if (std::optional<std::filesystem::path> path = resolveSceneFilepath(scene))
+ {
+ auto wd = inotify_add_watch(inotifyFd, path.value().c_str(), IN_MODIFY);
+ if (wd == -1)
+ {
+ ARMARX_WARNING << "inotify_add_watch for scene: " << scene << "` failed.";
+ }
+
+ ARMARX_INFO << "inotify_add_watch for scene: " << scene << "` added.";
+ wds.emplace(wd, scene);
+ }
+ else
+ {
+ ARMARX_WARNING << "Faild to add file watcher for scene: `" << scene << "`.";
+ }
+ }
+
+ ARMARX_INFO << "Set up inotify events.";
+
+ fileWatcherTask = new SimpleRunningTask<>(
+ [this, inotifyFd, wds]()
+ {
+ constexpr std::size_t BUF_LEN = (10 * (sizeof(inotify_event) + NAME_MAX + 1));
+ char buf[BUF_LEN];
+
+
+ for (;;)
+ {
+ ssize_t numRead;
+
+ numRead = read(inotifyFd, buf, BUF_LEN);
+ if (numRead == 0)
+ {
+ ARMARX_WARNING << "read() from inotify fd returned 0!";
+ }
+
+ if (numRead == -1)
+ {
+ ARMARX_VERBOSE << "read";
+ }
+
+ ARMARX_DEBUG << VAROUT(numRead);
+
+ for (char* p = buf; p < buf + numRead;)
+ {
+ auto* event = reinterpret_cast<inotify_event*>(p);
+
+ const auto& scene = wds.at(event->wd);
+ ARMARX_INFO << "File changed: " << VAROUT(scene);
+
+ p += sizeof(struct inotify_event) + event->len;
+
+ const bool lockMemory = true;
+ commitSceneSnapshotFromFilename(scene, lockMemory);
+ }
+ }
+ });
+
+ fileWatcherTask->start();
+ }
}
void
@@ -648,7 +736,7 @@ namespace armarx::armem::server::obj::instance
try
{
dto.fromAron(classInstance->data());
- robot::RobotDescription description;
+ robot_state::description::RobotDescription description;
fromAron(dto, description);
articulatedObject.description = description;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index b922f1211cbe769e89d9a55108b257b336c73642..458117502eca9380eb5fc93903b5b6ed04b13adc 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -7,6 +7,7 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -195,6 +196,8 @@ namespace armarx::armem::server::obj::instance
std::string sceneSnapshotsDirectory = "scenes";
std::string sceneSnapshotsToLoad = "";
+ bool autoReloadSceneSnapshotsOnFileChange = false;
+
std::vector<std::string> getSceneSnapshotsToLoad() const;
};
@@ -234,6 +237,8 @@ namespace armarx::armem::server::obj::instance
private:
std::unique_ptr<ArticulatedObjectVisu> visu;
+
+ SimpleRunningTask<>::pointer_type fileWatcherTask;
};
} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 564bf3b460a06e036cd33b481541f4d41119cf25..0a5719891d230e2055d96bba78d947dc159af6ec 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -28,7 +28,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Names.h>
@@ -135,10 +135,10 @@ namespace armarx::armem::attachment
namespace armarx::armem::articulated_object
{
- using ArticulatedObjectDescription = armarx::armem::robot::RobotDescription;
+ using ArticulatedObjectDescription = armarx::armem::robot_state::description::RobotDescription;
- using ArticulatedObject = armarx::armem::robot::Robot;
- using ArticulatedObjects = armarx::armem::robot::Robots;
+ using ArticulatedObject = armarx::armem::robot_state::Robot;
+ using ArticulatedObjects = armarx::armem::robot_state::Robots;
} // namespace armarx::armem::articulated_object
namespace armarx::armem::marker
diff --git a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
deleted file mode 100644
index 43e6af6f4a45f3d638c182bef38e5cdc9cf50d24..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
+++ /dev/null
@@ -1,44 +0,0 @@
-set(LIB_NAME armem_robot)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-
-armarx_add_library(
- LIBS
- # ArmarXCore
- ArmarXCore
- # ArmarXGui
- ArmarXGuiComponentPlugins
- # RobotAPI
- RobotAPI::ArViz
- RobotAPI::ComponentPlugins
- RobotAPI::Core
- RobotAPI::armem
- HEADERS
- types.h
-
- aron_conversions.h
- robot_conversions.h
-
- SOURCES
- types.cpp
-
- aron_conversions.cpp
- robot_conversions.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/RobotDescription.xml
- aron/RobotState.xml
- aron/Robot.xml
-)
-
-add_library(${PROJECT_NAME}::armem_robot ALIAS armem_robot)
-
-# add unit tests
-# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
deleted file mode 100644
index 3bd7b2dfb5f03716174c6a3367fe9257f68d2526..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ /dev/null
@@ -1,106 +0,0 @@
-#include "aron_conversions.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-
-
-namespace armarx::armem::robot
-{
-
- /* Robot */
-
- void fromAron(const arondto::Robot& dto, Robot& bo)
- {
- // fromAron(dto.description, bo.description);
- fromAron(dto.state, bo.config);
- }
-
- void toAron(arondto::Robot& dto, const Robot& bo)
- {
- // toAron(dto.description, bo.description);
- toAron(dto.state, bo.config);
- }
-
- /* RobotDescription */
-
- void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
- {
- aron::fromAron(dto.name, bo.name);
- fromAron(dto.xml, bo.xml);
- }
-
- void toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
- {
- aron::toAron(dto.name, bo.name);
- toAron(dto.xml, bo.xml);
- }
-
- /* RobotState */
-
- void fromAron(const arondto::RobotState& dto, RobotState& bo)
- {
- aron::fromAron(dto.timestamp, bo.timestamp);
- aron::fromAron(dto.globalPose, bo.globalPose.matrix());
- aron::fromAron(dto.jointMap, bo.jointMap);
-
- }
-
- void toAron(arondto::RobotState& dto, const RobotState& bo)
- {
- aron::toAron(dto.timestamp, bo.timestamp);
- aron::toAron(dto.globalPose, bo.globalPose.matrix());
- aron::toAron(dto.jointMap, bo.jointMap);
-
- }
-
-} // namespace armarx::armem::robot
-
-
-namespace armarx::armem
-{
-
- void robot::fromAron(const arondto::ObjectClass& dto, RobotDescription& bo)
- {
- bo.name = aron::fromAron<armarx::ObjectID>(dto.id).str();
- fromAron(dto.articulatedSimoxXmlPath, bo.xml);
- }
-
- void robot::toAron(arondto::ObjectClass& dto, const RobotDescription& bo)
- {
- toAron(dto.id, ObjectID(bo.name));
- toAron(dto.articulatedSimoxXmlPath, bo.xml);
- }
-
-
- void robot::fromAron(const arondto::ObjectInstance& dto, RobotState& bo)
- {
- fromAron(dto.pose, bo);
- }
-
- void robot::toAron(arondto::ObjectInstance& dto, const RobotState& bo)
- {
- toAron(dto.pose, bo);
- }
-
-
- void robot::fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
- {
- aron::fromAron(dto.timestamp, bo.timestamp);
- aron::fromAron(dto.objectPoseGlobal, bo.globalPose);
- aron::fromAron(dto.objectJointValues, bo.jointMap);
-
- }
-
- void robot::toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
- {
- aron::toAron(dto.timestamp, bo.timestamp);
- aron::toAron(dto.objectPoseGlobal, bo.globalPose.matrix());
- aron::toAron(dto.objectJointValues, bo.jointMap);
-
- }
-
-
-} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
deleted file mode 100644
index f2ea2ba959897ad48d9aed390a569d2e8009dd17..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-
-#include <RobotAPI/libraries/armem_robot/types.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-
-
-namespace armarx::armem::robot
-{
- // TODO move the following
- void fromAron(const long& dto, IceUtil::Time& time);
- void toAron(long& dto, const IceUtil::Time& time);
- // end TODO
-
- void fromAron(const arondto::Robot& dto, Robot& bo);
- void toAron(arondto::Robot& dto, const Robot& bo);
-
- void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
- void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
-
- void fromAron(const arondto::RobotState& dto, RobotState& bo);
- void toAron(arondto::RobotState& dto, const RobotState& bo);
-
- void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
- void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
-
- void fromAron(const arondto::ObjectInstance& dto, RobotState& bo);
- void toAron(arondto::ObjectInstance& dto, const RobotState& bo);
-
- void fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo);
- void toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo);
-
-} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
deleted file mode 100644
index 64d68ed6a3549fa7b0e6fa5745a1cc5731158a38..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/client/interfaces.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#pragma once
-
-
-#include "RobotAPI/libraries/armem/core/forward_declarations.h"
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
-
-namespace armarx::armem::robot
-{
- class ReaderInterface
- {
- public:
- virtual ~ReaderInterface() = default;
-
- virtual bool synchronize(Robot& obj, const armem::Time& timestamp) const = 0;
-
- virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) const = 0;
- virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) const = 0;
- };
-
- class WriterInterface
- {
- public:
- virtual ~WriterInterface() = default;
-
- // virtual bool store(const Robot& obj) = 0;
-
- virtual bool storeDescription(const RobotDescription& description,
- const armem::Time& timestamp = armem::Time::Invalid()) = 0;
-
- virtual bool storeState(const robot::RobotState& state,
- const std::string& robotTypeName,
- const std::string& robotName,
- const std::string& robotRootNodeName) = 0;
- };
-
-} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
deleted file mode 100644
index d94903c36c3df2678eddc4317751a65bc0ce506f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ /dev/null
@@ -1,91 +0,0 @@
-#pragma once
-
-#include <filesystem>
-#include <map>
-#include <vector>
-
-#include <Eigen/Geometry>
-
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/time/DateTime.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-
-namespace armarx::armem::robot
-{
- struct RobotDescription
- {
- // DateTime timestamp;
-
- std::string name;
- PackagePath xml{"", std::filesystem::path("")};
- };
-
-
- struct Twist
- {
- Eigen::Vector3f linear{Eigen::Vector3f::Zero()};
- Eigen::Vector3f angular{Eigen::Vector3f::Zero()};
- };
-
- struct PlatformState
- {
- Twist twist;
- };
-
- struct ForceTorque
- {
- Eigen::Vector3f force;
- Eigen::Vector3f gravityCompensatedForce;
- Eigen::Vector3f torque;
- Eigen::Vector3f gravityCompensatedTorque;
- };
-
- struct ToF
- {
- using DepthMatrixType = Eigen::MatrixXf; // FIXME uint16
- using DepthSigmaMatrixType = Eigen::MatrixXf; // FIXME uint8
- using TargetDetectedMatrixType = Eigen::MatrixXf; // FIXME uint8
-
- DepthMatrixType depth;
- DepthSigmaMatrixType sigma;
- TargetDetectedMatrixType targetDetected;
-
- std::string frame;
- };
-
-
- struct RobotState
- {
- using JointMap = std::map<std::string, float>;
- using Pose = Eigen::Affine3f;
-
- DateTime timestamp;
-
- Pose globalPose;
- JointMap jointMap;
-
- std::optional<armarx::armem::arondto::Proprioception> proprioception;
- };
-
-
- struct Robot
- {
- RobotDescription description;
- std::string instance;
-
- RobotState config;
-
- DateTime timestamp;
-
- std::string name() const;
- };
-
- using Robots = std::vector<Robot>;
- using RobotDescriptions = std::vector<RobotDescription>;
- using RobotStates = std::vector<RobotState>;
-
- std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs);
-
-} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index c71d1d839e5511f73cb67b13d8c80c28def503d4..25aa47c769a25c09926eca8d82431137c6f508c7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -18,9 +18,12 @@ armarx_add_library(
RobotAPICore
RobotAPIInterfaces
RobotAPI::armem
- RobotAPI::armem_robot
+ RobotUnitDataStreamingReceiver
+ # RobotAPI::ArViz
+ # RobotAPI::ComponentPlugins
+# RobotAPI::Core
aroncommon
-
+ # armem_objects
# System / External
Eigen3::Eigen
@@ -37,9 +40,12 @@ armarx_add_library(
client/localization/TransformReader.h
client/localization/TransformWriter.h
+ robot_conversions.h
+
aron_conversions.h
memory_ids.h
utils.h
+ types.h
SOURCES
common/localization/TransformHelper.cpp
@@ -52,9 +58,11 @@ armarx_add_library(
client/localization/TransformReader.cpp
client/localization/TransformWriter.cpp
+ robot_conversions.cpp
aron_conversions.cpp
memory_ids.cpp
utils.cpp
+ types.cpp
)
@@ -67,9 +75,58 @@ armarx_enable_aron_file_generation_for_target(
aron/Exteroception.xml
aron/TransformHeader.xml
aron/Transform.xml
+ aron/RobotDescription.xml
+ aron/RobotState.xml
+ aron/Robot.xml
)
add_library(RobotAPI::armem_robot_state ALIAS armem_robot_state)
add_subdirectory(server)
+
+
+
+# armarx_component_set_name("${LIB_NAME}")
+# armarx_set_target("Library: ${LIB_NAME}")
+
+
+# armarx_add_library(
+# LIBS
+# # ArmarXCore
+# ArmarXCore
+# # ArmarXGui
+# ArmarXGuiComponentPlugins
+# # RobotAPI
+# RobotAPI::ArViz
+# RobotAPI::ComponentPlugins
+# RobotAPI::Core
+# RobotAPI::armem
+# # armem_robot_state_aron
+# HEADERS
+# types.h
+
+# aron_conversions.h
+# robot_conversions.h
+
+# SOURCES
+# types.cpp
+
+# aron_conversions.cpp
+# robot_conversions.cpp
+# )
+
+
+# armarx_enable_aron_file_generation_for_target(
+# TARGET_NAME
+# "${LIB_NAME}"
+# ARON_FILES
+# aron/RobotDescription.xml
+# aron/RobotState.xml
+# aron/Robot.xml
+# )
+
+# add_library(${PROJECT_NAME}::armem_robot ALIAS armem_robot)
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Robot.xml
similarity index 92%
rename from source/RobotAPI/libraries/armem_robot/aron/Robot.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/Robot.xml
index b7192c366e7ae685c176578b03a44d9f533f1e75..2d1a0918edf7fe628bc1654b79af3528288f9401 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Robot.xml
@@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
- <PackagePath package="RobotAPI" path="libraries/armem_robot/aron/RobotDescription.xml" />
- <PackagePath package="RobotAPI" path="libraries/armem_robot/aron/RobotState.xml" />
+ <PackagePath package="RobotAPI" path="libraries/armem_robot_state/aron/RobotDescription.xml" />
+ <PackagePath package="RobotAPI" path="libraries/armem_robot_state/aron/RobotState.xml" />
<PackagePath package="RobotAPI" path="libraries/armem/aron/MemoryID.xml" />
</AronIncludes>
@@ -27,4 +27,3 @@
</GenerateTypes>
</AronTypeDefinition>
-
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot_state/aron/RobotState.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/RobotState.xml
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 714ee296537470b7119d0ed62bb9039bb6a38553..e00b27c3d77fb6130171e06244cffc1181c8fbc1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -1,30 +1,27 @@
#include "aron_conversions.h"
-#include <string>
-
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-namespace armarx::armem
+namespace armarx::armem::robot_state::localization
{
/* Transform */
void
- fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ fromAron(const arondto::Transform& dto, Transform& bo)
{
fromAron(dto.header, bo.header);
aron::fromAron(dto.transform, bo.transform);
}
void
- toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ toAron(arondto::Transform& dto, const Transform& bo)
{
toAron(dto.header, bo.header);
aron::toAron(dto.transform, bo.transform);
@@ -33,7 +30,7 @@ namespace armarx::armem
/* TransformHeader */
void
- toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ toAron(arondto::TransformHeader& dto, const TransformHeader& bo)
{
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
@@ -42,16 +39,19 @@ namespace armarx::armem
}
void
- fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ fromAron(const arondto::TransformHeader& dto, TransformHeader& bo)
{
aron::fromAron(dto.parentFrame, bo.parentFrame);
aron::fromAron(dto.frame, bo.frame);
aron::fromAron(dto.agent, bo.agent);
aron::fromAron(dto.timestamp, bo.timestamp);
}
+} // namespace armarx::armem::robot_state::localization
+namespace armarx::armem::robot_state
+{
void
- fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ fromAron(const armarx::armem::prop::arondto::Platform& dto, robot_state::PlatformState& bo)
{
bo.twist.linear.setZero();
bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
@@ -61,13 +61,16 @@ namespace armarx::armem
}
void
- toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ toAron(armarx::armem::prop::arondto::Platform& dto, const robot_state::PlatformState& bo)
{
ARMARX_ERROR << "Not implemented yet.";
}
+} // namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::proprioception
+{
void
- fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+ fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo)
{
aron::fromAron(dto.force, bo.force);
aron::fromAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
@@ -76,26 +79,89 @@ namespace armarx::armem
}
void
- toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+ toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo)
{
aron::toAron(dto.force, bo.force);
aron::toAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
aron::toAron(dto.torque, bo.torque);
aron::toAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
}
+} // namespace armarx::armem::robot_state::proprioception
-
- void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToF& bo){
+namespace armarx::armem::robot_state::exteroception
+{
+ void
+ fromAron(const armarx::armem::exteroception::arondto::ToF& dto, ToF& bo)
+ {
bo.depth = dto.depth;
bo.sigma = dto.sigma;
bo.targetDetected = dto.targetDetected;
}
- void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToF& bo){
+ void
+ toAron(armarx::armem::exteroception::arondto::ToF& dto, const ToF& bo)
+ {
dto.depth = bo.depth;
dto.sigma = bo.sigma;
dto.targetDetected = bo.targetDetected;
}
+} // namespace armarx::armem::robot_state::exteroception
+
+namespace armarx::armem::robot_state::description
+{
+ void
+ fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
+ {
+ aron::fromAron(dto.name, bo.name);
+ fromAron(dto.xml, bo.xml);
+ }
+
+ void
+ toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
+ {
+ aron::toAron(dto.name, bo.name);
+ toAron(dto.xml, bo.xml);
+ }
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+
+ /* Robot */
+
+ void
+ fromAron(const arondto::Robot& dto, Robot& bo)
+ {
+ // fromAron(dto.description, bo.description);
+ fromAron(dto.state, bo.config);
+ }
+
+ void
+ toAron(arondto::Robot& dto, const Robot& bo)
+ {
+ // toAron(dto.description, bo.description);
+ toAron(dto.state, bo.config);
+ }
+
+ /* RobotDescription */
+
+ /* RobotState */
+
+ void
+ fromAron(const arondto::RobotState& dto, RobotState& bo)
+ {
+ aron::fromAron(dto.timestamp, bo.timestamp);
+ aron::fromAron(dto.globalPose, bo.globalPose.matrix());
+ aron::fromAron(dto.jointMap, bo.jointMap);
+ }
+
+ void
+ toAron(arondto::RobotState& dto, const RobotState& bo)
+ {
+ aron::toAron(dto.timestamp, bo.timestamp);
+ aron::toAron(dto.globalPose, bo.globalPose.matrix());
+ aron::toAron(dto.jointMap, bo.jointMap);
+ }
-} // namespace armarx::armem
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 3df14cb39abfd1231b8222fff7e3d7b3a15905a3..d0a5716c732beacf0bde1dc033778fcee54ceb7f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -21,15 +21,28 @@
#pragma once
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+
+// #include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+// #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+// #include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
namespace armarx::armem
{
- namespace robot_state
+ namespace robot_state::localization
{
struct Transform;
struct TransformHeader;
+ } // namespace robot_state::localization
+
+ namespace robot_state
+ {
struct JointState;
} // namespace robot_state
@@ -38,7 +51,7 @@ namespace armarx::armem
{
struct Platform;
struct ForceTorque;
- }
+ } // namespace prop::arondto
namespace exteroception::arondto
{
@@ -54,21 +67,60 @@ namespace armarx::armem
} // namespace arondto
+} // namespace armarx::armem
+
+namespace armarx::armem::robot_state::localization
+{
+
+ void fromAron(const arondto::Transform& dto, Transform& bo);
+ void toAron(arondto::Transform& dto, const Transform& bo);
+
+ void fromAron(const arondto::TransformHeader& dto, TransformHeader& bo);
+ void toAron(arondto::TransformHeader& dto, const TransformHeader& bo);
+} // namespace armarx::armem::robot_state::localization
- void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
- void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
+namespace armarx::armem::robot_state
+{
- void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
- void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
- void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
- void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+} // namespace armarx::armem::robot_state
- void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
- void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+namespace armarx::armem::robot_state::proprioception
+{
+
+ void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo);
+ void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo);
+
+} // namespace armarx::armem::robot_state::proprioception
+
+namespace armarx::armem::robot_state::exteroception
+{
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, ToF& bo);
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const ToF& bo);
+
+
+} // namespace armarx::armem::robot_state::exteroception
+
+namespace armarx::armem::robot_state::description
+{
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
+
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+ // TODO move the following
+ void fromAron(const long& dto, IceUtil::Time& time);
+ void toAron(long& dto, const IceUtil::Time& time);
+ // end TODO
- void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToF& bo);
- void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToF& bo);
+ void fromAron(const arondto::Robot& dto, Robot& bo);
+ void toAron(arondto::Robot& dto, const Robot& bo);
+ void fromAron(const arondto::RobotState& dto, RobotState& bo);
+ void toAron(arondto::RobotState& dto, const RobotState& bo);
-} // namespace armarx::armem
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h b/source/RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ea5bfed9d9db84c832ac14d135077545942286e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h
@@ -0,0 +1,24 @@
+#pragma once
+
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::robot_state
+{
+ class ReaderInterface
+ {
+ public:
+ virtual ~ReaderInterface() = default;
+
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) const = 0;
+
+ virtual Robot get(const description::RobotDescription& description,
+ const armem::Time& timestamp) const = 0;
+ virtual std::optional<Robot> get(const std::string& name,
+ const armem::Time& timestamp) const = 0;
+ };
+
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 8a3b67e984d51a40d85e44113bc54a872233d910..1b4b0dd2ea571562291ddffcdf70b1c438b15fef 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -18,13 +18,12 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace fs = ::std::filesystem;
@@ -64,7 +63,7 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::Robot>
+ std::optional<Robot>
RobotReader::get(const std::string& name, const armem::Time& timestamp) const
{
const auto description = queryDescription(name, timestamp);
@@ -78,10 +77,11 @@ namespace armarx::armem::robot_state
return get(*description, timestamp);
}
- robot::Robot
- RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ Robot
+ RobotReader::get(const description::RobotDescription& description,
+ const armem::Time& timestamp) const
{
- robot::Robot robot{.description = description,
+ Robot robot{.description = description,
.instance = "", // TODO(fabian.reister):
.config = {}, // will be populated by synchronize
.timestamp = timestamp};
@@ -105,23 +105,23 @@ namespace armarx::armem::robot_state
}
bool
- RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp) const
+ RobotReader::synchronize(Robot& robot, const armem::Time& timestamp) const
{
const auto tsStartFunctionInvokation = armem::Time::Now();
while (true)
{
- auto state = queryState(obj.description, timestamp);
+ auto state = queryState(robot.description.name, timestamp);
if (not state) /* c++20 [[unlikely]] */
{
- ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
// if the syncTime is non-positive there will be no retry
const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
if (elapsedTime > syncTimeout)
{
- ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
return false;
}
@@ -129,12 +129,12 @@ namespace armarx::armem::robot_state
Clock::WaitFor(sleepAfterFailure);
}
- obj.config = std::move(*state);
+ robot.config = std::move(*state);
return true;
}
}
- std::optional<robot::RobotDescription>
+ std::optional<description::RobotDescription>
RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp) const
{
@@ -182,18 +182,18 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- std::optional<robot::RobotState>
- RobotReader::queryState(const robot::RobotDescription& description,
+ std::optional<RobotState>
+ RobotReader::queryState(const std::string& robotName,
const armem::Time& timestamp) const
{
- std::optional<robot::RobotState> robotState = queryJointState(description, timestamp);
+ std::optional<RobotState> robotState = queryJointState(robotName, timestamp);
if (robotState)
{
- const auto globalPose = queryGlobalPose(description, timestamp);
+ const auto globalPose = queryGlobalPose(robotName, timestamp);
if (not globalPose)
{
- ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
+ ARMARX_VERBOSE << "Failed to query global pose for robot " << robotName;
return std::nullopt;
}
robotState->globalPose = *globalPose;
@@ -202,35 +202,35 @@ namespace armarx::armem::robot_state
return robotState;
}
- std::optional<robot::RobotState>
- RobotReader::queryJointState(const robot::RobotDescription& description,
+ std::optional<RobotState>
+ RobotReader::queryJointState(const std::string& robotName,
const armem::Time& timestamp) const
{
- const auto proprioception = queryProprioception(description, timestamp);
+ const auto proprioception = queryProprioception(robotName, timestamp);
if (not proprioception.has_value())
{
- ARMARX_VERBOSE << "Failed to query proprioception for robot '" << description.name
+ ARMARX_VERBOSE << "Failed to query proprioception for robot '" << robotName
<< "'.";
return std::nullopt;
}
const auto jointMap = proprioception->joints.position;
- return robot::RobotState{.timestamp = timestamp,
- .globalPose = robot::RobotState::Pose::Identity(),
+ return RobotState{.timestamp = timestamp,
+ .globalPose = RobotState::Pose::Identity(),
.jointMap = jointMap,
.proprioception = proprioception};
}
- std::optional<::armarx::armem::robot_state::Transform>
- RobotReader::queryOdometryPose(const robot::RobotDescription& description,
+ std::optional<::armarx::armem::robot_state::localization::Transform>
+ RobotReader::queryOdometryPose(const std::string& robotName,
const armem::Time& timestamp) const
{
- common::robot_state::localization::TransformQuery query{
+ robot_state::localization::TransformQuery query{
.header = {.parentFrame = OdometryFrame,
.frame = constants::robotRootNodeName,
- .agent = description.name,
+ .agent = robotName,
.timestamp = timestamp}};
// try
@@ -250,7 +250,7 @@ namespace armarx::armem::robot_state
}
std::optional<armarx::armem::arondto::Proprioception>
- RobotReader::queryProprioception(const robot::RobotDescription& description,
+ RobotReader::queryProprioception(const std::string& robotName,
const armem::Time& timestamp) const // Why timestamp?!?!
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -258,12 +258,12 @@ namespace armarx::armem::robot_state
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying robot description for robot: " << description;
+ ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -281,7 +281,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getRobotProprioception(qResult.memory, description.name);
+ return getRobotProprioception(qResult.memory, robotName);
}
catch (...)
{
@@ -292,19 +292,19 @@ namespace armarx::armem::robot_state
}
RobotReader::JointTrajectory
- RobotReader::queryJointStates(const robot::RobotDescription& description,
+ RobotReader::queryJointStates(const std::string& robotName,
const armem::Time& begin,
const armem::Time& end) const
{
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+ ARMARX_DEBUG << "Querying robot joint states for robot: `" << robotName
<< "` on time interval [" << begin << "," << end << "]";
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().timeRange(begin, end);
// clang-format on
@@ -322,7 +322,7 @@ namespace armarx::armem::robot_state
return {};
}
- return getRobotJointStates(qResult.memory, description.name);
+ return getRobotJointStates(qResult.memory, robotName);
}
catch (...)
{
@@ -332,8 +332,8 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::PlatformState>
- RobotReader::queryPlatformState(const robot::RobotDescription& description,
+ std::optional<PlatformState>
+ RobotReader::queryPlatformState(const std::string& robotName,
const armem::Time& timestamp) const
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -341,12 +341,12 @@ namespace armarx::armem::robot_state
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying robot description for robot: " << description;
+ ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -364,7 +364,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getRobotPlatformState(qResult.memory, description.name);
+ return getRobotPlatformState(qResult.memory, robotName);
}
catch (...)
{
@@ -374,14 +374,14 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotState::Pose>
- RobotReader::queryGlobalPose(const robot::RobotDescription& description,
+ std::optional<RobotState::Pose>
+ RobotReader::queryGlobalPose(const std::string& robotName,
const armem::Time& timestamp) const
{
try
{
const auto result = transformReader.getGlobalPose(
- description.name, constants::robotRootNodeName, timestamp);
+ robotName, constants::robotRootNodeName, timestamp);
if (not result)
{
return std::nullopt;
@@ -397,7 +397,7 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotState>
+ std::optional<RobotState>
RobotReader::getRobotState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
@@ -431,7 +431,7 @@ namespace armarx::armem::robot_state
}
// Here, we access the RobotUnit streaming data stored in the proprioception segment.
- return robot::convertRobotState(*instance);
+ return convertRobotState(*instance);
}
// FIXME remove this, use armem/util/util.h
@@ -520,20 +520,20 @@ namespace armarx::armem::robot_state
}
// force torque for left and right
- std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
- RobotReader::queryForceTorque(const robot::RobotDescription& description,
+ std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
+ RobotReader::queryForceTorque(const std::string& robotName,
const armem::Time& timestamp) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+ ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -551,7 +551,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getForceTorque(qResult.memory, description.name);
+ return getForceTorque(qResult.memory, robotName);
}
catch (...)
{
@@ -562,8 +562,8 @@ namespace armarx::armem::robot_state
}
// force torque for left and right
- std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
- RobotReader::queryForceTorques(const robot::RobotDescription& description,
+ std::optional<std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
+ RobotReader::queryForceTorques(const std::string& robotName,
const armem::Time& start,
const armem::Time& end) const
{
@@ -571,12 +571,12 @@ namespace armarx::armem::robot_state
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+ ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().timeRange(start, end);
// clang-format on
@@ -594,7 +594,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getForceTorques(qResult.memory, description.name);
+ return getForceTorques(qResult.memory, robotName);
}
catch (...)
{
@@ -604,19 +604,19 @@ namespace armarx::armem::robot_state
}
}
- std::optional<std::map<RobotReader::Hand, robot::ToF>>
- RobotReader::queryToF(const robot::RobotDescription& description,
+ std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
+ RobotReader::queryToF(const std::string& robotName,
const armem::Time& timestamp) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+ ARMARX_DEBUG << "Querying ToF data for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::exteroceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -634,7 +634,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getToF(qResult.memory, description.name);
+ return getToF(qResult.memory, robotName);
}
catch (...)
{
@@ -644,11 +644,11 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::PlatformState>
+ std::optional<PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
- std::optional<robot::PlatformState> platformState;
+ std::optional<PlatformState> platformState;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -669,7 +669,7 @@ namespace armarx::armem::robot_state
tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
ARMARX_CHECK(proprioception.has_value());
- platformState = robot::PlatformState(); // initialize optional
+ platformState = PlatformState(); // initialize optional
fromAron(proprioception->platform, platformState.value());
});
@@ -692,11 +692,11 @@ namespace armarx::armem::robot_state
throw LocalException("Unknown hand name `" + name + "`!");
}
- std::map<RobotReader::Hand, robot::ForceTorque>
+ std::map<RobotReader::Hand, proprioception::ForceTorque>
RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
- std::map<RobotReader::Hand, robot::ForceTorque> forceTorques;
+ std::map<RobotReader::Hand, proprioception::ForceTorque> forceTorques;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -719,7 +719,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
- robot::ForceTorque forceTorque;
+ proprioception::ForceTorque forceTorque;
fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
@@ -730,11 +730,11 @@ namespace armarx::armem::robot_state
return forceTorques;
}
- std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
- std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> forceTorques;
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>> forceTorques;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -751,7 +751,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
- robot::ForceTorque forceTorque;
+ proprioception::ForceTorque forceTorque;
fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
@@ -762,10 +762,10 @@ namespace armarx::armem::robot_state
return forceTorques;
}
- std::map<RobotReader::Hand, robot::ToF>
+ std::map<RobotReader::Hand, exteroception::ToF>
RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
{
- std::map<RobotReader::Hand, robot::ToF> tofs;
+ std::map<RobotReader::Hand, exteroception::ToF> tofs;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -792,7 +792,7 @@ namespace armarx::armem::robot_state
{
ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
- robot::ToF tof;
+ exteroception::ToF tof;
fromAron(dtoFt, tof);
const auto hand = fromHandName(handName);
@@ -803,9 +803,9 @@ namespace armarx::armem::robot_state
return tofs;
}
- std::optional<robot::RobotDescription>
+ std::optional<description::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ const std::string& name) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -822,22 +822,22 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::convertRobotDescription(*instance);
+ return convertRobotDescription(*instance);
}
- std::vector<robot::RobotDescription>
+ std::vector<description::RobotDescription>
RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(constants::descriptionCoreSegment);
- std::vector<robot::RobotDescription> descriptions;
+ std::vector<description::RobotDescription> descriptions;
coreSegment.forEachInstance(
[&descriptions](const wm::EntityInstance& instance)
{
- if (const std::optional<robot::RobotDescription> desc =
- robot::convertRobotDescription(instance))
+ if (const std::optional<description::RobotDescription> desc =
+ convertRobotDescription(instance))
{
descriptions.push_back(desc.value());
}
@@ -846,7 +846,7 @@ namespace armarx::armem::robot_state
return descriptions;
}
- std::vector<robot::RobotDescription>
+ std::vector<description::RobotDescription>
RobotReader::queryDescriptions(const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -892,4 +892,4 @@ namespace armarx::armem::robot_state
return {};
}
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 650227dee1666bb124d4941a13c16088f62d88a5..5b4700a003fddaafc548811e1a983e4a4e95e03a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -28,9 +28,9 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::robot_state
{
@@ -40,7 +40,7 @@ namespace armarx::armem::robot_state
* The purpose of this class is to synchronize the armem data structure armem::Robot
* with the memory.
*/
- class RobotReader : virtual public robot::ReaderInterface
+ class RobotReader : virtual public ReaderInterface
{
public:
RobotReader() = default;
@@ -51,42 +51,39 @@ namespace armarx::armem::robot_state
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- [[nodiscard]] bool synchronize(robot::Robot& obj,
- const armem::Time& timestamp) const override;
+ [[nodiscard]] bool synchronize(Robot& obj, const armem::Time& timestamp) const override;
- std::optional<robot::Robot> get(const std::string& name,
- const armem::Time& timestamp) const override;
- robot::Robot get(const robot::RobotDescription& description,
- const armem::Time& timestamp) const override;
+ std::optional<Robot> get(const std::string& name,
+ const armem::Time& timestamp) const override;
+ Robot get(const description::RobotDescription& description,
+ const armem::Time& timestamp) const override;
- std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp) const;
+ std::optional<description::RobotDescription>
+ queryDescription(const std::string& name, const armem::Time& timestamp) const;
- std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp) const;
+ std::vector<description::RobotDescription>
+ queryDescriptions(const armem::Time& timestamp) const;
- std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<RobotState> queryState(const std::string& robotName,
+ const armem::Time& timestamp) const;
- std::optional<robot::RobotState> queryJointState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<RobotState> queryJointState(const std::string& robotName,
+ const armem::Time& timestamp) const;
std::optional<armarx::armem::arondto::Proprioception>
- queryProprioception(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ queryProprioception(const std::string& robotName, const armem::Time& timestamp) const;
- using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+ using JointTrajectory = std::map<armem::Time, RobotState::JointMap>;
- JointTrajectory queryJointStates(const robot::RobotDescription& description,
+ JointTrajectory queryJointStates(const std::string& robotName,
const armem::Time& begin,
const armem::Time& end) const;
- std::optional<robot::RobotState::Pose>
- queryGlobalPose(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<RobotState::Pose> queryGlobalPose(const std::string& robotName,
+ const armem::Time& timestamp) const;
- std::optional<robot::PlatformState>
- queryPlatformState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<PlatformState> queryPlatformState(const std::string& robotName,
+ const armem::Time& timestamp) const;
void setSyncTimeout(const armem::Duration& duration);
void setSleepAfterSyncFailure(const armem::Duration& duration);
@@ -97,18 +94,18 @@ namespace armarx::armem::robot_state
Right
};
- std::optional<std::map<Hand, robot::ForceTorque>>
- queryForceTorque(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<std::map<Hand, proprioception::ForceTorque>>
+ queryForceTorque(const std::string& robotName, const armem::Time& timestamp) const;
- std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
- queryForceTorques(const robot::RobotDescription& description,
+ std::optional<
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
+ queryForceTorques(const std::string& robotName,
const armem::Time& start,
const armem::Time& end) const;
- std::optional<std::map<Hand, robot::ToF>>
- queryToF(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+ std::optional<std::map<Hand, exteroception::ToF>>
+ queryToF(const std::string& robotName, const armem::Time& timestamp) const;
/**
* @brief retrieve the robot's pose in the odometry frame.
@@ -117,11 +114,10 @@ namespace armarx::armem::robot_state
*
* @param description
* @param timestamp
- * @return std::optional<robot::RobotState::Pose>
+ * @return std::optional<RobotState::Pose>
*/
- std::optional<::armarx::armem::robot_state::Transform>
- queryOdometryPose(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<::armarx::armem::robot_state::localization::Transform>
+ queryOdometryPose(const std::string& robotName, const armem::Time& timestamp) const;
protected:
// by default, no timeout mechanism
@@ -129,13 +125,13 @@ namespace armarx::armem::robot_state
armem::Duration sleepAfterFailure = armem::Duration::MicroSeconds(0);
private:
- std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ std::optional<RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
- std::optional<robot::RobotDescription>
+ std::optional<description::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::vector<robot::RobotDescription>
+ std::vector<description::RobotDescription>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
std::optional<armarx::armem::arondto::Proprioception>
@@ -145,19 +141,18 @@ namespace armarx::armem::robot_state
JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
- std::optional<robot::PlatformState>
- getRobotPlatformState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ std::optional<PlatformState> getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
- std::map<RobotReader::Hand, robot::ForceTorque>
+ std::map<RobotReader::Hand, proprioception::ForceTorque>
getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::map<RobotReader::Hand, robot::ToF> getToF(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ std::map<RobotReader::Hand, exteroception::ToF>
+ getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const;
struct Properties
{
@@ -168,7 +163,7 @@ namespace armarx::armem::robot_state
armem::client::Reader memoryReader;
mutable std::mutex memoryReaderMutex;
- client::robot_state::localization::TransformReader transformReader;
+ client::localization::TransformReader transformReader;
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
index e92e13d6421c71313bdd04a488e1f43397d1f222..89caaa7826d69c454761d515092e0d72c58c8ca4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -19,15 +19,14 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/TransformHeader.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace fs = ::std::filesystem;
@@ -65,7 +64,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotWriter::storeDescription(const robot::RobotDescription& description,
+ RobotWriter::storeDescription(const description::RobotDescription& description,
const armem::Time& timestamp)
{
const auto validTimestamp = timestamp.isInvalid() ? armarx::Clock::Now() : timestamp;
@@ -177,7 +176,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotWriter::storeState(const robot::RobotState& state,
+ RobotWriter::storeState(const RobotState& state,
const std::string& robotTypeName,
const std::string& robotName,
const std::string& robotRootNodeName)
@@ -191,4 +190,4 @@ namespace armarx::armem::robot_state
}
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
index d8ca351d6cf8643960f1707a49ef26b9697d4082..1095363fd7497bc4ba676e493fd87452f9e1786f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
@@ -28,9 +28,9 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::robot_state
{
@@ -40,7 +40,7 @@ namespace armarx::armem::robot_state
* The purpose of this class is to synchronize the armem data structure armem::Robot
* with the memory.
*/
- class RobotWriter : virtual public robot::WriterInterface
+ class RobotWriter : virtual public WriterInterface
{
public:
RobotWriter() = default;
@@ -52,11 +52,11 @@ namespace armarx::armem::robot_state
[[nodiscard]] bool
- storeDescription(const robot::RobotDescription& description,
+ storeDescription(const description::RobotDescription& description,
const armem::Time& timestamp = armem::Time::Invalid()) override;
- [[nodiscard]] bool storeState(const robot::RobotState& state,
+ [[nodiscard]] bool storeState(const RobotState& state,
const std::string& robotTypeName,
const std::string& robotName,
const std::string& robotRootNodeName) override;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index e23178dfa4a884654b8ba924a71006dbd88e8e8b..529aeba908795871d1690ef937dbecf0736b1a8f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -15,24 +15,12 @@
namespace armarx::armem::robot_state
{
- // TODO(fabian.reister): register property defs
- void
- VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
- {
- RobotReader::registerPropertyDefinitions(def);
- }
bool
VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
const armem::Time& timestamp) const
{
- // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
-
- const robot::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{"", ""}};
-
- const auto robotState = queryState(robotDescription, timestamp);
+ const auto robotState = queryState(robot.getName(), timestamp);
if (not robotState)
{
ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
@@ -51,13 +39,7 @@ namespace armarx::armem::robot_state
VirtualRobotReader::synchronizeRobotJoints(VirtualRobot::Robot& robot,
const armem::Time& timestamp) const
{
- // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
-
- const robot::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{"", ""}};
-
- const auto robotState = queryJointState(robotDescription, timestamp);
+ const auto robotState = queryJointState(robot.getName(), timestamp);
if (not robotState)
{
ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
@@ -145,16 +127,5 @@ namespace armarx::armem::robot_state
return nullptr;
}
- std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
- VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
- {
- const auto description = queryDescription(name, timestamp);
- if (not description.has_value())
- {
- return std::nullopt;
- }
- return RobotReader::queryForceTorque(description.value(), timestamp);
- }
-
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index d0a672c21ba5ae953d3bc3b8bed2333a9f6efa97..0c0eb05aa6cd8f96bec6acdeb90c0261aca43c80 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -31,7 +31,7 @@ namespace armarx::armem::robot_state
{
/**
* @brief The VirtualRobotReader class.
- *
+ *robot
* The aim of this class is to obtain a virtual robot instance and synchronize it
* with the data (joint positions, global pose, ...) stored in the working memory.
*
@@ -44,8 +44,6 @@ namespace armarx::armem::robot_state
~VirtualRobotReader() override = default;
- void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
-
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
const armem::Time& timestamp) const;
@@ -71,10 +69,6 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
- std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
- queryForceTorque(const std::string& name, const armem::Time& timestamp);
- using RobotReader::queryForceTorque;
-
private:
[[nodiscard]] VirtualRobot::RobotPtr
_getSynchronizedRobot(const std::string& name,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index e7525729d3f6d59d4bb8c3feaf237f5ff34d18de..48dc75da8b590436b65c5799591a5f25ad18be69 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -14,7 +14,7 @@
#include <ArmarXCore/core/time/Clock.h>
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/armem_robot_state/types.h"
#include "constants.h"
@@ -50,7 +50,7 @@ namespace armarx::armem::robot_state
{
const auto filename = robot.getFilename();
- const robot::RobotDescription desc{.name = robot.getType(),
+ const description::RobotDescription desc{.name = robot.getType(),
.xml = resolvePackagePath(filename)};
return RobotWriter::storeDescription(desc, timestamp);
@@ -59,9 +59,9 @@ namespace armarx::armem::robot_state
bool
VirtualRobotWriter::storeState(const VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
- const robot::RobotState robotState{.timestamp = timestamp,
+ const RobotState robotState{.timestamp = timestamp,
.globalPose =
- robot::RobotState::Pose(robot.getGlobalPose()),
+ RobotState::Pose(robot.getGlobalPose()),
.jointMap = robot.getJointValues(),
.proprioception = std::nullopt};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h b/source/RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..b4543e2cfd8bf3d6488c1685158d0c907f4bfb13
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h
@@ -0,0 +1,27 @@
+#pragma once
+
+
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::robot_state
+{
+
+ class WriterInterface
+ {
+ public:
+ virtual ~WriterInterface() = default;
+
+ // virtual bool store(const Robot& obj) = 0;
+
+ virtual bool storeDescription(const description::RobotDescription& description,
+ const armem::Time& timestamp = armem::Time::Invalid()) = 0;
+
+ virtual bool storeState(const RobotState& state,
+ const std::string& robotTypeName,
+ const std::string& robotName,
+ const std::string& robotRootNodeName) = 0;
+ };
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 5a221a342be4e0803c1cb28e708352b10b7e98f6..8613833baf8a001359f4878a5ff69a7d993658ce 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -57,7 +57,7 @@
#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
TransformReader::TransformReader(const TransformReader& t) :
memoryReader(t.memoryReader), properties(t.properties), propertyPrefix(t.propertyPrefix)
@@ -173,7 +173,7 @@ namespace armarx::armem::client::robot_state::localization
const auto& localizationCoreSegment =
qResult.memory.getCoreSegment(properties.localizationSegment);
- using armarx::armem::common::robot_state::localization::TransformHelper;
+ using armarx::armem::robot_state::localization::TransformHelper;
return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
catch (...)
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 32b3c2fc12eb46c9c67baa362d92f19552ada51d..37dc53c5c190e576af8fcc13d2d12772b719d921 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -27,7 +27,7 @@
#include "interfaces.h"
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
/**
* @defgroup Component-ExampleClient ExampleClient
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index f01f8e9241a5ebc605c6b158f3256407a5e9bd17..1a718cdc33df6ddaf9ce95f8abeef495465db48a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -47,7 +47,7 @@
#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
TransformWriter::~TransformWriter() = default;
@@ -82,7 +82,8 @@ namespace armarx::armem::client::robot_state::localization
}
bool
- TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
+ TransformWriter::commitTransform(
+ const ::armarx::armem::robot_state::localization::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
@@ -114,4 +115,4 @@ namespace armarx::armem::client::robot_state::localization
return updateResult.success;
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 182981846d14eb775256b3a29aeed9e2a4eb483b..9daf8f76360201f5ca0a65271ab1dbc17f0d42df 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -29,7 +29,7 @@
#include "interfaces.h"
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
/**
@@ -56,7 +56,8 @@ namespace armarx::armem::client::robot_state::localization
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
- bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
+ bool commitTransform(
+ const ::armarx::armem::robot_state::localization::Transform& transform) override;
private:
@@ -72,4 +73,4 @@ namespace armarx::armem::client::robot_state::localization
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
index 5689058cc27f39dbe86b0e5e1b3171466407ac50..b78e8693ca42e64c40e299fe921e10808915a7ab 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -30,11 +30,11 @@
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
- using armarx::armem::common::robot_state::localization::TransformQuery;
- using armarx::armem::common::robot_state::localization::TransformResult;
+ using armarx::armem::robot_state::localization::TransformQuery;
+ using armarx::armem::robot_state::localization::TransformResult;
class TransformInterface
{
@@ -63,7 +63,8 @@ namespace armarx::armem::client::robot_state::localization
public:
~TransformWriterInterface() override = default;
- virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
+ virtual bool
+ commitTransform(const ::armarx::armem::robot_state::localization::Transform& transform) = 0;
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index 252dd7869ec119acfb5c8bcc38a7bc0a5e3d9a6f..ac13c1fa3180d3bfe22edce4e5e8c0a54d7585db 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -24,7 +24,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-namespace armarx::armem::common::robot_state::localization
+namespace armarx::armem::robot_state::localization
{
template <class... Args>
@@ -44,7 +44,7 @@ namespace armarx::armem::common::robot_state::localization
query.header.agent + "`."};
}
- const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
+ const std::vector<Eigen::Isometry3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
const std::optional<armem::Time> sanitizedTimestamp =
@@ -76,8 +76,8 @@ namespace armarx::armem::common::robot_state::localization
"'. No memory data in time range. Reference time " + sanitizedTimestamp.value().toTimeString()};
}
- const Eigen::Affine3f transform = std::accumulate(
- transforms.begin(), transforms.end(), Eigen::Affine3f::Identity(), std::multiplies<>());
+ const Eigen::Isometry3f transform = std::accumulate(
+ transforms.begin(), transforms.end(), Eigen::Isometry3f::Identity(), std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
@@ -97,14 +97,14 @@ namespace armarx::armem::common::robot_state::localization
{
ARMARX_DEBUG << "TF chain is empty";
return {.header = query.header,
- .transforms = std::vector<Eigen::Affine3f>{},
+ .transforms = std::vector<Eigen::Isometry3f>{},
.status = TransformChainResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
" -> " + query.header.frame + "' for robot `" +
query.header.agent + "`."};
}
- const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
+ const std::vector<Eigen::Isometry3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
if (transforms.empty())
{
@@ -227,7 +227,7 @@ namespace armarx::armem::common::robot_state::localization
}
template <class... Args>
- std::vector<Eigen::Affine3f>
+ std::vector<Eigen::Isometry3f>
TransformHelper::_obtainTransforms(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const std::vector<std::string>& tfChain,
@@ -241,7 +241,7 @@ namespace armarx::armem::common::robot_state::localization
try
{
- std::vector<Eigen::Affine3f> transforms;
+ std::vector<Eigen::Isometry3f> transforms;
transforms.reserve(tfChain.size());
std::transform(tfChain.begin(),
tfChain.end(),
@@ -272,7 +272,7 @@ namespace armarx::armem::common::robot_state::localization
}
template <class... Args>
- Eigen::Affine3f
+ Eigen::Isometry3f
TransformHelper::_obtainTransform(
const std::string& entityName,
const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
@@ -287,10 +287,10 @@ namespace armarx::armem::common::robot_state::localization
// {
// // TODO(fabian.reister): fixme boom
// ARMARX_ERROR << "No snapshots received.";
- // return Eigen::Affine3f::Identity();
+ // return Eigen::Isometry3f::Identity();
// }
- std::vector<::armarx::armem::robot_state::Transform> transforms;
+ std::vector<::armarx::armem::robot_state::localization::Transform> transforms;
auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
ARMARX_CHECK(snapshot) << "No snapshot found before or at time " << timestamp;
@@ -321,20 +321,20 @@ namespace armarx::armem::common::robot_state::localization
return transforms.front().transform;
}
- ::armarx::armem::robot_state::Transform
+ ::armarx::armem::robot_state::localization::Transform
TransformHelper::_convertEntityToTransform(const armem::wm::EntityInstance& item)
{
arondto::Transform aronTransform;
aronTransform.fromAron(item.data());
- ::armarx::armem::robot_state::Transform transform;
+ ::armarx::armem::robot_state::localization::Transform transform;
fromAron(aronTransform, transform);
return transform;
}
auto
- findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
+ findFirstElementAfter(const std::vector<::armarx::armem::robot_state::localization::Transform>& transforms,
const armem::Time& timestamp)
{
const auto comp = [](const armem::Time& timestamp, const auto& transform)
@@ -342,7 +342,7 @@ namespace armarx::armem::common::robot_state::localization
const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
- auto timestampBeyond = [timestamp](const ::armarx::armem::robot_state::Transform& transform)
+ auto timestampBeyond = [timestamp](const localization::Transform& transform)
{ return transform.header.timestamp > timestamp; };
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
@@ -352,9 +352,9 @@ namespace armarx::armem::common::robot_state::localization
return poseNextIt;
}
- Eigen::Affine3f
+ Eigen::Isometry3f
TransformHelper::_interpolateTransform(
- const std::vector<::armarx::armem::robot_state::Transform>& queue,
+ const std::vector<::armarx::armem::robot_state::localization::Transform>& queue,
armem::Time timestamp)
{
ARMARX_TRACE;
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 80cdea39921497a5c2c369416d59115d7dffc4c5..fb1ffbc25eeee21b6e4bcd1fd368714db3ad6e85 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -21,8 +21,8 @@
#pragma once
-#include <vector>
#include <optional>
+#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -30,103 +30,75 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
-
-namespace armarx::armem::common::robot_state::localization
+namespace armarx::armem::robot_state::localization
{
- using armarx::armem::common::robot_state::localization::TransformQuery;
- using armarx::armem::common::robot_state::localization::TransformResult;
-
-
+ using armarx::armem::robot_state::localization::TransformQuery;
+ using armarx::armem::robot_state::localization::TransformResult;
class TransformHelper
{
public:
+ static TransformResult
+ lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
- static
- TransformResult
- lookupTransform(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
-
- static
- TransformResult
- lookupTransform(
- const armem::server::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
+ static TransformResult
+ lookupTransform(const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
- static
- TransformChainResult
- lookupTransformChain(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
+ static TransformChainResult
+ lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
- static
- TransformChainResult
- lookupTransformChain(
- const armem::server::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
+ static TransformChainResult
+ lookupTransformChain(const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
private:
+ template <class... Args>
+ static TransformResult
+ _lookupTransform(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
- template <class ...Args>
- static
- TransformResult
- _lookupTransform(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const TransformQuery& query);
-
-
- template <class ...Args>
- static
- TransformChainResult
- _lookupTransformChain(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const TransformQuery& query);
-
-
- template <class ...Args>
- static
- std::vector<std::string>
- _buildTransformChain(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const TransformQuery& query);
-
-
- template <class ...Args>
- static
- std::vector<Eigen::Affine3f>
- _obtainTransforms(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const std::vector<std::string>& tfChain,
- const std::string& agent,
- const armem::Time& timestamp);
-
-
- template <class ...Args>
- static
- Eigen::Affine3f
- _obtainTransform(
- const std::string& entityName,
- const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
- const armem::Time& timestamp);
-
- template <class ...Args>
- static
- std::optional<armarx::core::time::DateTime>
- _obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp);
-
- static
- Eigen::Affine3f
- _interpolateTransform(
- const std::vector<::armarx::armem::robot_state::Transform>& queue,
- armem::Time timestamp);
- static
- ::armarx::armem::robot_state::Transform
- _convertEntityToTransform(
- const armem::wm::EntityInstance& item);
+ template <class... Args>
+ static TransformChainResult
+ _lookupTransformChain(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ template <class... Args>
+ static std::vector<std::string>
+ _buildTransformChain(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ template <class... Args>
+ static std::vector<Eigen::Isometry3f>
+ _obtainTransforms(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp);
+
+
+ template <class... Args>
+ static Eigen::Isometry3f
+ _obtainTransform(const std::string& entityName,
+ const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
+ const armem::Time& timestamp);
+
+ template <class... Args>
+ static std::optional<armarx::core::time::DateTime>
+ _obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const armem::Time& timestamp);
+
+ static Eigen::Isometry3f _interpolateTransform(
+ const std::vector<::armarx::armem::robot_state::localization::Transform>& queue,
+ armem::Time timestamp);
+ static ::armarx::armem::robot_state::localization::Transform
+ _convertEntityToTransform(const armem::wm::EntityInstance& item);
};
-} // namespace armarx::armem::common::robot_state::localization
+} // namespace armarx::armem::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index 26311961772c03b70519de0cde245b44e17bf0db..8bd428ce382fea070e611bde7893be991910b5db 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -26,14 +26,13 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::common::robot_state::localization
+namespace armarx::armem::robot_state::localization
{
struct TransformResult
{
- ::armarx::armem::robot_state::Transform transform;
+ ::armarx::armem::robot_state::localization::Transform transform;
enum class Status
{
@@ -53,8 +52,8 @@ namespace armarx::armem::common::robot_state::localization
struct TransformChainResult
{
- ::armarx::armem::robot_state::TransformHeader header;
- std::vector<Eigen::Affine3f> transforms;
+ ::armarx::armem::robot_state::localization::TransformHeader header;
+ std::vector<Eigen::Isometry3f> transforms;
enum class Status
{
@@ -74,9 +73,9 @@ namespace armarx::armem::common::robot_state::localization
struct TransformQuery
{
- ::armarx::armem::robot_state::TransformHeader header;
+ ::armarx::armem::robot_state::localization::TransformHeader header;
// bool exact;
};
-} // namespace armarx::armem::common::robot_state::localization
+} // namespace armarx::armem::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
index 2f219eff39376ed3ecc44e47b0780fd1328785a0..07cdcd42a54502061b5c23326a81291c53dab7b0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
@@ -22,16 +22,15 @@
#include "memory_ids.h"
-
-namespace armarx::armem
+namespace armarx::armem::robot_state // ::constants
{
- const MemoryID robot_state::memoryID { "RobotState" };
+ const MemoryID memoryID{"RobotState"};
- const MemoryID robot_state::descriptionSegmentID { memoryID.withCoreSegmentName("Description") };
- const MemoryID robot_state::proprioceptionSegmentID { memoryID.withCoreSegmentName("Proprioception") };
- const MemoryID robot_state::exteroceptionSegmentID { memoryID.withCoreSegmentName("Exteroception") };
- const MemoryID robot_state::localizationSegmentID { memoryID.withCoreSegmentName("Localization") };
+ const MemoryID descriptionSegmentID{memoryID.withCoreSegmentName("Description")};
+ const MemoryID proprioceptionSegmentID{memoryID.withCoreSegmentName("Proprioception")};
+ const MemoryID exteroceptionSegmentID{memoryID.withCoreSegmentName("Exteroception")};
+ const MemoryID localizationSegmentID{memoryID.withCoreSegmentName("Localization")};
-}
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
index f653236b1f62ec8aef905d45a7ee86af079b05b9..e72fd8f7acef6c7b9418d57d63326df5a2bb35b0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
@@ -24,7 +24,6 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::robot_state
{
@@ -35,4 +34,4 @@ namespace armarx::armem::robot_state
extern const MemoryID exteroceptionSegmentID;
extern const MemoryID localizationSegmentID;
-} // namespace armarx::armem::objects
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
similarity index 71%
rename from source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
rename to source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
index 8b7829a2f0c3f6dadd54bf2ce624c9fcbe793efd..22790b52cc21f8af15877f147d620e735b79f520 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
@@ -2,23 +2,22 @@
#include <filesystem>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace fs = ::std::filesystem;
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state
{
- std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance)
+ std::optional<description::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance)
{
arondto::RobotDescription aronRobotDescription;
try
@@ -31,8 +30,7 @@ namespace armarx::armem::robot
return std::nullopt;
}
- RobotDescription robotDescription
- {
+ description::RobotDescription robotDescription{
.name = "",
.xml = ::armarx::PackagePath("", fs::path("")) // initialize empty, no default c'tor
};
@@ -42,8 +40,8 @@ namespace armarx::armem::robot
return robotDescription;
}
-
- std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance)
+ std::optional<RobotState>
+ convertRobotState(const armem::wm::EntityInstance& instance)
{
arondto::Robot aronRobot;
try
@@ -62,4 +60,4 @@ namespace armarx::armem::robot
return robotState;
}
-} // namespace armarx::armem::robot
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.h b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
similarity index 62%
rename from source/RobotAPI/libraries/armem_robot/robot_conversions.h
rename to source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
index 3692ae048dacb94d2f24469f5265e998ab23fb34..0e4f9a4fb3a8858963a23c6f84f679f730405698 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
@@ -9,8 +9,8 @@ namespace armarx::armem::wm
class EntityInstance;
}
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state
{
- std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
+ std::optional<description::RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance);
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 54d5823876f19fb00a6e9e6ec57ecc89db0c3a13..bfd08725caff157962ee91207954cdaba9116467 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -21,8 +21,9 @@ armarx_add_library(
RobotAPICore
RobotAPIInterfaces
RobotStatechartHelpers
+ RobotAPI::ComponentPlugins
+ ArViz
RobotAPI::armem_server
- RobotAPI::armem_robot
RobotAPI::armem_robot_state
aroncommon
aroneigenconverter
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 526d92d04927849420925e4345c3a28eaf983911..332dfd87d4216e9102fe7f3f925fa65299a7b244 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -84,9 +84,9 @@ namespace armarx::armem::server::robot_state
}
void
- Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots, bool useColModel)
+ Visu::visualizeRobots(viz::Layer& layer, const armem::robot_state::Robots& robots, bool useColModel)
{
- for (const robot::Robot& robot : robots)
+ for (const armem::robot_state::Robot& robot : robots)
{
const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
@@ -113,17 +113,17 @@ namespace armarx::armem::server::robot_state
void
Visu::visualizeFrames(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
{
for (const auto& [robotName, robotFrames] : frames)
{
- Eigen::Affine3f pose = Eigen::Affine3f::Identity();
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
int i = 0;
for (const auto& frame : robotFrames)
{
- Eigen::Affine3f from = pose;
- Eigen::Affine3f to = pose * frame;
+ Eigen::Isometry3f from = pose;
+ Eigen::Isometry3f to = pose * frame;
pose = to;
@@ -136,7 +136,7 @@ namespace armarx::armem::server::robot_state
void
Visu::visualizeFramesIndividual(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
{
std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
@@ -235,7 +235,7 @@ namespace armarx::armem::server::robot_state
<< "\n- " << globalPoses.size() << " global poses"
<< "\n- " << sensorValues.size() << " joint positions";
- const robot::Robots robots =
+ const armem::robot_state::Robots robots =
combine(robotDescriptions, globalPoses, sensorValues, timestamp);
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
@@ -271,7 +271,7 @@ namespace armarx::armem::server::robot_state
viz::Layer& layerForceTorque = layers.emplace_back(arviz.layer("Force Torque"));
std::stringstream log;
- for (const robot::Robot& robot : robots)
+ for (const armem::robot_state::Robot& robot : robots)
{
const std::string& robotName = robot.description.name;
@@ -326,7 +326,7 @@ namespace armarx::armem::server::robot_state
void
Visu::visualizeForceTorque(viz::Layer& layer,
- const robot::Robot& robot,
+ const armem::robot_state::Robot& robot,
const proprioception::SensorValues& sensorValues)
{
const std::string& robotName = robot.description.name;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index ede55c7a3f1cedb5aaba163d58bba4df46a465df..0cba9d7212e32c4374a268c8124cbf09d7f5702e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -23,13 +23,15 @@
#include <optional>
+#include <ArmarXCore/core/time.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
-#include <RobotAPI/libraries/armem_objects/types.h>
+// #include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
namespace armarx::armem::server::robot_state
@@ -55,22 +57,22 @@ namespace armarx::armem::server::robot_state
private:
void visualizeRun();
- void visualizeOnce(const Time& timestamp);
+ void visualizeOnce(const core::time::DateTime& timestamp);
- static void visualizeRobots(viz::Layer& layer, const robot::Robots& robots, bool useColModel = false);
+ static void visualizeRobots(viz::Layer& layer, const armem::robot_state::Robots& robots, bool useColModel = false);
static void visualizeFrames(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames);
static void visualizeFramesIndividual(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames);
void visualizeForceTorque(viz::Layer& layer,
- const robot::Robot& robot,
+ const armem::robot_state::Robot& robot,
const proprioception::SensorValues& sensorValues);
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
index 997253f25c8bce846e3dddfe8e14aa9f04414d78..71e9cbf253068d19925ae0e5ae43725fa9ffba35 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
@@ -1,39 +1,37 @@
#include "combine.h"
-#include <RobotAPI/libraries/armem_robot/types.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
+#include <sstream>
#include <ArmarXCore/core/logging/Logging.h>
-#include <sstream>
-
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::server
+namespace armarx::armem::server::robot_state
{
- robot::Robots
- robot_state::combine(
- const description::RobotDescriptionMap& robotDescriptions,
- const localization::RobotPoseMap& globalPoses,
- const proprioception::SensorValuesMap& sensorValues,
- const armem::Time& timestamp)
+ armem::robot_state::Robots
+ combine(const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::SensorValuesMap& sensorValues,
+ const armem::Time& timestamp)
{
std::stringstream logs;
- robot::Robots robots;
+ armem::robot_state::Robots robots;
for (const auto& [robotName, robotDescription] : robotDescriptions)
{
// Handle missing values gracefully instead of skipping the robot altogether.
- robot::Robot& robot = robots.emplace_back();
+ armem::robot_state::Robot& robot = robots.emplace_back();
robot.description = robotDescription;
- robot.instance = ""; // TODO(fabian.reister): set this properly
- robot.config.timestamp = timestamp;
- robot.config.globalPose = Eigen::Affine3f::Identity();
- robot.config.jointMap = {};
- robot.timestamp = timestamp;
+ robot.instance = ""; // TODO(fabian.reister): set this properly
+ robot.config.timestamp = timestamp;
+ robot.config.globalPose = Eigen::Isometry3f::Identity();
+ robot.config.jointMap = {};
+ robot.timestamp = timestamp;
if (auto it = globalPoses.find(robotName); it != globalPoses.end())
{
@@ -65,4 +63,4 @@ namespace armarx::armem::server
return robots;
}
-}
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
index b96ca240c552426651ed4871779ea747604706cf..328aa586e88f8387d30ff9d0d489183a40809a13 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
@@ -24,15 +24,12 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-
namespace armarx::armem::server::robot_state
{
- robot::Robots
- combine(
- const description::RobotDescriptionMap& robotDescriptions,
- const localization::RobotPoseMap& globalPoses,
- const proprioception::SensorValuesMap& sensorValues,
- const armem::Time& timestamp);
+ armem::robot_state::Robots combine(const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::SensorValuesMap& sensorValues,
+ const armem::Time& timestamp);
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 7a8f6ae0369050000b2a1b8f4df57ec4de4f050a..13587664c03a0c31ab83e8295561372e07630223 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -8,7 +8,7 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
@@ -18,10 +18,11 @@
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
@@ -45,7 +46,7 @@ namespace armarx::armem::server::robot_state::description
}
- void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
+ void Segment::commitRobotDescription(const armem::robot_state::description::RobotDescription& robotDescription)
{
const Time now = Time::Now();
@@ -61,7 +62,7 @@ namespace armarx::armem::server::robot_state::description
update.arrivedTime = update.referencedTime = update.sentTime = now;
arondto::RobotDescription dto;
- robot::toAron(dto, robotDescription);
+ toAron(dto, robotDescription);
update.instancesData =
{
dto.toAron()
@@ -83,7 +84,13 @@ namespace armarx::armem::server::robot_state::description
const std::string robotFilename = kinematicUnit->getRobotFilename();
const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ // const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ const std::string package = simox::alg::split(robotFilename, "/").front();
+
+ ARMARX_CHECK_NOT_EMPTY(package);
+
+ ARMARX_INFO << VAROUT(package);
+ ARMARX_INFO << VAROUT(robotFilename);
// make sure that the relative path is without the 'package/' prefix
const std::string robotFileRelPath = [&robotFilename, &package]()-> std::string {
@@ -100,7 +107,7 @@ namespace armarx::armem::server::robot_state::description
ARMARX_INFO << "Robot description '" << VAROUT(robotFileRelPath) << "' found in package " << package;
- const robot::RobotDescription robotDescription
+ const armem::robot_state::description::RobotDescription robotDescription
{
.name = kinematicUnit->getRobotName(),
.xml = {package, robotFileRelPath}
@@ -139,7 +146,7 @@ namespace armarx::armem::server::robot_state::description
segmentPtr->forEachEntity([this, &robotDescriptions](const wm::Entity & entity)
{
const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = robot::convertRobotDescription(entityInstance);
+ const auto description = ::armarx::armem::robot_state::convertRobotDescription(entityInstance);
if (description)
{
ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index fb617db14ae43f7b7f55d6ed8f88223f748a9ad3..9ea6ec26e0e6dc4f9ff89e30425166b3cdf66159 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -22,9 +22,9 @@
#pragma once
// STD / STL
-#include <string>
-#include <optional>
#include <mutex>
+#include <optional>
+#include <string>
#include <unordered_map>
// ArmarX
@@ -34,8 +34,8 @@
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
// Aron
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::server::robot_state::description
{
@@ -45,7 +45,6 @@ namespace armarx::armem::server::robot_state::description
using Base = segment::SpecializedCoreSegment;
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
@@ -58,12 +57,11 @@ namespace armarx::armem::server::robot_state::description
private:
-
- void commitRobotDescription(const robot::RobotDescription& robotDescription);
+ void commitRobotDescription(
+ const armem::robot_state::description::RobotDescription& robotDescription);
void updateRobotDescription();
RobotUnitInterfacePrx robotUnit;
-
};
-} // namespace armarx::armem::server::robot_state::description
+} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
index 63f24d0f46a156adc023b3b64b4f27d344b151c6..bb74546dd41aacb7460101f146fdf7145cfdc0eb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
@@ -16,10 +16,10 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
index e8471563d640800a44d4516d6eccdd7d7e355ccd..3b6d0042909223d93e7c69555ea96a2fe89d06f6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
@@ -29,7 +29,7 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
index 1b9c39543b21587f30a32feb2dfbbcac4fb5e257..f4b4a871bb499eb0d857a0cde0227a1f3b10d813 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
@@ -28,8 +28,6 @@
#include <Eigen/Geometry>
-
-
namespace armarx::armem::arondto
{
struct Transform;
@@ -38,18 +36,22 @@ namespace armarx::armem::arondto
struct JointState;
class RobotDescription;
-}
+} // namespace armarx::armem::arondto
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state
{
- struct RobotDescription;
+ namespace description
+ {
+ struct RobotDescription;
+ using RobotDescriptions = std::vector<RobotDescription>;
+
+ } // namespace description
struct RobotState;
struct Robot;
using Robots = std::vector<Robot>;
- using RobotDescriptions = std::vector<RobotDescription>;
using RobotStates = std::vector<RobotState>;
-}
+} // namespace armarx::armem::robot_state
namespace armarx::armem::robot_state
{
@@ -57,20 +59,21 @@ namespace armarx::armem::robot_state
struct Transform;
struct TransformHeader;
-}
+} // namespace armarx::armem::robot_state
namespace armarx::armem::server::robot_state::description
{
- using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
+ using RobotDescriptionMap =
+ std::unordered_map<std::string, armarx::armem::robot_state::description::RobotDescription>;
class Segment;
-}
+} // namespace armarx::armem::server::robot_state::description
namespace armarx::armem::server::robot_state::localization
{
- using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
- using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Affine3f>>;
+ using RobotPoseMap = std::unordered_map<std::string, Eigen::Isometry3f>;
+ using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>;
class Segment;
-}
+} // namespace armarx::armem::server::robot_state::localization
namespace armarx::armem::server::robot_state::proprioception
{
@@ -81,5 +84,4 @@ namespace armarx::armem::server::robot_state::proprioception
using ForceTorqueValuesMap = std::unordered_map<std::string, ForceTorqueValues>;
using SensorValuesMap = std::unordered_map<std::string, SensorValues>;
class Segment;
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index a503bae532af41ee91809d394c22cd74f4aab071..7384f90ed1af0f6237f98c570c4b855a31cef583 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -19,8 +19,8 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -80,8 +80,8 @@ namespace armarx::armem::server::robot_state::localization
RobotFramePoseMap
Segment::getRobotFramePoses(const armem::Time& timestamp) const
{
- using common::robot_state::localization::TransformHelper;
- using common::robot_state::localization::TransformQuery;
+ using ::armarx::armem::robot_state::localization::TransformHelper;
+ using ::armarx::armem::robot_state::localization::TransformQuery;
RobotFramePoseMap frames;
for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
@@ -126,8 +126,8 @@ namespace armarx::armem::server::robot_state::localization
RobotPoseMap
Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
{
- using common::robot_state::localization::TransformHelper;
- using common::robot_state::localization::TransformQuery;
+ using ::armarx::armem::robot_state::localization::TransformHelper;
+ using ::armarx::armem::robot_state::localization::TransformQuery;
RobotPoseMap robotGlobalPoses;
for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
@@ -160,7 +160,7 @@ namespace armarx::armem::server::robot_state::localization
}
- bool Segment::commitTransform(const armarx::armem::robot_state::Transform& transform)
+ bool Segment::commitTransform(const armarx::armem::robot_state::localization::Transform& transform)
{
Commit commit;
commit.add(makeUpdate(transform));
@@ -170,7 +170,7 @@ namespace armarx::armem::server::robot_state::localization
}
- bool Segment::commitTransformLocking(const armarx::armem::robot_state::Transform& transform)
+ bool Segment::commitTransformLocking(const armarx::armem::robot_state::localization::Transform& transform)
{
Commit commit;
commit.add(makeUpdate(transform));
@@ -180,7 +180,7 @@ namespace armarx::armem::server::robot_state::localization
}
- EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::Transform& transform) const
+ EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::localization::Transform& transform) const
{
const armem::Time& timestamp = transform.header.timestamp;
const MemoryID providerID = segmentPtr->id().withProviderSegmentName(transform.header.agent);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index 0f1fe71345bf1d2767f5cfc7b0c268e1f41e7eba..b974ff31af81496c7d320953f16ae57900a7c897 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -36,6 +36,7 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::server::robot_state::localization
@@ -62,13 +63,13 @@ namespace armarx::armem::server::robot_state::localization
RobotFramePoseMap getRobotFramePoses(const armem::Time& timestamp) const;
RobotFramePoseMap getRobotFramePosesLocking(const armem::Time& timestamp) const;
- bool commitTransform(const armem::robot_state::Transform& transform);
- bool commitTransformLocking(const armem::robot_state::Transform& transform);
+ bool commitTransform(const armem::robot_state::localization::Transform& transform);
+ bool commitTransformLocking(const armem::robot_state::localization::Transform& transform);
private:
- EntityUpdate makeUpdate(const armem::robot_state::Transform& transform) const;
+ EntityUpdate makeUpdate(const armem::robot_state::localization::Transform& transform) const;
PredictionResult predictLinear(const PredictionRequest& request);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 5c4580c4c8c5d4540b0867365c657807d0502a7d..611257b20771979a763daf62202b3aa96f244ceb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -98,7 +98,7 @@ namespace armarx::armem::server::robot_state::proprioception
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
- for (const armem::robot_state::Transform& transform : update.localization)
+ for (const armem::robot_state::localization::Transform& transform : update.localization)
{
localizationSegment.doLocked(
[&localizationSegment, &transform]()
@@ -210,7 +210,7 @@ namespace armarx::armem::server::robot_state::proprioception
return update;
}
- armem::robot_state::Transform
+ armem::robot_state::localization::Transform
RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
const Time& timestamp) const
{
@@ -219,11 +219,11 @@ namespace armarx::armem::server::robot_state::proprioception
const Eigen::Vector3f& relPose = platform.relativePosition;
- Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ Eigen::Isometry3f odometryPose = Eigen::Isometry3f::Identity();
odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
- armem::robot_state::Transform transform;
+ armem::robot_state::localization::Transform transform;
transform.header.parentFrame = armarx::OdometryFrame;
transform.header.frame = armarx::armem::robot_state::constants::robotRootNodeName;
transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 27922266466b6156bb8f217dc887699a193027d9..917a122df0f8fda9b3cf875f228a2bd63e86e4c2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -72,14 +72,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
armem::Commit proprioception;
armem::Commit exteroception;
- std::vector<armem::robot_state::Transform> localization;
+ std::vector<armem::robot_state::localization::Transform> localization;
};
Update buildUpdate(const RobotUnitData& dataQueue);
private:
- armem::robot_state::Transform
+ armem::robot_state::localization::Transform
getTransform(
const aron::data::DictPtr& platformData,
const Time& timestamp) const;
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");
}
diff --git a/source/RobotAPI/libraries/armem_robot/types.cpp b/source/RobotAPI/libraries/armem_robot_state/types.cpp
similarity index 59%
rename from source/RobotAPI/libraries/armem_robot/types.cpp
rename to source/RobotAPI/libraries/armem_robot_state/types.cpp
index 6f09eee1acef62472a1e8cc0027f49c9db6efe46..055973126ac840dce8208f05917d45a8c15e0771 100644
--- a/source/RobotAPI/libraries/armem_robot/types.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/types.cpp
@@ -2,23 +2,27 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state::description
{
- std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const RobotDescription& rhs)
{
os << "RobotDescription { name: '" << rhs.name << "', xml: '" << rhs.xml << "' }";
return os;
}
+} // namespace armarx::armem::robot_state::description
+namespace armarx::armem::robot_state
+{
- std::string Robot::name() const
+ std::string
+ Robot::name() const
{
ARMARX_CHECK_NOT_EMPTY(description.name) << "The robot name must be set!";
ARMARX_CHECK_NOT_EMPTY(instance) << "The robot instance name must be provided!";
return description.name + "/" + instance;
}
-
-}
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index b6d2b67dec2b28425e7b3dd4b0630fc39df02017..ef1b9c1e9fff8e8cb09cdf1dfa2c869231ec0423 100644
--- a/source/RobotAPI/libraries/armem_robot_state/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -21,36 +21,137 @@
#pragma once
+#include <filesystem>
+#include <map>
+#include <vector>
+
#include <Eigen/Geometry>
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot_state
{
- struct TransformHeader
+
+ namespace description
{
- std::string parentFrame;
- std::string frame;
+ struct RobotDescription
+ {
+ // DateTime timestamp;
+
+ std::string name;
+ PackagePath xml{"", std::filesystem::path("")};
+ };
- std::string agent;
+ using RobotDescriptions = std::vector<RobotDescription>;
- armem::Time timestamp;
+ std::ostream& operator<<(std::ostream& os, const RobotDescription& rhs);
+
+
+ } // namespace description
+
+ struct Twist
+ {
+ Eigen::Vector3f linear{Eigen::Vector3f::Zero()};
+ Eigen::Vector3f angular{Eigen::Vector3f::Zero()};
};
- struct Transform
+ struct PlatformState
{
- TransformHeader header;
+ Twist twist;
+ };
+
+ namespace proprioception
+ {
+
+ struct ForceTorque
+ {
+ Eigen::Vector3f force;
+ Eigen::Vector3f gravityCompensatedForce;
+ Eigen::Vector3f torque;
+ Eigen::Vector3f gravityCompensatedTorque;
+ };
+
+ struct JointState
+ {
+ std::string name;
+ float position;
+ };
+
+ } // namespace proprioception
+
+ namespace exteroception
+ {
+
+ struct ToF
+ {
+ using DepthMatrixType = Eigen::MatrixXf; // FIXME uint16
+ using DepthSigmaMatrixType = Eigen::MatrixXf; // FIXME uint8
+ using TargetDetectedMatrixType = Eigen::MatrixXf; // FIXME uint8
+
+ DepthMatrixType depth;
+ DepthSigmaMatrixType sigma;
+ TargetDetectedMatrixType targetDetected;
- Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+ std::string frame;
+ };
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ } // namespace exteroception
+
+ struct RobotState
+ {
+ using JointMap = std::map<std::string, float>;
+ using Pose = Eigen::Isometry3f;
+
+ DateTime timestamp;
+
+ Pose globalPose;
+ JointMap jointMap;
+
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
};
- struct JointState
+ struct Robot
{
- std::string name;
- float position;
+ description::RobotDescription description;
+ std::string instance;
+
+ RobotState config;
+
+ DateTime timestamp;
+
+ std::string name() const;
};
-} // namespace armarx::armem::robot_state
+ using Robots = std::vector<Robot>;
+ using RobotStates = std::vector<RobotState>;
+
+ namespace localization
+ {
+
+ struct TransformHeader
+ {
+ std::string parentFrame;
+ std::string frame;
+
+ std::string agent;
+
+ armem::Time timestamp;
+ };
+
+ struct Transform
+ {
+ TransformHeader header;
+
+ Eigen::Isometry3f transform = Eigen::Isometry3f::Identity();
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+ } // namespace localization
+
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.cpp b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
index 9f4b84fdfbee87a4ca768837aa41039f1970115e..5e35c27c75eb84e0493008290879dcecec22efd1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/utils.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
@@ -2,10 +2,10 @@
namespace armarx::armem::robot_state
{
- armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc)
+ armarx::armem::MemoryID makeMemoryID(const description::RobotDescription& desc)
{
return MemoryID("RobotState/Description")
.withProviderSegmentName(desc.name)
.withEntityName("description");
}
-}
\ No newline at end of file
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.h b/source/RobotAPI/libraries/armem_robot_state/utils.h
index 9b495f91f30544fbdb2cfc2f68ee10bb111e1903..0119abe3a9377385197addd404331fd3162b6fec 100644
--- a/source/RobotAPI/libraries/armem_robot_state/utils.h
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.h
@@ -1,9 +1,9 @@
#pragma once
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::robot_state
{
- armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc);
+ armarx::armem::MemoryID makeMemoryID(const description::RobotDescription& desc);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
index 3b05caec20fae2d7a43b406f043e4c2da703923c..9923f490f5e439367946230e5584cc4a865c9e2c 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
@@ -22,12 +22,10 @@ armarx_add_test(
TEST_FILE
aronOperatorTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
aron
aroncommon
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -39,9 +37,10 @@ armarx_add_test(
TEST_FILE
aronCodeGenerationTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
+ Eigen3::Eigen
ARON_FILES
aron/AnyTest.xml
aron/TemplateTest.xml
@@ -62,8 +61,6 @@ armarx_add_test(
aron/OptionalTest.xml
aron/DtoTest.xml
INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
- ${Eigen3_INCLUDE_DIR}
${IVT_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
@@ -78,13 +75,11 @@ armarx_add_test(
TEST_FILE
aronNavigateTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/NaturalIKTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -96,14 +91,12 @@ armarx_add_test(
TEST_FILE
aronExtendsTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/BaseClassTest.xml
aron/DerivedClassTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -115,13 +108,11 @@ armarx_add_test(
TEST_FILE
aronConversionTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/HumanPoseTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -133,14 +124,12 @@ armarx_add_test(
TEST_FILE
aronJsonExportTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/OptionalTest.xml
aron/MatrixTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
########################
@@ -152,13 +141,14 @@ armarx_add_test(
# TEST_FILE
# aronRandomizedTest.cpp
# LIBS
-# SimoxUtility # Simox::SimoxUtility
+# Simox::SimoxUtility
# ArmarXCore
# RobotAPI::aron
# ivt
# ivtopencv
# ${PCL_COMMON_LIBRARIES}
# aronjsonconverter
+# Eigen3::Eigen
# ARON_FILES
# aron/DictTest.xml
# aron/MatrixTest.xml
@@ -176,8 +166,6 @@ armarx_add_test(
# aron/OptionalTest.xml
# aron/ImageTest.xml
# INCLUDE_DIRECTORIES
-# ${Simox_INCLUDE_DIR}
-# ${Eigen3_INCLUDE_DIR}
# ${IVT_INCLUDE_DIRS}
# ${OpenCV_INCLUDE_DIRS}
# ${PCL_INCLUDE_DIRS}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
index b0799cdbd06bc44004359ebf87a1e1469296f18d..d060c70606a439a9b7abe8ec03f89c565aefaa17 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
@@ -508,7 +508,7 @@ namespace armarx::aron::data
RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(DataInput& o, TypeInput& t)
{
std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>> ret;
- auto data = aron::data::Dict::DynamicCastAndCheck(o);
+ auto data = aron::data::Dict::DynamicCast(o);
auto type = aron::type::Object::DynamicCastAndCheck(t);
if (data)
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 7756a2cd5be8edad7ee10e8fdcdc25b4501cb008..5cf0313b9a1ac623134646c345d9fdbc85bf0faf 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -1,22 +1,18 @@
armarx_set_target("RobotAPI Core Library: RobotAPICore")
find_package(Eigen3 QUIET)
-find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+find_package(Simox ${ArmarX_Simox_VERSION} REQUIRED)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-if (Eigen3_FOUND AND Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
- include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
-endif()
-
set(LIB_NAME RobotAPICore)
set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
- VirtualRobot
- Saba
- SimDynamics
+ Simox::VirtualRobot
+ # Simox::Saba
+ # Simox::SimDynamics
+ Eigen3::Eigen
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/skills_gui/CMakeLists.txt b/source/RobotAPI/libraries/skills_gui/CMakeLists.txt
index a1e53c1a2d96b3a6e50d4720efbd9be38d6760de..2d2e9b57de36b5119a3bcf3e3802ffc9bca76548 100644
--- a/source/RobotAPI/libraries/skills_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills_gui/CMakeLists.txt
@@ -63,7 +63,7 @@ set(SOURCES
SkillMemoryGui.cpp
gui_utils.cpp
- StatusLabel.cpp
+
)
set(HEADERS
aron_tree_widget/visitors/AronTreeWidgetCreator.h
@@ -105,7 +105,6 @@ set(HEADERS
SkillMemoryGui.h
gui_utils.h
- StatusLabel.h
)
armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${LIBRARIES}")
diff --git a/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.cpp b/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.cpp
index 4d74cf11e2b5c208a30eb553c986a9636dabd562..ac0cfaa2420a1dd2d11d01b77a4fb38c02a62fc6 100644
--- a/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.cpp
+++ b/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.cpp
@@ -105,22 +105,25 @@ namespace armarx::skills::gui
&PeriodicUpdateWidget::startTimerIfEnabled);
connect(this, &SkillMemoryGUI::connectGui, skillGroupBox, &SkillGroupBox::connectGui);
- // update cascade
- connect(this, &SkillMemoryGUI::updateGui, skillGroupBox, &SkillGroupBox::updateGui);
+ // timer
+ connect(this->updateWidget,
+ &PeriodicUpdateWidget::update,
+ this->memory.get(),
+ &SkillManagerWrapper::updateFromMemory);
+
+ // updates
+ connect(this->memory.get(),
+ &SkillManagerWrapper::updateAvailable,
+ this,
+ &SkillMemoryGUI::updateGui);
+
+ connect(this, &SkillMemoryGUI::updateGui, this->skillGroupBox, &SkillGroupBox::updateGui);
- // we don't want to update the skill details periodically, update can be triggered manually
- /*
- connect(
- this, &SkillMemoryGUI::updateGui, skillDetailGroupBox, &SkillDetailGroupBox::updateGui);
- */
connect(this,
&SkillMemoryGUI::updateGui,
- skillExecutionTreeWidget,
- &SkillExecutionTreeWidget::updateExecutions);
+ this->skillExecutionTreeWidget,
+ &SkillExecutionTreeWidget::updateGui);
- // timer -> update
- connect(
- this->updateWidget, &PeriodicUpdateWidget::update, this, &SkillMemoryGUI::updateGui);
// stop all
connect(stopAllButton,
diff --git a/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.h b/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.h
index 64361f811ef2251ad679fad0c2efa1395a5aa563..09c26b287bc62ebd08ff3ee58366285bfc419860 100644
--- a/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.h
+++ b/source/RobotAPI/libraries/skills_gui/SkillMemoryGui.h
@@ -10,8 +10,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/PeriodicUpdateWidget.h>
-
-#include "RobotAPI/libraries/skills_gui/StatusLabel.h"
+#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/StatusLabel.h>
#include "./aron_tree_widget/widgets/SkillDescriptionWidget.h"
#include "./executions/SkillExecutionTreeWidget.h"
@@ -53,10 +52,7 @@ namespace armarx::skills::gui
*/
void connectGui(std::string observerName);
- /**
- * @brief Notify widgets to update themselves
- */
- void updateGui();
+ void updateGui(SkillManagerWrapper::Snapshot update);
private:
void setupUi();
diff --git a/source/RobotAPI/libraries/skills_gui/StatusLabel.cpp b/source/RobotAPI/libraries/skills_gui/StatusLabel.cpp
deleted file mode 100644
index 6b65f166aa9677f0eed55b95436a1a76777471f3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills_gui/StatusLabel.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "StatusLabel.h"
-
-#include <QHBoxLayout>
-
-namespace armarx::skills::gui
-{
-
- StatusLabel::StatusLabel()
- {
- this->label = new QLabel("");
- this->resetButton = new QPushButton("");
- this->setupUi();
- }
-
- void
- StatusLabel::handleMessage(const std::string& message, std::string const& error)
- {
- this->label->setText(QString::fromStdString(message));
- this->resetButton->setHidden(false);
- label->setToolTip(QString::fromStdString(error));
- }
-
- void
- StatusLabel::resetLabel()
- {
- this->label->setText(QString::fromStdString(""));
- this->resetButton->setHidden(true);
- }
-
- void
- StatusLabel::setupUi()
- {
- QHBoxLayout* layout = new QHBoxLayout();
- layout->addWidget(resetButton);
- layout->addWidget(label);
- this->setLayout(layout);
- layout->setStretch(1, 2);
- label->setStyleSheet("QLabel { color : red; }");
- this->resetButton->setHidden(true);
-
- label->setMinimumHeight(35);
- label->setMaximumHeight(35);
-
- QPixmap pixmap(":/icons/delete.ico");
- QIcon ButtonIcon(pixmap);
- resetButton->setIcon(ButtonIcon);
- resetButton->setIconSize(pixmap.rect().size() / 2);
-
- connect(this->resetButton, &QPushButton::clicked, this, &StatusLabel::resetLabel);
- }
-
-} // namespace armarx::skills::gui
diff --git a/source/RobotAPI/libraries/skills_gui/StatusLabel.h b/source/RobotAPI/libraries/skills_gui/StatusLabel.h
deleted file mode 100644
index b8e0f91e425e3715d5d5f617f877cfeeceee9a39..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills_gui/StatusLabel.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include <QLabel>
-#include <QPushButton>
-
-namespace armarx::skills::gui
-{
- class StatusLabel : public QWidget
- {
- public:
- /**
- * @brief Constructor for StatusLabel
- */
- StatusLabel();
-
- public slots:
- /**
- * @brief Display a message to indicate an update.
- */
- void handleMessage(std::string const& message, std::string const& error);
-
- private slots:
- /**
- * @brief Reset the label to default state.
- */
- void resetLabel();
-
- private:
- void setupUi();
-
- // contents
- QLabel* label = nullptr;
- QPushButton* resetButton = nullptr;
- };
-} // namespace armarx::skills::gui
diff --git a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
index aae241ae41b69d9770544708373be904023a671b..780c2bb728874f9865d7147ee2a17be55e925527 100644
--- a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
@@ -20,11 +20,11 @@ namespace armarx::skills::gui
// Stop skill
QAction* stopSkillAction = new QAction("Stop execution", this);
- const auto& executions = memory->fetchExecutions();
+ const auto& executions = memory->getExecutions();
if (executions.count(selectedExecution.skillExecutionId) == 0)
return;
skills::SkillStatus currentStatus =
- memory->fetchExecutions().at(selectedExecution.skillExecutionId).status;
+ memory->getExecutions().at(selectedExecution.skillExecutionId).status;
stopSkillAction->setDisabled(currentStatus == skills::SkillStatus::Aborted ||
currentStatus == skills::SkillStatus::Failed ||
currentStatus == skills::SkillStatus::Succeeded);
@@ -60,7 +60,7 @@ namespace armarx::skills::gui
return;
// we don't want to hold state in the gui, so we need to get the parameters from memory:
skills::SkillExecutionID currentExecutionId = this->selectedExecution.skillExecutionId;
- auto executions = memory->fetchExecutions();
+ auto executions = memory->getExecutions();
if (executions.empty())
return;
@@ -140,13 +140,14 @@ namespace armarx::skills::gui
}
void
- SkillExecutionTreeWidget::updateExecutions()
+ SkillExecutionTreeWidget::updateGui(SkillManagerWrapper::Snapshot update)
{
- auto currentManagerStatuses = memory->fetchExecutions();
- if (currentManagerStatuses.empty())
+ if (update.statuses.empty())
+ {
return;
+ }
- for (const auto& [k, v] : currentManagerStatuses)
+ for (const auto& [k, v] : update.statuses)
{
skills::SkillExecutionID executionId = k;
skills::SkillStatusUpdate statusUpdate = v;
diff --git a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.h b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.h
index 795045302b4b47655e196e4cea566d53140f90d6..09fbdae081ed61aed705a278cbc903c053f30a80 100644
--- a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.h
+++ b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.h
@@ -9,6 +9,7 @@
namespace armarx::skills::gui
{
+ using StatusMap = std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>;
class SkillExecutionTreeWidget : public QTreeWidget, public MemoryCommunicatorBase
{
@@ -38,7 +39,7 @@ namespace armarx::skills::gui
public slots:
void disconnectGui();
- void updateExecutions();
+ void updateGui(SkillManagerWrapper::Snapshot update);
private slots:
void executionSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem* previous);
diff --git a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
index d8ff3fd78b6d76c4b8d0ca7d805260e8bccd4681..1b7b492b5c24dcc87f536eccb2dffae29a94ca89 100644
--- a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
+++ b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
@@ -10,8 +10,8 @@ namespace armarx::skills::gui
using SkillMap =
std::map<skills::ProviderID, std::map<skills::SkillID, skills::SkillDescription>>;
- const StatusMap
- SkillManagerWrapper::fetchExecutions()
+ StatusMap
+ SkillManagerWrapper::fetchExecutionsFromMemory()
{
if (!memory)
{
@@ -74,16 +74,20 @@ namespace armarx::skills::gui
return true;
}
- void
- SkillManagerWrapper::filterSkillUpdate(
- std::map<skills::manager::dto::SkillID, skills::manager::dto::SkillDescription>& update)
+ SkillManagerWrapper::Snapshot
+ SkillManagerWrapper::filterUpdate(Snapshot update)
{
// empty search => do not filter
if (this->currentSkillSearch.empty())
{
- return;
+ return update;
}
+ Snapshot filtered;
+
+ // for now we don't change the executions
+ filtered.statuses = update.statuses;
+
std::vector<std::string> substrings;
{
@@ -95,22 +99,31 @@ namespace armarx::skills::gui
}
- for (auto it = update.begin(); it != update.end();)
+ for (auto& provider_and_descrMap : update.skills)
{
- if (not matches(simox::alg::to_lower(skills::SkillID::FromIce(it->first).skillName),
- substrings))
- {
- it = update.erase(it);
- }
- else
+ using DescriptionMap = std::map<skills::SkillID, skills::SkillDescription>;
+
+ DescriptionMap& descriptionMap = provider_and_descrMap.second;
+ skills::ProviderID provider = provider_and_descrMap.first;
+
+ for (auto& skill_and_description : descriptionMap)
{
- ++it;
+ skills::SkillID sid = skill_and_description.first;
+ skills::SkillDescription descr = skill_and_description.second;
+
+ if (matches(simox::alg::to_lower(sid.skillName), substrings))
+ {
+ // add to map
+ filtered.skills[provider][sid] = descr;
+ }
}
}
+
+ return filtered;
}
- const SkillMap
- SkillManagerWrapper::fetchSkills()
+ SkillMap
+ SkillManagerWrapper::fetchSkillsFromMemory()
{
if (!memory)
{
@@ -125,9 +138,6 @@ namespace armarx::skills::gui
auto managerSkills = memory->getSkillDescriptions();
- this->filterSkillUpdate(managerSkills);
-
-
for (const auto& [sid, desc] : managerSkills)
{
auto description = skills::SkillDescription::FromIce(desc);
@@ -179,13 +189,29 @@ namespace armarx::skills::gui
SkillManagerWrapper::acceptSearchRequest(std::string const& search)
{
this->currentSkillSearch = search;
+ emit searchAccepted();
}
void
SkillManagerWrapper::stopAllExecutions()
{
ARMARX_IMPORTANT << "Stopping all running executions.";
- const auto& executions = this->fetchExecutions();
+
+ StatusMap executions;
+
+ // we ALWAYS want the newest information when stopping all!
+ // e.g. there is some new skill not known to the GUI which we explicitely want to stop too.
+ // the stop-all function is often used in an emergency, so we'll live with the extra call...
+ try
+ {
+ executions = this->fetchExecutionsFromMemory();
+ }
+ catch (...) // if any error occurs, we use the snapshot as backup. better to miss a skill
+ // than to not do anything.
+ {
+ executions = this->getExecutions();
+ }
+
for (auto& [executionId, status] : executions)
{
// select all running executions...
@@ -197,6 +223,18 @@ namespace armarx::skills::gui
}
}
+ void
+ SkillManagerWrapper::updateFromMemory()
+ {
+ std::scoped_lock l(mutex_snapshot);
+
+ snapshot.statuses = fetchExecutionsFromMemory();
+ snapshot.skills = fetchSkillsFromMemory();
+
+ // notify registered widgets of update
+ emit updateAvailable(filterUpdate(snapshot));
+ }
+
const std::optional<ProviderID>
SkillManagerWrapper::findFirstProvider(SkillMap const& map, SkillID const& skillId)
{
@@ -221,6 +259,23 @@ namespace armarx::skills::gui
return std::nullopt;
}
+ SkillMap
+ SkillManagerWrapper::getSkills()
+ {
+ std::scoped_lock l(mutex_snapshot);
+
+ Snapshot filtered = filterUpdate(snapshot);
+
+ return filtered.skills;
+ }
+
+ StatusMap
+ SkillManagerWrapper::getExecutions()
+ {
+ std::scoped_lock l(mutex_snapshot);
+ return snapshot.statuses;
+ }
+
void
SkillManagerWrapper::stopExecution(skills::SkillExecutionID const& executionId,
const unsigned int max_retries)
@@ -295,8 +350,13 @@ namespace armarx::skills::gui
}
std::map<skills::SkillID, skills::SkillDescription> skillDescriptions;
+ if (this->UPDATE_ON_EXECUTION_REQUEST)
+ {
+ skillDescriptions = this->fetchSkillsFromMemory().at(providerId.value());
+ }
+ else
{
- skillDescriptions = this->fetchSkills().at(providerId.value());
+ skillDescriptions = this->getSkills().at(providerId.value());
}
if (skillDescriptions.find(skillId) == skillDescriptions.end())
diff --git a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.h b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.h
index 22eec04e20bfd499aa444eae74136be6d4c5490f..e191e1f963d82b29abeafe0535fe5f50a384bda3 100644
--- a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.h
+++ b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.h
@@ -52,23 +52,36 @@ namespace armarx::skills::gui
void stopExecution(skills::SkillExecutionID const& executionId,
const unsigned int max_retries = 0);
+ static const std::optional<skills::ProviderID> findFirstProvider(SkillMap const& map,
+ SkillID const& skillId);
+
/**
- * @brief Fetches and returns the latest skills update from memory.
- * @return The map representing all skills in memory. Empty, if error occurred.
+ * @brief Returns the latest skills snapshot.
+ * @return The map representing all currently known skills.
*/
- const SkillMap fetchSkills();
+ SkillMap getSkills();
/**
- * @brief Fetches and returns the latest status update from memory.
- * @return The map containing status updates for all execution ids. Empty, if error occurred.
+ * @brief Returns the latest status snapshot.
+ * @return The map containing status updates for all execution ids.
*/
- const StatusMap fetchExecutions();
+ StatusMap getExecutions();
- static const std::optional<skills::ProviderID> findFirstProvider(SkillMap const& map,
- SkillID const& skillId);
+ struct Snapshot
+ {
+ StatusMap statuses;
+ SkillMap skills;
+
+ // default constructor (construct empty)
+ Snapshot() : statuses({}), skills({})
+ {
+ }
+ };
signals:
void connectionUpdate(std::string const& message, std::string const& error);
+ void updateAvailable(Snapshot update);
+ void searchAccepted();
public slots:
/**
@@ -87,17 +100,44 @@ namespace armarx::skills::gui
*/
void stopAllExecutions();
+ /**
+ * @brief Requests this wrapper to overwrite its own state from memory.
+ */
+ void updateFromMemory();
+
private:
mutable std::mutex mutex_memory;
+ mutable std::mutex mutex_snapshot;
armarx::skills::manager::dti::SkillManagerInterfacePrx memory;
std::string currentSkillSearch = "";
+ Snapshot snapshot;
+
+ /**
+ * @brief Fetches and returns the latest skills update from memory.
+ * @return The map representing all skills in memory. Empty, if error occurred.
+ */
+ SkillMap fetchSkillsFromMemory();
+
+ /**
+ * @brief Fetches and returns the latest status update from memory.
+ * @return The map containing status updates for all execution ids. Empty, if error occurred.
+ */
+ StatusMap fetchExecutionsFromMemory();
+
/**
- * @brief Modifies the given map to match the search.
- * @param update The map to modify.
+ * @brief Returns the given snapshot, matching the search.
+ * @param update The snapshot to modify.
*/
- void filterSkillUpdate(std::map<skills::manager::dto::SkillID,
- skills::manager::dto::SkillDescription>& update);
+ Snapshot filterUpdate(Snapshot update);
+
+ // config
+ // TODO: perhaps use proper config file for the gui
+
+ // should we do an ice call to use the latest information when requesting an execution?
+ static constexpr bool UPDATE_ON_EXECUTION_REQUEST = true;
+
+ // ---
};
} // namespace armarx::skills::gui
diff --git a/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.cpp b/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.cpp
index 20295978a7ea67e75136277af2a1e617e5a9aae5..ac56b71debaed0b901378d931fc03b51cb78ea26 100644
--- a/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.cpp
@@ -7,6 +7,14 @@
namespace armarx::skills::gui
{
+ QIcon
+ getIcon(const std::string name)
+ {
+ QPixmap pix(QString::fromStdString(":icons/" + name));
+ QIcon icon(pix);
+ return icon;
+ }
+
void
ProfileMenuWidget::setupUi()
{
@@ -33,7 +41,9 @@ namespace armarx::skills::gui
// Text
setArgsFromClipboard->setText(QString::fromStdString(SET_ARGS_BUTTON_TEXT));
copyArgsToClipboard->setText(QString::fromStdString(COPY_ARGS_BUTTON_TEXT));
+ copyArgsToClipboard->setIcon(getIcon("edit-copy-4.svg"));
resetArgsToProfile->setText(QString::fromStdString(RESET_ARGS_BUTTON_TEXT));
+ resetArgsToProfile->setIcon(getIcon("refresh-black.svg"));
profileSelector->addItem(QString::fromStdString(DEFAULT_PROFILE_TEXT));
profileSelector->setDisabled(true);
profileSelector->setToolTip(QString::fromStdString(PROFILE_NOT_IMPLEMENTED));
diff --git a/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.h b/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.h
index 32dc1f278649060b97d58f680f2a8c74d5e1f75a..3046b18b8d0a60224395eea24347f14cc98e1e80 100644
--- a/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.h
+++ b/source/RobotAPI/libraries/skills_gui/skill_details/ProfileMenuWidget.h
@@ -14,8 +14,8 @@ namespace armarx::skills::gui
Q_OBJECT
public:
static const constexpr char* SET_ARGS_BUTTON_TEXT = "Set Args from Clipboard";
- static const constexpr char* COPY_ARGS_BUTTON_TEXT = "Copy Args to Clipboard";
- static const constexpr char* RESET_ARGS_BUTTON_TEXT = "Reset Args to Profile";
+ static const constexpr char* COPY_ARGS_BUTTON_TEXT = " Copy Args to Clipboard";
+ static const constexpr char* RESET_ARGS_BUTTON_TEXT = " Reset Args to Profile";
static const constexpr char* DEFAULT_PROFILE_TEXT = "<No Profile selected. Using root>";
static const constexpr char* PROFILE_NOT_IMPLEMENTED =
"Profiles other than the root profile are currently not supported.";
diff --git a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp
index b54d55c8a21b3ef8f1eb912e52473a9bcfbd230f..8eaa3633f7248dcb7c78b91c16874ccc7811a407 100644
--- a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp
+++ b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp
@@ -8,7 +8,7 @@ namespace armarx::skills::gui
if (_skillId.skillName == SkillID::UNKNOWN)
return;
- auto const& skills = memory->fetchSkills();
+ auto const& skills = memory->getSkills();
std::optional<skills::ProviderID> provider_opt;
// check skillId
@@ -113,6 +113,12 @@ namespace armarx::skills::gui
// Text
this->executeSkillButton->setText(
QString::fromStdString(SkillDetailGroupBox::EXECUTE_SKILL_BUTTON_TEXT));
+ {
+ QPixmap pix(QString::fromStdString(":icons/run.svg"));
+ QIcon icon(pix);
+ this->executeSkillButton->setIcon(icon);
+ }
+
connectSignals();
}
diff --git a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h
index 918fa2ec3241ae512645c878de16d53042fb0005..1723870bc6fb5a0c32c24efad41d17c9baf0fb58 100644
--- a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h
+++ b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h
@@ -19,7 +19,7 @@ namespace armarx::skills::gui
{
Q_OBJECT
public:
- static const constexpr char* EXECUTE_SKILL_BUTTON_TEXT = "Request Execution";
+ static const constexpr char* EXECUTE_SKILL_BUTTON_TEXT = " Request Execution";
static const constexpr char* GROUP_BOX_TITLE = "Skill Description";
SkillDetailGroupBox(std::shared_ptr<SkillManagerWrapper> _memory,
@@ -32,14 +32,9 @@ namespace armarx::skills::gui
signals:
void disconnectGui();
- /**
- * @brief Notify widgets to update themselves
- */
- void updateGui();
-
public slots:
/**
- * @brief Update subwidgets of an updated skill selection.
+ * @brief Notify subwidgets of an updated skill selection.
* @param skillId
*/
void updateSkillDetails(skills::SkillID& skillId);
diff --git a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsTreeWidget.cpp b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsTreeWidget.cpp
index 0287c5468f3a6b1f27e182e88bdbf589f4736af5..57b26011bc7e71920c1f08ade4c0c6434e850877 100644
--- a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsTreeWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsTreeWidget.cpp
@@ -91,7 +91,7 @@ namespace armarx::skills::gui
ARMARX_CHECK(sid.isFullySpecified());
// maybe the search is empty?
- auto skillsMap = memory->fetchSkills();
+ auto skillsMap = memory->getSkills();
if (skillsMap.count(sid.providerId.value()) == 0 ||
skillsMap.at(sid.providerId.value()).count(sid) == 0)
{
@@ -99,7 +99,7 @@ namespace armarx::skills::gui
return;
}
- auto descr = memory->fetchSkills().at(sid.providerId.value()).at(sid);
+ auto descr = memory->getSkills().at(sid.providerId.value()).at(sid);
this->updateContents(sid, descr);
}
@@ -196,7 +196,7 @@ namespace armarx::skills::gui
return;
}
skills::SkillID& skillId = shownSkill.value().skillId;
- auto& skills = memory->fetchSkills();
+ const auto skills = memory->getSkills();
ARMARX_CHECK(skillId.isProviderSpecified());
// find description
diff --git a/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp b/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp
index 91fb11c021677e96890a001a0538fc6cf98f4aa0..30b26364853dd4d96fb159847edb575b372ae833 100644
--- a/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp
+++ b/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp
@@ -12,12 +12,31 @@ namespace armarx::skills::gui
emit searchRequest(search);
}
+ void
+ SkillGroupBox::filterAndFetch()
+ {
+ memory->updateFromMemory();
+ handleSearch();
+ }
+
void
SkillGroupBox::connectGui(std::string observerName)
{
setTitle(QString::fromStdString(observerName));
}
+ void
+ SkillGroupBox::updateGui(SkillManagerWrapper::Snapshot update)
+ {
+ skillTreeWidget->updateGui(update.skills);
+ }
+
+ void
+ SkillGroupBox::searchCallback()
+ {
+ this->skillTreeWidget->updateGui(memory->getSkills());
+ }
+
void
SkillGroupBox::setupUi()
{
@@ -43,7 +62,12 @@ namespace armarx::skills::gui
// text
this->searchBar->setPlaceholderText(QString::fromStdString("Search ..."));
- this->acceptSearchButton->setText(QString::fromStdString("Search"));
+ this->acceptSearchButton->setText(QString::fromStdString("Update and Search"));
+
+ // some further information to explain the search bar
+ this->searchBar->setToolTip(
+ QString::fromStdString("Accepting the search (with Enter) searches all currently known "
+ "skills. This is not the same as 'Update & Search'!"));
connectSignals();
}
@@ -52,12 +76,16 @@ namespace armarx::skills::gui
SkillGroupBox::connectSignals()
{
connect(
- this->acceptSearchButton, &QPushButton::clicked, this, &SkillGroupBox::handleSearch);
+ this->acceptSearchButton, &QPushButton::clicked, this, &SkillGroupBox::filterAndFetch);
connect(this->searchBar, &QLineEdit::editingFinished, this, &SkillGroupBox::handleSearch);
connect(this,
&SkillGroupBox::searchRequest,
this->memory.get(),
&SkillManagerWrapper::acceptSearchRequest);
+ connect(memory.get(),
+ &SkillManagerWrapper::searchAccepted,
+ this,
+ &SkillGroupBox::searchCallback);
connect(skillTreeWidget,
&SkillTreeWidget::updateSkillDetails,
@@ -67,8 +95,5 @@ namespace armarx::skills::gui
// disconnect
connect(
this, &SkillGroupBox::disconnectGui, skillTreeWidget, &SkillTreeWidget::disconnectGui);
-
- // update cascade
- connect(this, &SkillGroupBox::updateGui, skillTreeWidget, &SkillTreeWidget::updateSkills);
}
} // namespace armarx::skills::gui
diff --git a/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.h b/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.h
index 2ac6044ba72807ef300ae48fa19fe53d9230d1f3..7089f01b0e0103592b6eda7ff4f8d5d3571ca915 100644
--- a/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.h
+++ b/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.h
@@ -36,20 +36,25 @@ namespace armarx::skills::gui
void disconnectGui();
- /**
- * @brief Notify widgets to update themselves
- */
- void updateGui();
-
public slots:
void connectGui(std::string observerName);
+ void updateGui(SkillManagerWrapper::Snapshot update);
+
+ // is called, after the search has been accepted in the memory wrapper.
+ void searchCallback();
+
private slots:
/**
* Takes string from line edit and emits the search request.
*/
void handleSearch();
+ /**
+ * @brief Applies the update, and requests an update from memory.
+ */
+ void filterAndFetch();
+
private:
void setupUi();
void connectSignals();
diff --git a/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.cpp b/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.cpp
index c1b100130e4eef7e0e49e287d163e6f880f4f523..a9ebcdd98cfbde9b54abcb929504dd384e0a7c5f 100644
--- a/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.cpp
@@ -25,10 +25,9 @@ namespace armarx::skills::gui
}
void
- SkillTreeWidget::updateSkills()
+ SkillTreeWidget::updateGui(SkillMap update)
{
setSortingEnabled(false);
- const auto skills = memory->fetchSkills();
// update tree view. Remove non-existing elements
int i = 0;
@@ -38,7 +37,7 @@ namespace armarx::skills::gui
auto providerName = providerItem->text(0).toStdString();
skills::ProviderID providerId{.providerName = providerName};
- if (skills.find(providerId) == skills.end())
+ if (update.find(providerId) == update.end())
{
providerItem = nullptr; // reset
auto remove = this->takeTopLevelItem(i);
@@ -49,8 +48,8 @@ namespace armarx::skills::gui
++i;
// sanity check
- ARMARX_CHECK(skills.count(providerId) > 0);
- auto& providedSkills = skills.at(providerId);
+ ARMARX_CHECK(update.count(providerId) > 0);
+ auto& providedSkills = update.at(providerId);
int j = 0;
while (j < providerItem->childCount())
@@ -75,7 +74,7 @@ namespace armarx::skills::gui
}
// update tree view. Add new elements
- for (const auto& [providerId, providedSkills] : skills)
+ for (const auto& [providerId, providedSkills] : update)
{
bool newProvider = false;
QTreeWidgetItem* providerItem = nullptr;
diff --git a/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.h b/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.h
index 61ec5cc85314d0318a951e8216a620d1f1d9a7c8..f94bdcaae2d62dd45dc10f8a98f6916117a0eb4d 100644
--- a/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.h
+++ b/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidget.h
@@ -9,6 +9,8 @@
namespace armarx::skills::gui
{
+ using SkillMap =
+ std::map<skills::ProviderID, std::map<skills::SkillID, skills::SkillDescription>>;
class SkillTreeWidget : public QTreeWidget, public MemoryCommunicatorBase
{
@@ -42,7 +44,8 @@ namespace armarx::skills::gui
public slots:
void disconnectGui();
- void updateSkills();
+
+ void updateGui(SkillMap update);
private slots:
void skillSelectionChanged(QTreeWidgetItem* current, int column);