From 12232e3888f5393a9704715ff80aa204e49f3ee2 Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Thu, 12 Oct 2023 10:04:34 +0200
Subject: [PATCH] unify readers and writers
---
.../Component.cpp | 45 +++++-----
.../Impl.cpp | 16 +---
.../RobotStatePredictionClientExample/Impl.h | 20 ++---
.../SkillProviderExample.cpp | 4 +-
.../SkillManagerMonitorWidgetController.cpp | 4 +-
.../RobotAPI/libraries/armem/client/Reader.h | 32 ++++---
.../RobotAPI/libraries/armem/client/Writer.h | 28 +++---
.../armem/client/plugins/ReaderWriterPlugin.h | 14 ++-
.../armem_locations/client/Reader.cpp | 23 ++---
.../libraries/armem_locations/client/Reader.h | 5 +-
.../client/articulated_object/Reader.cpp | 56 ++++++------
.../client/articulated_object/Reader.h | 12 +--
.../client/articulated_object/Writer.cpp | 86 ++++++++++---------
.../client/articulated_object/Writer.h | 34 ++++----
.../client/instance/ObjectReader.cpp | 13 +--
.../client/instance/ObjectReader.h | 8 +-
.../client/common/RobotReader.cpp | 13 +--
.../client/common/RobotReader.h | 17 ++--
.../client/common/RobotWriter.cpp | 12 ++-
.../client/common/RobotWriter.h | 11 +--
.../client/common/VirtualRobotReader.cpp | 64 +++++++-------
.../client/common/VirtualRobotReader.h | 4 +-
.../client/common/VirtualRobotWriter.cpp | 26 +++---
.../client/common/VirtualRobotWriter.h | 5 +-
.../client/localization/TransformReader.cpp | 68 +++++++--------
.../client/localization/TransformReader.h | 17 ++--
.../client/localization/TransformWriter.cpp | 47 +++++-----
.../client/localization/TransformWriter.h | 15 ++--
.../client/localization/interfaces.h | 5 +-
29 files changed, 333 insertions(+), 371 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
index 9f13e111e..c1a2f394a 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
@@ -21,33 +21,31 @@
*/
-#include "RobotStatePredictionClientExample.h"
-#include "Impl.h"
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "Impl.h"
+#include "RobotStatePredictionClientExample.h"
namespace armarx::robot_state_prediction_client_example
{
- Component::Component() :
- pimpl(std::make_unique<Impl>(memoryNameSystem()))
+ Component::Component() : pimpl(std::make_unique<Impl>())
{
}
-
RobotStatePredictionClientExample::~RobotStatePredictionClientExample() = default;
-
- std::string Component::getDefaultName() const
+ std::string
+ Component::getDefaultName() const
{
return "RobotStatePredictionClientExample";
}
-
- armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
ARMARX_CHECK_NOT_NULL(pimpl);
pimpl->defineProperties(defs, "p.");
@@ -55,13 +53,13 @@ namespace armarx::robot_state_prediction_client_example
return defs;
}
-
- void Component::onInitComponent()
+ void
+ Component::onInitComponent()
{
}
-
- void Component::onConnectComponent()
+ void
+ Component::onConnectComponent()
{
pimpl->connect(memoryNameSystem(), arviz);
pimpl->start();
@@ -70,19 +68,19 @@ namespace armarx::robot_state_prediction_client_example
RemoteGui_startRunningTask();
}
-
- void Component::onDisconnectComponent()
+ void
+ Component::onDisconnectComponent()
{
pimpl->stop();
}
-
- void Component::onExitComponent()
+ void
+ Component::onExitComponent()
{
}
-
- void Component::createRemoteGuiTab()
+ void
+ Component::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -90,8 +88,9 @@ namespace armarx::robot_state_prediction_client_example
RemoteGui_createTab(getName(), root, &tab);
}
- void Component::RemoteGui_update()
+ void
+ Component::RemoteGui_update()
{
}
-}
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
index c43366963..a303f1c39 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
@@ -37,7 +37,6 @@
#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
-
namespace simox::alg
{
template <class... Args>
@@ -49,7 +48,6 @@ namespace simox::alg
return conc;
}
-
template <class KeyT, class ValueT>
std::map<KeyT, ValueT>
map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
@@ -64,7 +62,6 @@ namespace simox::alg
return map;
}
-
template <class KeyT, class ValueT>
std::vector<ValueT>
multi_at(const std::map<KeyT, ValueT>& map,
@@ -111,15 +108,13 @@ namespace simox::alg
namespace armarx::robot_state_prediction_client_example
{
- Impl::Impl(armem::client::MemoryNameSystem& memoryNameSystem)
+ Impl::Impl()
{
- client.remote.robotReader.emplace(memoryNameSystem);
+ client.remote.robotReader.emplace();
}
-
Impl::~Impl() = default;
-
void
Impl::defineProperties(IceUtil::Handle<PropertyDefinitionContainer>& defs,
const std::string& prefix)
@@ -133,7 +128,6 @@ namespace armarx::robot_state_prediction_client_example
client.remote.robotReader->registerPropertyDefinitions(defs);
}
-
void
Impl::connect(armem::client::MemoryNameSystem& mns, viz::Client arviz)
{
@@ -148,12 +142,11 @@ namespace armarx::robot_state_prediction_client_example
ARMARX_WARNING << e.what();
}
- client.remote.robotReader->connect();
+ client.remote.robotReader->connect(mns);
this->remote.arviz = arviz;
}
-
void
Impl::start()
{
@@ -161,14 +154,12 @@ namespace armarx::robot_state_prediction_client_example
task->start();
}
-
void
Impl::stop()
{
task->stop();
}
-
void
Impl::run()
{
@@ -182,7 +173,6 @@ namespace armarx::robot_state_prediction_client_example
}
}
-
void
Impl::runOnce()
{
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
index 272f9e2e6..d716f5d94 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
@@ -27,27 +27,25 @@
#include <ArmarXCore/util/tasks.h>
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/client/forward_declarations.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-
#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include "RobotStatePredictionClient.h"
-
namespace armarx::robot_state_prediction_client_example
{
class Impl
{
public:
-
- Impl(armem::client::MemoryNameSystem& memoryNameSystem);
+ Impl();
~Impl();
- void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs, const std::string& prefix);
+ void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs,
+ const std::string& prefix);
void connect(armem::client::MemoryNameSystem& mns, viz::Client arviz);
void start();
@@ -58,7 +56,6 @@ namespace armarx::robot_state_prediction_client_example
public:
-
struct Properties
{
float updateFrequencyHz = 10;
@@ -66,12 +63,14 @@ namespace armarx::robot_state_prediction_client_example
std::string robotName = "Armar6";
float predictAheadSeconds = 1.0;
};
+
Properties properties;
struct Remote
{
viz::Client arviz;
};
+
Remote remote;
armarx::SimpleRunningTask<>::pointer_type task;
@@ -82,6 +81,5 @@ namespace armarx::robot_state_prediction_client_example
std::optional<std::vector<armem::MemoryID>> localizationEntityIDs;
std::optional<std::vector<armem::MemoryID>> propioceptionEntityIDs;
std::optional<viz::Robot> robotViz;
-
};
-}
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 2fbc92142..a183a100c 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -59,7 +59,7 @@ namespace armarx::skills::provider
d.description,
{},
d.timeout + armarx::core::time::Duration::Seconds(2),
- d.acceptedType};
+ d.parametersType};
}
Skill::PrepareResult
@@ -73,7 +73,7 @@ namespace armarx::skills::provider
{
auto d = HelloWorldSkill::GetSkillDescription();
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
- this->setParameters(d.rootProfileParameterization);
+ this->setParameters(d.rootProfileDefaults);
});
foo.detach();
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 846c34ae5..b7880b87f 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -564,8 +564,8 @@ namespace armarx
skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
{QString::fromStdString("Arguments")});
- auto aron_args = skillDesc.acceptedType;
- auto default_args_of_profile = skillDesc.rootProfileParameterization;
+ auto aron_args = skillDesc.parametersType;
+ auto default_args_of_profile = skillDesc.rootProfileDefaults;
aronTreeWidgetController =
std::make_shared<AronTreeWidgetController>(widget.treeWidgetSkillDetails,
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 875250eb8..d476b25ef 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -2,9 +2,9 @@
// STD/STL
+#include <mutex>
#include <optional>
#include <vector>
-#include <mutex>
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
@@ -15,7 +15,6 @@
#include "Query.h"
-
namespace armarx::armem::client
{
@@ -26,11 +25,11 @@ namespace armarx::armem::client
{
public:
-
/**
* @brief Construct a memory reader.
* @param memory The memory proxy.
*/
+ Reader(const Reader&) = default;
Reader(server::ReadingMemoryInterfacePrx readingMemory = nullptr,
server::PredictingMemoryInterfacePrx predictingMemory = nullptr);
@@ -41,8 +40,10 @@ namespace armarx::armem::client
QueryResult query(const QueryInput& input) const;
armem::query::data::Result query(const armem::query::data::Input& input) const;
- QueryResult query(armem::query::data::MemoryQueryPtr query, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
- QueryResult query(const armem::query::data::MemoryQuerySeq& queries, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult query(armem::query::data::MemoryQueryPtr query,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult query(const armem::query::data::MemoryQuerySeq& queries,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
QueryResult query(const QueryBuilder& queryBuilder) const;
@@ -114,7 +115,8 @@ namespace armarx::armem::client
* @return The query result.
*/
QueryResult
- queryMemoryIDs(const std::vector<MemoryID>& ids, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ queryMemoryIDs(const std::vector<MemoryID>& ids,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -132,8 +134,9 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The query result.
*/
- QueryResult
- getLatestSnapshotsIn(const MemoryID& id, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult getLatestSnapshotsIn(
+ const MemoryID& id,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
* @brief Get the latest snapshot under the given memory ID.
@@ -141,8 +144,9 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The latest contained snapshot, if any.
*/
- std::optional<wm::EntitySnapshot>
- getLatestSnapshotIn(const MemoryID& id, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ std::optional<wm::EntitySnapshot> getLatestSnapshotIn(
+ const MemoryID& id,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -150,8 +154,8 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The query result.
*/
- QueryResult
- getAllLatestSnapshots(armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult getAllLatestSnapshots(
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -163,7 +167,8 @@ namespace armarx::armem::client
getAll(armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
- server::dto::DirectlyStoreResult directlyStore(const server::dto::DirectlyStoreInput& input) const;
+ server::dto::DirectlyStoreResult
+ directlyStore(const server::dto::DirectlyStoreInput& input) const;
void startRecording() const;
void stopRecording() const;
@@ -201,7 +206,6 @@ namespace armarx::armem::client
int recursionDepth);
public:
-
server::ReadingMemoryInterfacePrx readingPrx;
server::PredictingMemoryInterfacePrx predictionPrx;
};
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 6c4b223ce..61eeef93b 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -1,10 +1,8 @@
#pragma once
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
-
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-
namespace armarx::armem::client
{
@@ -24,17 +22,21 @@ namespace armarx::armem::client
class Writer
{
public:
-
/**
* @brief Construct a memory writer.
* @param memory The memory proxy.
*/
+ Writer(const Writer&) = default;
Writer(server::WritingMemoryInterfacePrx memory = nullptr);
- data::AddSegmentResult addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName, bool clearWhenExists = false);
- data::AddSegmentResult addSegment(const MemoryID& providerSegmentID, bool clearWhenExists = false);
- data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names, bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const MemoryID& providerSegmentID,
+ bool clearWhenExists = false);
+ 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);
@@ -46,10 +48,9 @@ namespace armarx::armem::client
/// Commit a single entity update.
EntityUpdateResult commit(const EntityUpdate& update);
/// Commit a single entity update.
- EntityUpdateResult commit(
- const MemoryID& entityID,
- const std::vector<aron::data::DictPtr>& instancesData,
- Time referencedTime);
+ EntityUpdateResult commit(const MemoryID& entityID,
+ const std::vector<aron::data::DictPtr>& instancesData,
+ Time referencedTime);
// with bare-ice types
data::CommitResult commit(const data::Commit& commit);
@@ -63,18 +64,15 @@ namespace armarx::armem::client
}
private:
-
/// Sets `timeSent` on all entity updates and performs the commit,
data::CommitResult _commit(data::Commit& commit);
public:
-
server::WritingMemoryInterfacePrx memory;
-
};
-}
+} // namespace armarx::armem::client
namespace armarx::armem::data
{
@@ -82,4 +80,4 @@ namespace armarx::armem::data
std::ostream& operator<<(std::ostream& os, const AddSegmentsInput& rhs);
std::ostream& operator<<(std::ostream& os, const AddSegmentResult& rhs);
std::ostream& operator<<(std::ostream& os, const AddSegmentsResult& rhs);
-}
+} // namespace armarx::armem::data
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
index 168686040..868d3ef30 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
@@ -23,13 +23,12 @@
#include <string>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
namespace armarx::armem::client::plugins
{
@@ -45,8 +44,8 @@ namespace armarx::armem::client::plugins
class ReaderWriterPlugin : public armarx::ComponentPlugin
{
public:
- ReaderWriterPlugin(ManagedIceObject& parent, const std::string pre) :
- ComponentPlugin(parent, pre), readerWriter(memoryNameSystem())
+ ReaderWriterPlugin(ManagedIceObject& parent, const std::string& pre) :
+ ComponentPlugin(parent, pre)
{
if (not armemPlugin)
{
@@ -74,7 +73,7 @@ namespace armarx::armem::client::plugins
return;
}
- readerWriter.connect();
+ readerWriter.connect(memoryNameSystem());
}
bool
@@ -114,9 +113,8 @@ namespace armarx::armem::client::plugins
}
armarx::armem::client::plugins::Plugin* armemPlugin = nullptr;
-
- T readerWriter;
+ T readerWriter;
};
} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
index 0fd0c6a1b..13cc8ff5a 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -10,13 +10,6 @@
namespace armarx::armem::locations::client
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem),
- objReader(memoryNameSystem),
- robotReader(memoryNameSystem)
- {
- }
-
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -28,7 +21,7 @@ namespace armarx::armem::locations::client
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
@@ -45,8 +38,8 @@ namespace armarx::armem::locations::client
return;
}
- objReader.connect();
- robotReader.connect();
+ objReader.connect(memoryNameSystem);
+ robotReader.connect(memoryNameSystem);
}
std::map<std::string, armarx::navigation::location::arondto::Location>
@@ -63,11 +56,11 @@ namespace armarx::armem::locations::client
{
auto i = e.findLatestInstance();
if (i)
- {
- auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
- ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
+ {
+ auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
+ ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
}
- });
+ });
return ret;
}
@@ -83,7 +76,7 @@ namespace armarx::armem::locations::client
for (auto& [locName, location] : locations)
{
- (void) locName;
+ (void)locName;
if (location.framedPose.header.frame == armarx::GlobalFrame)
{
location.framedPose.header.agent = ""; //sanity set
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.h b/source/RobotAPI/libraries/armem_locations/client/Reader.h
index ad895c269..7a25269d1 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.h
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.h
@@ -23,11 +23,11 @@ namespace armarx::armem::locations::client
std::string memoryName = "Navigation";
};
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
Properties
getProperties()
@@ -44,7 +44,6 @@ namespace armarx::armem::locations::client
const std::string propertyPrefix = "mem.nav.location";
- armem::client::MemoryNameSystem& memoryNameSystem;
armarx::armem::obj::instance::Reader objReader;
armarx::armem::robot_state::RobotReader robotReader;
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 ac01fb3b8..4c9ec304a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -5,19 +5,20 @@
#include <Eigen/Geometry>
+#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include "RobotAPI/libraries/armem/core/Commit.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#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_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>
@@ -30,20 +31,17 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName << "' ...";
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName
+ << "' ...";
try
{
memoryReader = memoryNameSystem.useReader(objects::constants::MemoryName);
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName << "'";
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -96,7 +94,9 @@ namespace armarx::armem::articulated_object
}
std::optional<ArticulatedObject>
- Reader::get(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::get(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
const auto splits = simox::alg::split(name, "/");
ARMARX_CHECK_EQUAL(splits.size(), 3) << "`name` must be of form `DATASET/NAME/INSTANCE`";
@@ -118,7 +118,8 @@ namespace armarx::armem::articulated_object
ArticulatedObject
Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp,
- const std::string& instanceName, const std::optional<std::string>& providerName)
+ const std::string& instanceName,
+ const std::optional<std::string>& providerName)
{
ArticulatedObject obj{.description = description,
.instance = instanceName,
@@ -131,7 +132,9 @@ namespace armarx::armem::articulated_object
}
bool
- Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::synchronize(ArticulatedObject& obj,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
ARMARX_CHECK_NOT_EMPTY(obj.instance) << "An instance name must be provided!";
@@ -148,7 +151,8 @@ namespace armarx::armem::articulated_object
}
std::vector<robot::RobotDescription>
- Reader::queryDescriptions(const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryDescriptions(const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -174,12 +178,14 @@ namespace armarx::armem::articulated_object
}
std::optional<robot::RobotDescription>
- Reader::queryDescription(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryDescription(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// Query all entities from provider.
armem::client::query::Builder qb;
- if(providerName.has_value()) // query single provider
+ if (providerName.has_value()) // query single provider
{
// clang-format off
qb
@@ -189,7 +195,7 @@ namespace armarx::armem::articulated_object
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
}
- else // query all providers
+ else // query all providers
{
// clang-format off
qb
@@ -199,7 +205,7 @@ namespace armarx::armem::articulated_object
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
}
-
+
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -215,7 +221,9 @@ namespace armarx::armem::articulated_object
}
std::optional<robot::RobotState>
- Reader::queryState(const std::string& instanceName, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryState(const std::string& instanceName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -242,7 +250,6 @@ namespace armarx::armem::articulated_object
return getArticulatedObjectState(qResult.memory);
}
-
std::optional<robot::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
@@ -262,7 +269,8 @@ namespace armarx::armem::articulated_object
robot::RobotState robotState{.timestamp = objectPose.timestamp,
.globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues, .proprioception=std::nullopt};
+ .jointMap = objectPose.objectJointValues,
+ .proprioception = std::nullopt};
return robotState;
}
@@ -288,7 +296,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
@@ -308,7 +315,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::vector<robot::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
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 fc8f1da38..0eb5bd104 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -39,11 +39,15 @@ namespace armarx::armem::articulated_object
class Reader : virtual public ReaderInterface
{
public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def){}
- void connect();
+ void
+ registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ }
+
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool synchronize(ArticulatedObject& obj,
const armem::Time& timestamp,
@@ -88,8 +92,6 @@ namespace armarx::armem::articulated_object
const std::string propertyPrefix = "mem.obj.articulated.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
};
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 20c3458aa..b87554869 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -22,15 +22,10 @@
#include "utils.h"
-
namespace armarx::armem::articulated_object
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
- void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -46,10 +41,12 @@ namespace armarx::armem::articulated_object
"Name of the memory core segment to use for object classes.");
ARMARX_IMPORTANT << "Writer: add property '" << prefix << "ProviderName'";
- def->required(properties.providerName, prefix + "write.ProviderName", "Name of this provider");
+ def->required(
+ properties.providerName, prefix + "write.ProviderName", "Name of this provider");
}
- void Writer::connect()
+ void
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
@@ -80,7 +77,8 @@ namespace armarx::armem::articulated_object
memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects);
}
- void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
+ void
+ Writer::updateKnownObject(const armem::MemoryID& snapshotId)
{
arondto::RobotDescription aronArticulatedObjectDescription;
// aronArticulatedObjectDescription.fromAron(snapshotId.ent);
@@ -88,21 +86,24 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
- const std::vector<armem::MemoryID>& snapshotIDs)
+ void
+ Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
updateKnownObjects();
}
- void Writer::updateKnownObjects()
+ void
+ Writer::updateKnownObjects()
{
knownObjects = queryDescriptions(Time::Now());
ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
}
- std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
+ std::optional<armem::MemoryID>
+ Writer::storeOrGetClass(const ArticulatedObject& obj)
{
ARMARX_TRACE;
@@ -125,7 +126,8 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
+ std::optional<armem::MemoryID>
+ Writer::storeClass(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
@@ -154,7 +156,7 @@ namespace armarx::armem::articulated_object
toAron(aronArticulatedObjectDescription, obj.description);
update.instancesData = {aronArticulatedObjectDescription.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -173,17 +175,20 @@ namespace armarx::armem::articulated_object
return updateResult.snapshotID;
}
- std::string Writer::getProviderName() const
+ std::string
+ Writer::getProviderName() const
{
return properties.providerName;
}
- void Writer::setProviderName(const std::string& providerName)
+ void
+ Writer::setProviderName(const std::string& providerName)
{
this->properties.providerName = providerName;
}
- bool Writer::storeInstance(const ArticulatedObject& obj)
+ bool
+ Writer::storeInstance(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
@@ -192,12 +197,13 @@ namespace armarx::armem::articulated_object
ARMARX_CHECK(not obj.instance.empty()) << "An object instance name must be provided!";
const std::string entityName = obj.description.name + "/" + obj.instance;
- ARMARX_DEBUG << "Storing articulated object instance '" << entityName << "' (provider '" << properties.providerName << "')";
+ ARMARX_DEBUG << "Storing articulated object instance '" << entityName << "' (provider '"
+ << properties.providerName << "')";
const auto providerId = armem::MemoryID()
- .withMemoryName(properties.memoryName)
- .withCoreSegmentName(properties.coreInstanceSegmentName)
- .withProviderSegmentName(properties.providerName);
+ .withMemoryName(properties.memoryName)
+ .withCoreSegmentName(properties.coreInstanceSegmentName)
+ .withProviderSegmentName(properties.providerName);
armem::EntityUpdate update;
update.entityID = providerId.withEntityName(entityName);
@@ -235,7 +241,7 @@ namespace armarx::armem::articulated_object
objectInstance.pose.attachmentValid = false;
update.instancesData = {objectInstance.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -250,7 +256,8 @@ namespace armarx::armem::articulated_object
return updateResult.success;
}
- bool Writer::store(const ArticulatedObject& obj)
+ bool
+ Writer::store(const ArticulatedObject& obj)
{
const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
@@ -292,22 +299,23 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(properties.coreClassSegmentName);
std::unordered_map<std::string, armem::MemoryID> descriptions;
- coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
- {
- if (entity.empty())
+ coreSegment.forEachEntity(
+ [&descriptions](const wm::Entity& entity)
{
- ARMARX_WARNING << "No entity found";
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return true;
+ }
+
+ const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
+ if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
+ {
+ const armem::MemoryID snapshotID(sn.id());
+ descriptions.insert({robotDescription->name, snapshotID});
+ }
return true;
- }
-
- const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
- if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
- {
- const armem::MemoryID snapshotID(sn.id());
- descriptions.insert({robotDescription->name, snapshotID});
- }
- return true;
- });
+ });
return descriptions;
}
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 3e827d66a..6245ef374 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -32,19 +32,17 @@
#include "interfaces.h"
-
namespace armarx::armem::articulated_object
{
- class Writer:
- virtual public WriterInterface
+ class Writer : virtual public WriterInterface
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystemopti);
+ Writer() = default;
virtual ~Writer() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool store(const ArticulatedObject& obj) override;
@@ -59,33 +57,33 @@ namespace armarx::armem::articulated_object
private:
-
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
- void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs);
void updateKnownObjects();
void updateKnownObject(const armem::MemoryID& snapshotId);
// TODO duplicate
- std::unordered_map<std::string, armem::MemoryID> queryDescriptions(const armem::Time& timestamp);
- std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
- std::unordered_map<std::string, armem::MemoryID> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
-
+ std::unordered_map<std::string, armem::MemoryID>
+ queryDescriptions(const armem::Time& timestamp);
+ std::optional<robot::RobotDescription>
+ getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::unordered_map<std::string, armem::MemoryID>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
struct Properties
{
- std::string memoryName = "Object";
+ std::string memoryName = "Object";
std::string coreInstanceSegmentName = "Instance";
- std::string coreClassSegmentName = "Class";
- std::string providerName = "";
+ std::string coreClassSegmentName = "Class";
+ std::string providerName = "";
- bool allowClassCreation = false;
+ bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
@@ -97,4 +95,4 @@ namespace armarx::armem::articulated_object
};
-} // namespace armarx::armem::articulated_object
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index b2b740b73..399fce56d 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -21,11 +21,6 @@
namespace armarx::armem::obj::instance
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -35,7 +30,7 @@ namespace armarx::armem::obj::instance
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
@@ -48,8 +43,8 @@ namespace armarx::armem::obj::instance
this->objPoseStorage =
objpose::ObjectPoseStorageInterfacePrx::checkedCast(r.readingPrx);
- ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '"
- << p.memoryName << "'";
+ ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '" << p.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -93,8 +88,6 @@ namespace armarx::armem::obj::instance
return requestResult.results.at(requestObject).result.success;
}
return false;
-
-
}
std::optional<objpose::ObjectPose>
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 81a0fc030..18f52a611 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -45,11 +45,11 @@ namespace armarx::armem::obj::instance
std::string memoryName = "Object";
};
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
// localization stuff. Requires an instance index to be set.
std::map<std::string, bool> requestLocalization(const ObjectID& instanceId,
@@ -74,7 +74,8 @@ namespace armarx::armem::obj::instance
return this->p;
}
- objpose::ObjectPoseStorageInterfacePrx getObjectPoseStorage() const
+ objpose::ObjectPoseStorageInterfacePrx
+ getObjectPoseStorage() const
{
return objPoseStorage;
}
@@ -84,7 +85,6 @@ namespace armarx::armem::obj::instance
const std::string propertyPrefix = "mem.obj.instance.";
- armem::client::MemoryNameSystem& memoryNameSystem;
objpose::ObjectPoseStorageInterfacePrx objPoseStorage;
};
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 5fb5004c9..8d9440026 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -31,10 +31,11 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
-
-
- RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
+ RobotReader::RobotReader(const RobotReader& r) :
+ properties(r.properties),
+ propertyPrefix(r.propertyPrefix),
+ memoryReader(r.memoryReader),
+ transformReader(r.transformReader)
{
}
@@ -45,9 +46,9 @@ namespace armarx::armem::robot_state
}
void
- RobotReader::connect()
+ RobotReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
- transformReader.connect();
+ transformReader.connect(memoryNameSystem);
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << constants::memoryName << "' ...";
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 6f90a9b24..dbf81e69a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-
namespace armarx::armem::robot_state
{
/**
@@ -44,14 +43,16 @@ namespace armarx::armem::robot_state
class RobotReader : virtual public robot::ReaderInterface
{
public:
- RobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ RobotReader() = default;
+ RobotReader(const RobotReader&);
virtual ~RobotReader() = default;
- virtual void connect();
+ virtual void connect(armem::client::MemoryNameSystem& memoryNameSystem);
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) const override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj,
+ const armem::Time& timestamp) const override;
std::optional<robot::Robot> get(const std::string& name,
const armem::Time& timestamp) const override;
@@ -68,7 +69,7 @@ namespace armarx::armem::robot_state
std::optional<armarx::armem::arondto::Proprioception>
queryProprioception(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
@@ -135,7 +136,8 @@ namespace armarx::armem::robot_state
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
std::optional<armarx::armem::arondto::Proprioception>
- getRobotProprioception(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ getRobotProprioception(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
@@ -160,9 +162,8 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
client::robot_state::localization::TransformReader transformReader;
};
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 f971c88c3..e92e13d64 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -34,21 +34,20 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotWriter::~RobotWriter() = default;
-
-
- RobotWriter::RobotWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ RobotWriter::RobotWriter(const RobotWriter& r) :
+ properties(r.properties), propertyPrefix(r.propertyPrefix), memoryWriter(r.memoryWriter)
{
}
+ RobotWriter::~RobotWriter() = default;
+
void
RobotWriter::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
}
void
- RobotWriter::connect()
+ RobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "RobotWriter: Waiting for memory '" << constants::memoryName << "' ...";
@@ -65,7 +64,6 @@ namespace armarx::armem::robot_state
}
}
-
bool
RobotWriter::storeDescription(const robot::RobotDescription& description,
const armem::Time& timestamp)
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 f1e20614e..d8ca351d6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-
namespace armarx::armem::robot_state
{
/**
@@ -44,10 +43,11 @@ namespace armarx::armem::robot_state
class RobotWriter : virtual public robot::WriterInterface
{
public:
- RobotWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ RobotWriter() = default;
+ RobotWriter(const RobotWriter&);
~RobotWriter() override;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
@@ -72,7 +72,6 @@ namespace armarx::armem::robot_state
const std::string& robotName,
const armem::Time& timestamp);
-
struct Properties
{
@@ -80,9 +79,7 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
armem::client::Writer memoryWriter;
};
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 0e972f791..0effa5c23 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -15,34 +15,28 @@
namespace armarx::armem::robot_state
{
-
- VirtualRobotReader::VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- RobotReader(memoryNameSystem)
- {
- }
-
- void VirtualRobotReader::connect()
- {
- RobotReader::connect();
- }
-
// TODO(fabian.reister): register property defs
- void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
RobotReader::registerPropertyDefinitions(def);
}
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp) const
+ 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 robot::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{"", ""}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `" << robot.getName() << "` "
+ ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
+ << robot.getName() << "` "
<< "(type `" << robot.getType() << "`)!";
return false;
}
@@ -53,15 +47,19 @@ namespace armarx::armem::robot_state
return true;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name, const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
- ARMARX_VERBOSE << deactivateSpam(60) << "Querying robot description for robot '" << name << "'";
+ ARMARX_VERBOSE << deactivateSpam(60) << "Querying robot description for robot '" << name
+ << "'";
const auto description = queryDescription(name, timestamp);
if (not description)
{
- ARMARX_VERBOSE << deactivateSpam(5) << "The description of robot `" << name << "` is not a available!";
+ ARMARX_VERBOSE << deactivateSpam(5) << "The description of robot `" << name
+ << "` is not a available!";
return nullptr;
}
@@ -69,8 +67,8 @@ namespace armarx::armem::robot_state
const std::string xmlFilename = description->xml.toSystemPath();
ARMARX_CHECK(std::filesystem::exists(xmlFilename)) << xmlFilename;
- ARMARX_VERBOSE << deactivateSpam(5) << "Loading (virtual) robot '" << description->name << "' from XML file '"
- << xmlFilename << "'";
+ ARMARX_VERBOSE << deactivateSpam(5) << "Loading (virtual) robot '" << description->name
+ << "' from XML file '" << xmlFilename << "'";
auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
ARMARX_CHECK_NOT_NULL(robot) << "Could not load robot from file `" << xmlFilename << "`";
@@ -80,24 +78,30 @@ namespace armarx::armem::robot_state
return robot;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name,
- const VirtualRobot::BaseIO::RobotDescription& loadMode,
- bool blocking)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getSynchronizedRobot(const std::string& name,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
{
return _getSynchronizedRobot(name, armem::Time::Invalid(), loadMode, blocking);
}
VirtualRobot::RobotPtr
- VirtualRobotReader::getSynchronizedRobot(const std::string& name, const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode,
- const bool blocking)
+ VirtualRobotReader::getSynchronizedRobot(
+ const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode,
+ const bool blocking)
{
return _getSynchronizedRobot(name, timestamp, loadMode, blocking);
}
- VirtualRobot::RobotPtr VirtualRobotReader::_getSynchronizedRobot(const std::string& name, const Time& timestamp,
- const VirtualRobot::BaseIO::RobotDescription& loadMode,
- bool blocking)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::_getSynchronizedRobot(
+ const std::string& name,
+ const Time& timestamp,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
{
while (blocking)
{
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 7a2f39c17..1d91f0582 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -40,10 +40,10 @@ namespace armarx::armem::robot_state
class VirtualRobotReader : virtual public RobotReader
{
public:
- VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ using RobotReader::RobotReader;
+
~VirtualRobotReader() override = default;
- void connect() override;
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
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 fc3aed1ee..e7525729d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -18,23 +18,15 @@
#include "constants.h"
-
namespace armarx::armem::robot_state
{
- VirtualRobotWriter::VirtualRobotWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- RobotWriter(memoryNameSystem)
- {
- }
-
-
VirtualRobotWriter::~VirtualRobotWriter() = default;
-
void
- VirtualRobotWriter::connect()
+ VirtualRobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
- RobotWriter::connect();
+ RobotWriter::connect(memoryNameSystem);
}
// TODO(fabian.reister): register property defs
@@ -70,12 +62,14 @@ namespace armarx::armem::robot_state
const robot::RobotState robotState{.timestamp = timestamp,
.globalPose =
robot::RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues(), .proprioception=std::nullopt};
-
- return RobotWriter::storeState(robotState,
- robot.getType(),
- robot.getName(),
- constants::robotRootNodeName /*robot.getRootNode()->getName()*/);
+ .jointMap = robot.getJointValues(),
+ .proprioception = std::nullopt};
+
+ return RobotWriter::storeState(
+ robotState,
+ robot.getType(),
+ robot.getName(),
+ constants::robotRootNodeName /*robot.getRootNode()->getName()*/);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
index 260032c62..29a009714 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
@@ -29,7 +29,6 @@
#include "RobotWriter.h"
-
namespace armarx::armem::robot_state
{
/**
@@ -43,10 +42,10 @@ namespace armarx::armem::robot_state
class VirtualRobotWriter : virtual public RobotWriter
{
public:
- VirtualRobotWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ VirtualRobotWriter() = default;
~VirtualRobotWriter() override;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
[[nodiscard]] bool storeDescription(const VirtualRobot::Robot& robot,
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 21b8e55a9..7fa14f5a1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -38,39 +38,31 @@
#include <SimoxUtility/math/pose/interpolate.h>
// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
// this package
-#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#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/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-
-#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-
-#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
-
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::client::robot_state::localization
{
-
- TransformReader::TransformReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem) {}
-
TransformReader::~TransformReader() = default;
- void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
@@ -83,7 +75,8 @@ namespace armarx::armem::client::robot_state::localization
def->optional(properties.memoryName, prefix + "Memory");
}
- void TransformReader::connect()
+ void
+ TransformReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
@@ -100,15 +93,15 @@ namespace armarx::armem::client::robot_state::localization
}
}
- TransformResult TransformReader::getGlobalPose(const std::string& agentName,
- const std::string& robotRootFrame,
- const armem::Time& timestamp) const
+ TransformResult
+ TransformReader::getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const armem::Time& timestamp) const
{
const TransformQuery query{.header = {.parentFrame = GlobalFrame,
- .frame = robotRootFrame,
- .agent = agentName,
- .timestamp = timestamp
- }};
+ .frame = robotRootFrame,
+ .agent = agentName,
+ .timestamp = timestamp}};
return lookupTransform(query);
}
@@ -135,13 +128,14 @@ namespace armarx::armem::client::robot_state::localization
// }
- TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
+ TransformResult
+ TransformReader::lookupTransform(const TransformQuery& query) const
{
const auto& timestamp = query.header.timestamp;
ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
const IceUtil::Time durationEpsilon = IceUtil::Time::milliSeconds(-1);
- (void) durationEpsilon;
+ (void)durationEpsilon;
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -159,22 +153,20 @@ namespace armarx::armem::client::robot_state::localization
if (not qResult.success)
{
- return
- {
- .transform =
- {
- .header = query.header,
- },
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage
- };
+ return {.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
+ query.header.frame + "' : " + qResult.errorMessage};
}
- const auto& localizationCoreSegment = qResult.memory.getCoreSegment(properties.localizationSegment);
+ const auto& localizationCoreSegment =
+ qResult.memory.getCoreSegment(properties.localizationSegment);
using armarx::armem::common::robot_state::localization::TransformHelper;
return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
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 433694ec8..4d9ee5983 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -40,15 +40,15 @@ namespace armarx::armem::client::robot_state::localization
*
* Detailed description of class ExampleClient.
*/
- class TransformReader :
- virtual public TransformReaderInterface
+ class TransformReader : virtual public TransformReaderInterface
{
public:
- TransformReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ TransformReader() = default;
+ TransformReader(const TransformReader&) = default;
~TransformReader() override;
- void connect() override;
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem) override;
TransformResult getGlobalPose(const std::string& agentName,
const std::string& robotRootFrame,
@@ -59,18 +59,15 @@ namespace armarx::armem::client::robot_state::localization
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationSegment = "Localization";
+ std::string memoryName = "RobotState";
+ std::string localizationSegment = "Localization";
} properties;
-
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
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 9f93ae531..f01f8e924 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -21,45 +21,38 @@
#include "TransformWriter.h"
-#include <optional>
#include <algorithm>
#include <iterator>
#include <numeric>
+#include <optional>
#include <Eigen/Geometry>
#include <IceUtil/Time.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::client::robot_state::localization
{
-
- TransformWriter::TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem) {}
-
TransformWriter::~TransformWriter() = default;
- void TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
@@ -69,15 +62,17 @@ namespace armarx::armem::client::robot_state::localization
"Name of the localization memory core segment to use.");
}
- void TransformWriter::connect()
+ void
+ TransformWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.coreSegmentID.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '"
+ << properties.coreSegmentID.memoryName << "' ...";
try
{
memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID);
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '" << properties.coreSegmentID << "'";
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '"
+ << properties.coreSegmentID << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -86,15 +81,17 @@ namespace armarx::armem::client::robot_state::localization
}
}
- bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
+ bool
+ TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
- const MemoryID providerId = properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
+ const MemoryID providerId =
+ properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
// const auto& timestamp = transform.header.timestamp;
- const MemoryID entityID = providerId.withEntityName(
- transform.header.parentFrame + "," + transform.header.frame);
- const Time timestamp = Time::Now(); // FIXME remove
+ const MemoryID entityID =
+ providerId.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+ const Time timestamp = Time::Now(); // FIXME remove
armem::EntityUpdate update;
update.entityID = entityID;
@@ -117,4 +114,4 @@ namespace armarx::armem::client::robot_state::localization
return updateResult.success;
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::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 e553b0734..182981846 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,6 @@
#include "interfaces.h"
-
namespace armarx::armem::client::robot_state::localization
{
@@ -44,17 +43,15 @@ namespace armarx::armem::client::robot_state::localization
*
* Detailed description of class ExampleClient.
*/
- class TransformWriter :
- virtual public TransformWriterInterface
+ class TransformWriter : virtual public TransformWriterInterface
{
public:
-
- TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ TransformWriter() = default;
~TransformWriter() override;
// TransformWriterInterface
/// to be called in Component::onConnectComponent
- void connect() override;
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem) override;
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
@@ -63,18 +60,16 @@ namespace armarx::armem::client::robot_state::localization
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
// Properties
struct Properties
{
- MemoryID coreSegmentID { "RobotState", "Localization" };
+ MemoryID coreSegmentID{"RobotState", "Localization"};
} properties;
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::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 ae35822c6..5689058cc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -41,7 +42,7 @@ namespace armarx::armem::client::robot_state::localization
virtual ~TransformInterface() = default;
virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
- virtual void connect() = 0;
+ virtual void connect(armem::client::MemoryNameSystem& memoryNameSystem) = 0;
};
class TransformReaderInterface : virtual public TransformInterface
@@ -65,4 +66,4 @@ namespace armarx::armem::client::robot_state::localization
virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
};
-} // namespace armarx::armem::client::robot_state::localization
\ No newline at end of file
+} // namespace armarx::armem::client::robot_state::localization
--
GitLab