Skip to content
Snippets Groups Projects
Commit 42d2aa93 authored by Fabian Reister's avatar Fabian Reister
Browse files

adapting articulated object reader and writer

parent c17da674
No related branches found
No related tags found
2 merge requests!443Feature/end effector trajectories,!437Feature/armarde tof fix
...@@ -5,7 +5,6 @@ ...@@ -5,7 +5,6 @@
#include "../error.h" #include "../error.h"
namespace armarx::armem::client namespace armarx::armem::client
{ {
...@@ -13,7 +12,10 @@ 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; data::AddSegmentInput input;
input.coreSegmentName = coreSegmentName; input.coreSegmentName = coreSegmentName;
...@@ -22,24 +24,30 @@ namespace armarx::armem::client ...@@ -22,24 +24,30 @@ namespace armarx::armem::client
return addSegment(input); 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); 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}); data::AddSegmentsResult results = addSegments({input});
ARMARX_CHECK_EQUAL(results.size(), 1); ARMARX_CHECK_EQUAL(results.size(), 1);
return results.at(0); 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); ARMARX_CHECK_NOT_NULL(memory);
data::AddSegmentsResult results = memory->addSegments(inputs); data::AddSegmentsResult results = memory->addSegments(inputs);
...@@ -47,8 +55,8 @@ namespace armarx::armem::client ...@@ -47,8 +55,8 @@ namespace armarx::armem::client
return results; return results;
} }
CommitResult
CommitResult Writer::commit(const Commit& commit) Writer::commit(const Commit& commit) const
{ {
ARMARX_CHECK_NOT_NULL(memory); ARMARX_CHECK_NOT_NULL(memory);
...@@ -63,15 +71,15 @@ namespace armarx::armem::client ...@@ -63,15 +71,15 @@ namespace armarx::armem::client
return result; return result;
} }
data::CommitResult
data::CommitResult Writer::commit(const data::Commit& _commit) Writer::commit(const data::Commit& _commit) const
{ {
data::Commit commit = _commit; data::Commit commit = _commit;
return this->_commit(commit); return this->_commit(commit);
} }
EntityUpdateResult
EntityUpdateResult Writer::commit(const EntityUpdate& update) Writer::commit(const EntityUpdate& update) const
{ {
armem::Commit commit; armem::Commit commit;
commit.updates.push_back(update); commit.updates.push_back(update);
...@@ -81,10 +89,10 @@ namespace armarx::armem::client ...@@ -81,10 +89,10 @@ namespace armarx::armem::client
return result.results.at(0); return result.results.at(0);
} }
EntityUpdateResult Writer::commit( EntityUpdateResult
const MemoryID& entityID, Writer::commit(const MemoryID& entityID,
const std::vector<aron::data::DictPtr>& instancesData, const std::vector<aron::data::DictPtr>& instancesData,
Time referencedTime) Time referencedTime) const
{ {
EntityUpdate update; EntityUpdate update;
update.entityID = entityID; update.entityID = entityID;
...@@ -99,7 +107,8 @@ namespace armarx::armem::client ...@@ -99,7 +107,8 @@ namespace armarx::armem::client
this->memory = memory; this->memory = memory;
} }
data::CommitResult Writer::_commit(data::Commit& commit) data::CommitResult
Writer::_commit(data::Commit& commit) const
{ {
ARMARX_CHECK_NOT_NULL(memory); ARMARX_CHECK_NOT_NULL(memory);
...@@ -119,11 +128,11 @@ namespace armarx::armem::client ...@@ -119,11 +128,11 @@ namespace armarx::armem::client
} }
data::CommitResult result; data::CommitResult result;
auto handleError = [&commit, &result](const std::string & what) auto handleError = [&commit, &result](const std::string& what)
{ {
for (const auto& _ : commit.updates) for (const auto& _ : commit.updates)
{ {
(void) _; (void)_;
data::EntityUpdateResult& r = result.results.emplace_back(); data::EntityUpdateResult& r = result.results.emplace_back();
r.success = false; r.success = false;
r.errorMessage = "Memory component not registered.\n" + std::string(what); r.errorMessage = "Memory component not registered.\n" + std::string(what);
...@@ -149,18 +158,20 @@ namespace armarx::armem::client ...@@ -149,18 +158,20 @@ namespace armarx::armem::client
return result; return result;
} }
} } // namespace armarx::armem::client
std::ostream&
std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegmentInput& rhs) armarx::armem::data::operator<<(std::ostream& os, const AddSegmentInput& rhs)
{ {
return os << "AddSegmentInput: " return os << "AddSegmentInput: "
<< "\n- core segment: \t'" << rhs.coreSegmentName << "'" << "\n- core segment: \t'" << rhs.coreSegmentName << "'"
<< "\n- provider segment: \t'" << rhs.providerSegmentName << "'" << "\n- provider segment: \t'" << rhs.providerSegmentName << "'"
<< "\n- clear when exists:\t" << rhs.clearWhenExists << "" << "\n- clear when exists:\t" << rhs.clearWhenExists << ""
<< "\n"; << "\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) for (size_t i = 0; i < rhs.size(); ++i)
{ {
...@@ -168,15 +179,18 @@ std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegment ...@@ -168,15 +179,18 @@ std::ostream& armarx::armem::data::operator<<(std::ostream& os, const AddSegment
} }
return os; 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:" return os << "AddSegmentResult:"
<< "\n- success: \t" << rhs.success << "\n- success: \t" << rhs.success << "\n- segment ID: \t'" << rhs.segmentID
<< "\n- segment ID: \t'" << rhs.segmentID << "'" << "'"
<< "\n- error message: \t" << rhs.errorMessage << "\n- error message: \t" << rhs.errorMessage << "\n";
<< "\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) for (size_t i = 0; i < rhs.size(); ++i)
{ {
......
...@@ -32,28 +32,28 @@ namespace armarx::armem::client ...@@ -32,28 +32,28 @@ namespace armarx::armem::client
data::AddSegmentResult addSegment(const std::string& coreSegmentName, data::AddSegmentResult addSegment(const std::string& coreSegmentName,
const std::string& providerSegmentName, const std::string& providerSegmentName,
bool clearWhenExists = false); bool clearWhenExists = false) const;
data::AddSegmentResult addSegment(const MemoryID& providerSegmentID, data::AddSegmentResult addSegment(const MemoryID& providerSegmentID,
bool clearWhenExists = false); bool clearWhenExists = false) const;
data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names, data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names,
bool clearWhenExists = false); bool clearWhenExists = false) const;
data::AddSegmentResult addSegment(const data::AddSegmentInput& input); data::AddSegmentResult addSegment(const data::AddSegmentInput& input) const;
data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input); data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input) const;
/** /**
* @brief Writes a `Commit` to the memory. * @brief Writes a `Commit` to the memory.
*/ */
CommitResult commit(const Commit& commit); CommitResult commit(const Commit& commit) const;
/// Commit a single entity update. /// Commit a single entity update.
EntityUpdateResult commit(const EntityUpdate& update); EntityUpdateResult commit(const EntityUpdate& update) const;
/// Commit a single entity update. /// Commit a single entity update.
EntityUpdateResult commit(const MemoryID& entityID, EntityUpdateResult commit(const MemoryID& entityID,
const std::vector<aron::data::DictPtr>& instancesData, const std::vector<aron::data::DictPtr>& instancesData,
Time referencedTime); Time referencedTime) const;
// with bare-ice types // with bare-ice types
data::CommitResult commit(const data::Commit& commit); data::CommitResult commit(const data::Commit& commit) const;
void setWritingMemory(server::WritingMemoryInterfacePrx memory); void setWritingMemory(server::WritingMemoryInterfacePrx memory);
...@@ -65,7 +65,7 @@ namespace armarx::armem::client ...@@ -65,7 +65,7 @@ namespace armarx::armem::client
private: private:
/// Sets `timeSent` on all entity updates and performs the commit, /// Sets `timeSent` on all entity updates and performs the commit,
data::CommitResult _commit(data::Commit& commit); data::CommitResult _commit(data::Commit& commit) const;
public: public:
......
...@@ -15,8 +15,9 @@ namespace armarx::armem::articulated_object ...@@ -15,8 +15,9 @@ namespace armarx::armem::articulated_object
const std::optional<std::string>& providerName, const std::optional<std::string>& providerName,
const std::string& instanceName = ""); const std::string& instanceName = "");
bool synchronizeArticulatedObject(VirtualRobot::Robot& object, [[nodiscard]] bool
const armem::Time& timestamp, synchronizeArticulatedObject(VirtualRobot::Robot& object,
const std::optional<std::string>& providerName); const armem::Time& timestamp,
const std::optional<std::string>& providerName);
}; };
} // namespace armarx::armem::articulated_object } // namespace armarx::armem::articulated_object
...@@ -26,7 +26,6 @@ ...@@ -26,7 +26,6 @@
#include "utils.h" #include "utils.h"
namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object namespace armarx::armem::articulated_object
{ {
...@@ -100,7 +99,7 @@ namespace armarx::armem::articulated_object ...@@ -100,7 +99,7 @@ namespace armarx::armem::articulated_object
std::optional<ArticulatedObject> std::optional<ArticulatedObject>
Reader::get(const std::string& name, Reader::get(const std::string& name,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName) const std::optional<std::string>& providerName) const
{ {
ARMARX_TRACE; ARMARX_TRACE;
...@@ -125,7 +124,7 @@ namespace armarx::armem::articulated_object ...@@ -125,7 +124,7 @@ namespace armarx::armem::articulated_object
Reader::get(const ArticulatedObjectDescription& description, Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::string& instanceName, const std::string& instanceName,
const std::optional<std::string>& providerName) const std::optional<std::string>& providerName) const
{ {
ARMARX_TRACE; ARMARX_TRACE;
...@@ -142,7 +141,7 @@ namespace armarx::armem::articulated_object ...@@ -142,7 +141,7 @@ namespace armarx::armem::articulated_object
bool bool
Reader::synchronize(ArticulatedObject& obj, Reader::synchronize(ArticulatedObject& obj,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName) const std::optional<std::string>& providerName) const
{ {
ARMARX_TRACE; ARMARX_TRACE;
...@@ -164,7 +163,7 @@ namespace armarx::armem::articulated_object ...@@ -164,7 +163,7 @@ namespace armarx::armem::articulated_object
std::vector<robot::RobotDescription> std::vector<robot::RobotDescription>
Reader::queryDescriptions(const armem::Time& timestamp, Reader::queryDescriptions(const armem::Time& timestamp,
const std::optional<std::string>& providerName) const std::optional<std::string>& providerName) const
{ {
ARMARX_TRACE; ARMARX_TRACE;
// Query all entities from provider. // Query all entities from provider.
...@@ -209,7 +208,7 @@ namespace armarx::armem::articulated_object ...@@ -209,7 +208,7 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotDescription> std::optional<robot::RobotDescription>
Reader::queryDescription(const std::string& name, Reader::queryDescription(const std::string& name,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName) const std::optional<std::string>& providerName) const
{ {
ARMARX_TRACE; ARMARX_TRACE;
...@@ -256,7 +255,7 @@ namespace armarx::armem::articulated_object ...@@ -256,7 +255,7 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotState> std::optional<robot::RobotState>
Reader::queryState(const std::string& instanceName, Reader::queryState(const std::string& instanceName,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName) const std::optional<std::string>& providerName) const
{ {
ARMARX_TRACE; ARMARX_TRACE;
......
...@@ -26,13 +26,17 @@ ...@@ -26,13 +26,17 @@
#include <VirtualRobot/VirtualRobot.h> #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/Reader.h>
#include <RobotAPI/libraries/armem/client/forward_declarations.h>
#include "interfaces.h" #include "interfaces.h"
// #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
// #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
namespace armarx::armem::articulated_object namespace armarx::armem::articulated_object
{ {
...@@ -40,7 +44,7 @@ namespace armarx::armem::articulated_object ...@@ -40,7 +44,7 @@ namespace armarx::armem::articulated_object
{ {
public: public:
Reader() = default; Reader() = default;
virtual ~Reader() = default; ~Reader() override = default;
void void
registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
...@@ -49,30 +53,30 @@ namespace armarx::armem::articulated_object ...@@ -49,30 +53,30 @@ namespace armarx::armem::articulated_object
void connect(armem::client::MemoryNameSystem& memoryNameSystem); void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool synchronize(ArticulatedObject& obj, [[nodiscard]] bool synchronize(ArticulatedObject& obj,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName) override; const std::optional<std::string>& providerName) const override;
std::optional<ArticulatedObject> std::optional<ArticulatedObject>
get(const std::string& name, get(const std::string& name,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName) override; const std::optional<std::string>& providerName) const override;
ArticulatedObject get(const ArticulatedObjectDescription& description, ArticulatedObject get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::string& instanceName, 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, std::optional<robot::RobotState> queryState(const std::string& instanceName,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName); const std::optional<std::string>& providerName) const;
std::optional<robot::RobotDescription> std::optional<robot::RobotDescription>
queryDescription(const std::string& name, queryDescription(const std::string& name,
const armem::Time& timestamp, const armem::Time& timestamp,
const std::optional<std::string>& providerName); const std::optional<std::string>& providerName) const;
std::vector<robot::RobotDescription> std::vector<robot::RobotDescription>
queryDescriptions(const armem::Time& timestamp, queryDescriptions(const armem::Time& timestamp,
const std::optional<std::string>& providerName); const std::optional<std::string>& providerName) const;
std::string getProviderName() const; std::string getProviderName() const;
void setProviderName(const std::string& providerName); void setProviderName(const std::string& providerName);
......
...@@ -103,7 +103,7 @@ namespace armarx::armem::articulated_object ...@@ -103,7 +103,7 @@ namespace armarx::armem::articulated_object
} }
std::optional<armem::MemoryID> std::optional<armem::MemoryID>
Writer::storeOrGetClass(const ArticulatedObject& obj) Writer::storeOrGetClass(const ArticulatedObject& obj) const
{ {
ARMARX_TRACE; ARMARX_TRACE;
...@@ -129,7 +129,7 @@ namespace armarx::armem::articulated_object ...@@ -129,7 +129,7 @@ namespace armarx::armem::articulated_object
} }
std::optional<armem::MemoryID> std::optional<armem::MemoryID>
Writer::storeClass(const ArticulatedObject& obj) Writer::storeClass(const ArticulatedObject& obj) const
{ {
std::lock_guard g{memoryWriterMutex}; std::lock_guard g{memoryWriterMutex};
...@@ -190,7 +190,7 @@ namespace armarx::armem::articulated_object ...@@ -190,7 +190,7 @@ namespace armarx::armem::articulated_object
} }
bool bool
Writer::storeInstance(const ArticulatedObject& obj) Writer::storeInstance(const ArticulatedObject& obj) const
{ {
std::lock_guard g{memoryWriterMutex}; std::lock_guard g{memoryWriterMutex};
...@@ -259,7 +259,7 @@ namespace armarx::armem::articulated_object ...@@ -259,7 +259,7 @@ namespace armarx::armem::articulated_object
} }
bool bool
Writer::store(const ArticulatedObject& obj) Writer::store(const ArticulatedObject& obj) const
{ {
const std::optional<armem::MemoryID> classId = storeOrGetClass(obj); const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
......
...@@ -39,16 +39,16 @@ namespace armarx::armem::articulated_object ...@@ -39,16 +39,16 @@ namespace armarx::armem::articulated_object
{ {
public: public:
Writer() = default; Writer() = default;
virtual ~Writer() = default; ~Writer() override = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def); void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect(armem::client::MemoryNameSystem& memoryNameSystem); void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool store(const ArticulatedObject& obj) override; bool store(const ArticulatedObject& obj) const override;
bool storeInstance(const ArticulatedObject& obj); bool storeInstance(const ArticulatedObject& obj) const;
std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj); std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj) const;
// const std::string& getPropertyPrefix() const override; // const std::string& getPropertyPrefix() const override;
...@@ -57,7 +57,7 @@ namespace armarx::armem::articulated_object ...@@ -57,7 +57,7 @@ namespace armarx::armem::articulated_object
private: private:
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj); std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj) const;
void updateKnownObjects(const armem::MemoryID& subscriptionID, void updateKnownObjects(const armem::MemoryID& subscriptionID,
const std::vector<armem::MemoryID>& snapshotIDs); const std::vector<armem::MemoryID>& snapshotIDs);
...@@ -85,13 +85,13 @@ namespace armarx::armem::articulated_object ...@@ -85,13 +85,13 @@ namespace armarx::armem::articulated_object
const std::string propertyPrefix = "mem.obj.articulated."; const std::string propertyPrefix = "mem.obj.articulated.";
armem::client::Writer memoryWriter; armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex; mutable std::mutex memoryWriterMutex;
armem::client::Reader memoryReader; armem::client::Reader memoryReader;
std::mutex memoryReaderMutex; mutable std::mutex memoryReaderMutex;
// key: name of object: RobotDescription::name // key: name of object: RobotDescription::name
std::unordered_map<std::string, MemoryID> knownObjects; mutable std::unordered_map<std::string, MemoryID> knownObjects;
}; };
......
...@@ -11,19 +11,26 @@ namespace armarx::armem::articulated_object ...@@ -11,19 +11,26 @@ namespace armarx::armem::articulated_object
public: public:
virtual ~ReaderInterface() = default; virtual ~ReaderInterface() = default;
virtual bool synchronize(ArticulatedObject& obj, const armem::Time& timestamp, const std::optional<std::string>& providerName) = 0; virtual bool synchronize(ArticulatedObject& obj,
const armem::Time& timestamp,
virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp, const std::string& instanceName, const std::optional<std::string>& providerName) = 0; 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) = 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 class WriterInterface
{ {
public: public:
virtual ~WriterInterface() = default; 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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment