From ad6e248fcf70ea7174d3d4e35bef93ffee010086 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 May 2021 08:20:48 +0200
Subject: [PATCH] standard query functions for segments
---
.../server/description/Segment.cpp | 35 +++++++++++-------
.../server/localization/Segment.cpp | 36 ++++++++++++++++---
2 files changed, 54 insertions(+), 17 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index b8f2d7ffa..f00a970c1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -84,7 +84,11 @@ namespace armarx::armem::server::robot_state::description
Commit commit;
commit.updates.push_back(update);
- iceMemory.commit(commit);
+ {
+ // std::lock_guard g{memoryMutex};
+ iceMemory.commit(commit);
+ }
+
}
void Segment::updateRobotDescription()
@@ -103,25 +107,30 @@ namespace armarx::armem::server::robot_state::description
Segment::RobotDescriptionMap Segment::getRobotDescriptions() const
{
+
RobotDescriptionMap robotDescriptions;
- for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
{
- for (const auto &[name, entity] : provSeg.entities())
- {
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = robot::convertRobotDescription(entityInstance);
+ // std::lock_guard g{memoryMutex};
- if (not description)
+ for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto &[name, entity] : provSeg.entities())
{
- ARMARX_WARNING << "Could not convert entity instance to "
- "'RobotDescription'";
- continue;
- }
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto description = robot::convertRobotDescription(entityInstance);
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to "
+ "'RobotDescription'";
+ continue;
+ }
- robotDescriptions.emplace(description->name, *description);
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ robotDescriptions.emplace(description->name, *description);
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index d245d2c87..18d01e592 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -1,5 +1,6 @@
#include "Segment.h"
+#include <Eigen/src/Geometry/Transform.h>
#include <IceUtil/Time.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <iterator>
@@ -13,6 +14,7 @@
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
@@ -50,8 +52,6 @@ namespace armarx::armem::server::robot_state::localization
coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::Transform::toInitialAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
-
-
}
void Segment::connect(viz::Client arviz)
@@ -63,14 +63,42 @@ namespace armarx::armem::server::robot_state::localization
{
std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
- // TODO(fabian.reister): implement
+ for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ // arondto::Transform aronTransform;
+ // aronTransform.fromAron(entityInstance);
+
+
+
+ // // Assemble trafo
+
+ // if (not robotState)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotState'";
+ // continue;
+ // }
+
+ // ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ // TODO
+
+ }
+
+ robotGlobalPoses.emplace(robotName, Eigen::Affine3f::Identity());
+
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Number of known robot poses: " << robotGlobalPoses.size();
return robotGlobalPoses;
}
bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
{
-
const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
const MemoryID providerID = coreSegment->id().withProviderSegmentName(transform.header.agent);
--
GitLab