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

articulated object reader: fixing memory queries

parent 812f59a3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+ 77
− 66
View file @ fcb8e784
......@@ -6,14 +6,13 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/PackagePath.h>
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
......@@ -38,7 +37,6 @@ namespace armarx::armem::articulated_object
def->optional(properties.providerName, prefix + "ProviderName");
}
void Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
......@@ -55,7 +53,7 @@ namespace armarx::armem::articulated_object
memoryReader.setReadingMemory(result.proxy);
armem::MemoryID id = armem::MemoryID();
id.memoryName = properties.memoryName;
id.memoryName = properties.memoryName;
id.coreSegmentName = properties.coreClassSegmentName;
// listen to all provider segments!
......@@ -72,7 +70,8 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
......@@ -89,15 +88,14 @@ namespace armarx::armem::articulated_object
// const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
// std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
// {
// updateKnownObject(snapshotID);
// });
}
std::optional<ArticulatedObject> Reader::get(const std::string& name, const armem::Time& timestamp)
std::optional<ArticulatedObject> Reader::get(const std::string& name,
const armem::Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
......@@ -110,17 +108,13 @@ namespace armarx::armem::articulated_object
return get(*description, timestamp);
}
ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp)
{
ArticulatedObject obj
{
.description = description,
.instance = "", // TODO(fabian.reister):
.config = {}, // will be populated by synchronize
.timestamp = timestamp
};
ArticulatedObject obj{.description = description,
.instance = "", // TODO(fabian.reister):
.config = {}, // will be populated by synchronize
.timestamp = timestamp};
synchronize(obj, timestamp);
......@@ -165,7 +159,8 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name,
const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
......@@ -174,8 +169,8 @@ namespace armarx::armem::articulated_object
qb
.coreSegments().withName(properties.coreClassSegmentName)
.providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
.entities().withName(name)
.snapshots().atTime(timestamp);
.entities().all() // withName(name)
.snapshots().latest();
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
......@@ -184,13 +179,15 @@ namespace armarx::armem::articulated_object
if (not qResult.success) /* c++20 [[unlikely]] */
{
ARMARX_WARNING << qResult.errorMessage;
return std::nullopt;
}
return getRobotDescription(qResult.memory);
}
std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description,
const armem::Time& timestamp)
{
// TODO(fabian.reister): how to deal with multiple providers?
......@@ -200,9 +197,9 @@ namespace armarx::armem::articulated_object
// clang-format off
qb
.coreSegments().withName(properties.coreInstanceSegmentName)
.providerSegments().withName(properties.providerName) // agent
.providerSegments().all() // withName(properties.providerName) // agent
.entities().withName(description.name)
.snapshots().atTime(timestamp);
.snapshots().latest();
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
......@@ -217,76 +214,91 @@ namespace armarx::armem::articulated_object
return getRobotState(qResult.memory);
}
std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
std::optional<robot::RobotState>
Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
.getCoreSegment(properties.coreInstanceSegmentName)
.getProviderSegment(properties.providerName);
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(properties.coreInstanceSegmentName);
// clang-format on
const auto entities = simox::alg::get_values(providerSegment.entities());
if (entities.empty())
for (const auto &[_, providerSegment] : coreSegment.providerSegments())
{
ARMARX_WARNING << "No entity found";
return std::nullopt;
}
const auto entitySnapshots = simox::alg::get_values(entities.front().history());
const auto entities = simox::alg::get_values(providerSegment.entities());
if (entitySnapshots.empty())
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
}
if (entities.empty())
{
ARMARX_WARNING << "No entity found";
continue;
}
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
const auto entitySnapshots = simox::alg::get_values(entities.front().history());
return robot::convertRobotState(instance);
}
if (entitySnapshots.empty())
{
ARMARX_WARNING << "No entity snapshots found";
continue;
}
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
return robot::convertRobotState(instance);
}
return std::nullopt;
}
std::optional<robot::RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
.getCoreSegment(properties.coreClassSegmentName)
.getProviderSegment(properties.providerName); // TODO(fabian.reister): all
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(properties.coreClassSegmentName);
// .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
// clang-format on
const auto entities = simox::alg::get_values(providerSegment.entities());
if (entities.empty())
for (const auto &[_, providerSegment] : coreSegment.providerSegments())
{
ARMARX_WARNING << "No entity found";
return std::nullopt;
}
const auto entities = simox::alg::get_values(providerSegment.entities());
if (entities.empty())
{
ARMARX_WARNING << "No entity found";
continue;
// return std::nullopt;
}
const auto entitySnapshots = simox::alg::get_values(entities.front().history());
const auto entitySnapshots = simox::alg::get_values(entities.front().history());
if (entitySnapshots.empty())
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
}
if (entitySnapshots.empty())
{
ARMARX_WARNING << "No entity snapshots found";
continue;
// return std::nullopt;
}
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
return robot::convertRobotDescription(instance);
}
return robot::convertRobotDescription(instance);
return std::nullopt;
}
std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
std::vector<robot::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
std::vector<robot::RobotDescription> descriptions;
const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(properties.coreClassSegmentName);
for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
for (const auto &[providerName, providerSegment] : coreSegment.providerSegments())
{
for (const auto& [name, entity] : providerSegment.entities())
for (const auto &[name, entity] : providerSegment.entities())
{
if (entity.empty())
{
......@@ -309,5 +321,4 @@ namespace armarx::armem::articulated_object
return descriptions;
}
} // namespace armarx::armem::articulated_object
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment