From c3d457017feab91e78bbaee4176a5e04387b5fba Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 3 Mar 2023 12:53:51 +0100
Subject: [PATCH] robot reader: querying all robot descriptions
---
.../client/common/RobotReader.cpp | 96 +++++++++++++++----
.../client/common/RobotReader.h | 14 ++-
2 files changed, 90 insertions(+), 20 deletions(-)
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 465e1e5a3..eaa2e9ca0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -22,9 +22,9 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
@@ -136,9 +136,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::RobotDescription>
- RobotReader::queryDescription(
- const std::string& name,
- const armem::Time& timestamp)
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -159,7 +157,7 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
ARMARX_WARNING << "Memory reader is null. Did you forget to call "
- "RobotReader::connect() in onConnectComponent()?";
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -345,7 +343,8 @@ namespace armarx::armem::robot_state
{
try
{
- const auto result = transformReader.getGlobalPose(description.name, constants::robotRootNodeName, timestamp);
+ const auto result = transformReader.getGlobalPose(
+ description.name, constants::robotRootNodeName, timestamp);
if (not result)
{
return std::nullopt;
@@ -430,7 +429,7 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&jointMap](const wm::Entity& entity)
{
- if(not entity.getLatestSnapshot().hasInstance(0))
+ if (not entity.getLatestSnapshot().hasInstance(0))
{
return;
}
@@ -565,11 +564,11 @@ namespace armarx::armem::robot_state
}
-
- std::optional<std::map<RobotReader::Hand, robot::ToFArray>> RobotReader::queryToF(const robot::RobotDescription& description,
- const armem::Time& timestamp) const
+ std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
+ RobotReader::queryToF(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
{
- // Query all entities from provider.
+ // Query all entities from provider.
armem::client::query::Builder qb;
ARMARX_DEBUG << "Querying ToF data for robot: " << description;
@@ -610,7 +609,7 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&platformState](const wm::Entity& entity)
{
- if(not entity.getLatestSnapshot().hasInstance(0))
+ if (not entity.getLatestSnapshot().hasInstance(0))
{
return;
}
@@ -658,7 +657,7 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&forceTorques](const wm::Entity& entity)
{
- if(not entity.getLatestSnapshot().hasInstance(0))
+ if (not entity.getLatestSnapshot().hasInstance(0))
{
return;
}
@@ -716,8 +715,7 @@ namespace armarx::armem::robot_state
}
std::map<RobotReader::Hand, robot::ToFArray>
- RobotReader::getToF(const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
{
std::map<RobotReader::Hand, robot::ToFArray> tofs;
@@ -731,7 +729,7 @@ namespace armarx::armem::robot_state
{
ARMARX_DEBUG << "Processing ToF element";
- if(not entity.getLatestSnapshot().hasInstance(0))
+ if (not entity.getLatestSnapshot().hasInstance(0))
{
return;
}
@@ -780,4 +778,70 @@ namespace armarx::armem::robot_state
return robot::convertRobotDescription(*instance);
}
+ std::vector<robot::RobotDescription>
+ RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(constants::descriptionCoreSegment);
+
+ std::vector<robot::RobotDescription> descriptions;
+
+ coreSegment.forEachInstance(
+ [&descriptions](const wm::EntityInstance& instance)
+ {
+ if (const std::optional<robot::RobotDescription> desc =
+ robot::convertRobotDescription(instance))
+ {
+ descriptions.push_back(desc.value());
+ }
+ });
+
+ return descriptions;
+ }
+
+ std::vector<robot::RobotDescription>
+ RobotReader::queryDescriptions(const armem::Time& timestamp)
+ {
+ const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::descriptionCoreSegment)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(sanitizedTimestamp);
+ // clang-format on
+
+ ARMARX_DEBUG << "Lookup query in reader";
+
+ if (not memoryReader)
+ {
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
+ return {};
+ }
+
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+ catch (...)
+ {
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
+ }
+
+ return {};
+ }
} // namespace armarx::armem::robot_state
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 da57e74af..a9cddc6b2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -23,6 +23,7 @@
#include <mutex>
#include <optional>
+#include <vector>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -60,6 +61,8 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
const armem::Time& timestamp);
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
const armem::Time& timestamp);
@@ -100,8 +103,8 @@ namespace armarx::armem::robot_state
const armem::Time& end) const;
- std::optional<std::map<Hand, robot::ToFArray>> queryToF(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<std::map<Hand, robot::ToFArray>>
+ queryToF(const robot::RobotDescription& description, const armem::Time& timestamp) const;
/**
* @brief retrieve the robot's pose in the odometry frame.
@@ -128,6 +131,9 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::vector<robot::RobotDescription>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
@@ -145,8 +151,8 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::map<RobotReader::Hand, robot::ToFArray>
- getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::map<RobotReader::Hand, robot::ToFArray> getToF(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
struct Properties
{
--
GitLab