diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index ec17f944c60747160a3306032fa89c13899d6b86..8c77f92dc15a820ccf13736598bc7a21a37b336e 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -6,11 +6,10 @@
#include <map>
#include <optional>
#include <ostream>
+#include <type_traits>
#include <utility>
#include <vector>
-#include <type_traits>
-
// ICE
#include <IceUtil/Handle.h>
#include <IceUtil/Time.h>
@@ -34,15 +33,13 @@
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
// RobotAPI Armem
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
@@ -65,18 +62,20 @@ namespace armarx::armem::laser_scans::client
registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
-
}
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
- << constants::memoryName << "' ...";
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
+ << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName << "'";
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -91,27 +90,43 @@ namespace armarx::armem::laser_scans::client
armarx::armem::client::query::Builder qb;
ARMARX_VERBOSE << "Query for agent: " << query.agent
- << " memory name: " << constants::memoryName;
+ << " memory name: " << constants::memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().all()
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
else
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
return qb;
@@ -126,16 +141,14 @@ namespace armarx::armem::laser_scans::client
ARMARX_WARNING << "No entities!";
}
- const auto convert =
- [](const auto& aronLaserScanStamped,
- const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert = [](const auto& aronLaserScanStamped,
+ const wm::EntityInstance& ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
const auto ndArrayNavigator =
- aron::data::NDArray::DynamicCast(
- ei.data()->getElement("scan"));
+ aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
@@ -145,47 +158,48 @@ namespace armarx::armem::laser_scans::client
};
// loop over all entities and their snapshots
- providerSegment.forEachEntity([&outV, &convert](const wm::Entity & entity)
- {
- // If we don't need this warning, we could directly iterate over the snapshots.
- if (entity.empty())
+ providerSegment.forEachEntity(
+ [&outV, &convert](const wm::Entity& entity)
{
- ARMARX_WARNING << "Empty history for " << entity.id();
- }
- ARMARX_DEBUG << "History size: " << entity.size();
-
- entity.forEachInstance([&outV, &convert](const wm::EntityInstance & entityInstance)
- {
- if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
{
- outV.push_back(convert(*o, entityInstance));
+ ARMARX_WARNING << "Empty history for " << entity.id();
}
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&outV, &convert](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ return true;
+ });
return true;
});
- return true;
- });
return outV;
}
- Reader::Result Reader::queryData(const Query& query) const
+ Reader::Result
+ Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
ARMARX_DEBUG << "[MappingDataReader] query ... ";
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
if (not qResult.success)
{
- ARMARX_WARNING << "Failed to query data from memory: "
- << qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
@@ -195,16 +209,17 @@ namespace armarx::armem::laser_scans::client
const auto laserScans = asLaserScans(providerSegment);
std::vector<std::string> sensors;
- providerSegment.forEachEntity([&sensors](const wm::Entity & entity)
- {
- sensors.push_back(entity.name());
- return true;
- });
+ providerSegment.forEachEntity(
+ [&sensors](const wm::Entity& entity)
+ {
+ sensors.push_back(entity.name());
+ return true;
+ });
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 70703521336f8ac0b1fdf9649df4a3777d95f431..55beab204b12a7f72432ef2e38789f1856058934 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -72,7 +72,8 @@ namespace armarx::armem::laser_scans::client
{
std::string agent;
- TimeRange timeRange;
+ // if not provided, only latest is queried
+ std::optional<TimeRange> timeRange;
// if empty, all sensors will be queried
std::vector<std::string> sensorList;