From 9a472af9c6163dc565dda7411e9b0afc4705617d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 12 Apr 2021 21:18:21 +0200
Subject: [PATCH] fixing query
---
source/RobotAPI/libraries/armem/util/util.h | 40 ++++++++-----
.../armem_robot_mapping/MappingDataReader.cpp | 56 +++++++++++--------
2 files changed, 60 insertions(+), 36 deletions(-)
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 8dcb4b997..4fa457ab6 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -22,6 +22,7 @@
#pragma once
+#include "ArmarXCore/core/logging/Logging.h"
#include <vector>
#include <optional>
@@ -29,14 +30,15 @@
#include <RobotAPI/libraries/armem/core/EntityInstance.h>
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
-namespace armarx::armem {
+namespace armarx::armem
+{
/**
* @brief Tries to cast a armem::EntityInstance to AronClass
- *
+ *
* @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
- * @param item
- * @return std::optional<AronClass>
+ * @param item
+ * @return std::optional<AronClass>
*/
template <typename AronClass>
std::optional<AronClass> tryCast(const EntityInstance& item)
@@ -58,10 +60,10 @@ namespace armarx::armem {
/**
* @brief Returns all entities that can be cast to AronClass
- *
+ *
* @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
* @param entities collection of entities
- * @return std::vector<AronClass>
+ * @return std::vector<AronClass>
*/
template <typename AronClass>
std::vector<AronClass>
@@ -93,8 +95,8 @@ namespace armarx::armem {
}
/**
- * @brief filter + transform for entities.
- *
+ * @brief filter + transform for entities.
+ *
* Can be used instead of
*
* std::vector<Bar> ret;
@@ -102,26 +104,38 @@ namespace armarx::armem {
* const auto allOf = allOfType<Foo>(entities);
* std::transform(allOf.begin(), allOf.end(), std::back_inserter(ret), pred)
*
- * This function has the benefit that the transform function will be applied directly.
- * No intermediate vector has to be created (e.g. "allOf" in the example above).
- *
+ * This function has the benefit that the transform function will be applied directly.
+ * No intermediate vector has to be created (e.g. "allOf" in the example above).
+ *
* @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
* @param entities collection of entities
* @param pred binary predicate function, applied to all entity instances
- * @return vector of "pred"-transformed elements that can be cast to AronClass
+ * @return vector of "pred"-transformed elements that can be cast to AronClass
*/
template <typename AronClass>
auto transformAllOfType(const std::map<std::string, Entity>& entities,
- auto pred) -> std::vector<decltype(pred(AronClass()))>
+ auto pred) -> std::vector<decltype(pred(AronClass()))>
{
static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
AronClass>::value);
std::vector<decltype(pred(AronClass()))> outV;
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ }
+
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
+ if (entity.history.empty())
+ {
+ ARMARX_WARNING << "Empty history for " << s;
+ }
+
+ ARMARX_INFO << "History size: " << entity.history.size();
+
for (const auto &[ss, entitySnapshot] : entity.history)
{
for (const auto& entityInstance : entitySnapshot.instances)
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 126c6b003..01250bbb4 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -10,19 +10,19 @@
#include <RobotAPI/libraries/armem/core/EntityInstance.h>
#include <RobotAPI/libraries/armem/core/EntitySnapshot.h>
#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_mapping/types.h>
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
namespace armarx::armem
{
- MappingDataReader::MappingDataReader(ManagedIceObject& component) : armarx::armem::MemoryConnector(component) {}
+ MappingDataReader::MappingDataReader(ManagedIceObject& component)
+ : armarx::armem::MemoryConnector(component) {}
MappingDataReader::~MappingDataReader() = default;
-
void MappingDataReader::registerPropertyDefinitions(
armarx::PropertyDefinitionsPtr& def)
{
@@ -60,34 +60,42 @@ namespace armarx::armem
{
armem::client::query::Builder qb;
- // clang-format off
- auto qbProviderSegments = qb
- .coreSegments().withName(properties.mappingMemoryName)
- .providerSegments().withName(query.agent);
- // clang-format on
+ ARMARX_INFO << "Query for agent: " << query.agent
+ << " memory name: " << properties.mappingMemoryName;
- auto& qbEntities = [&]() -> armarx::armem::client::query::EntitySelector &
+ if (query.sensorList.empty()) // all sensors
{
- if (query.sensorList.empty())
- {
- return qbProviderSegments.entities().all();
- }
-
- return qbProviderSegments.entities().withNames(query.sensorList);
- }();
-
- qbEntities.snapshots().timeRange(
- IceUtil::Time::microSeconds(query.timeRange.min),
- IceUtil::Time::microSeconds(query.timeRange.max));
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.mappingMemoryName)
+ .providerSegments().withName(query.agent)
+ .entities().all()
+ .snapshots().timeRange(
+ IceUtil::Time::microSeconds(query.timeRange.min),
+ IceUtil::Time::microSeconds(query.timeRange.max));
+ // clang-format on
+ }
+ else
+ {
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.mappingMemoryName)
+ .providerSegments().withName(query.agent)
+ .entities().withNames(query.sensorList)
+ .snapshots().timeRange(
+ IceUtil::Time::microSeconds(query.timeRange.min),
+ IceUtil::Time::microSeconds(query.timeRange.max));
+ // clang-format on
+ }
return qb;
- }
+ }
MappingDataReader::Result
MappingDataReader::queryData(const Query& query) const
{
- auto qb = buildQuery(query);
+ const auto qb = buildQuery(query);
ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
@@ -98,6 +106,8 @@ namespace armarx::armem
if (not qResult.success)
{
+ ARMARX_WARNING << "Failed to query data from memory: "
+ << qResult.errorMessage;
return {.laserScans = {},
.status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
@@ -123,4 +133,4 @@ namespace armarx::armem
.errorMessage = ""};
}
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem
--
GitLab