diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
index ce25109502b1da09cb01399396a5c7bca92ed50f..b0bbf2869134f92cdf15e45bde4dd264dda2d1b0 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -20,6 +20,8 @@ namespace armarx::armem::client::util
const std::string prefix = propertyPrefix();
+ props = defaultProperties();
+
def->optional(props.memoryName, prefix + "Memory");
def->optional(props.coreSegmentName, prefix + "CoreSegment");
}
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index cc81788baa40cf3089de08f3699ac8e9cf026c36..f8bbe5ec1db7641c9f634ff51ba6416e1d2dbb81 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -19,13 +19,18 @@ armarx_add_library(
./client/laser_scans/Writer.h
./client/occupancy_grid/Reader.h
./client/occupancy_grid/Writer.h
+ ./client/laser_scanner_features/Reader.h
+ ./client/laser_scanner_features/Writer.h
./OccupancyGridHelper.h
+ constants.h
SOURCES
./aron_conversions.cpp
./client/laser_scans/Reader.cpp
./client/laser_scans/Writer.cpp
./client/occupancy_grid/Reader.cpp
./client/occupancy_grid/Writer.cpp
+ ./client/laser_scanner_features/Reader.cpp
+ ./client/laser_scanner_features/Writer.cpp
./OccupancyGridHelper.cpp
)
@@ -34,6 +39,7 @@ armarx_enable_aron_file_generation_for_target(
"${LIB_NAME}"
ARON_FILES
aron/LaserScan.xml
+ aron/LaserScannerFeatures.xml
aron/OccupancyGrid.xml
)
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
index 4b936b6e5bbc7de8141ea380efdac63f3e8d14ad..89ee1de1132faa3298765bd1c28fda208ca164a7 100644
--- a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -2,14 +2,14 @@
#include <Eigen/Core>
-namespace armarx::armem
+namespace armarx::armem::vision
{
struct OccupancyGrid;
}
namespace armarx
{
- using armarx::armem::OccupancyGrid;
+ using armarx::armem::vision::OccupancyGrid;
namespace detail
{
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
index f2d11c2e4111dfd0c24896ba807aef6435b7014b..9ad92092e4f2fde9a14450e0bf36bacb8cc84326 100644
--- a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::armem::arondto::LaserScannerInfo'>
+ <Object name='armarx::armem::vision::arondto::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
</ObjectChild>
@@ -26,7 +26,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::armem::arondto::SensorHeader">
+ <Object name="armarx::armem::vision::arondto::SensorHeader">
<ObjectChild key="agent">
<string/>
</ObjectChild>
@@ -39,9 +39,9 @@
</Object>
- <Object name='armarx::armem::arondto::LaserScanStamped'>
+ <Object name='armarx::armem::vision::arondto::LaserScanStamped'>
<ObjectChild key="header">
- <armarx::armem::arondto::SensorHeader />
+ <armarx::armem::vision::arondto::SensorHeader />
</ObjectChild>
<!--
@@ -51,4 +51,4 @@
</Object>
</GenerateTypes>
-</AronTypeDefinition>
\ No newline at end of file
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
new file mode 100644
index 0000000000000000000000000000000000000000..32600d7d19eef8f3969bde99c4a66d60635cf4c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
@@ -0,0 +1,67 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::armem::vision::arondto::Ellipsoid'>
+ <ObjectChild key='globalPose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='radii'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::armem::vision::arondto::Circle'>
+ <ObjectChild key='center'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ <ObjectChild key='radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::vision::arondto::LaserScannerFeature">
+ <ObjectChild key="convexHull">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ <ObjectChild key="circle">
+ <armarx::armem::vision::arondto::Circle/>
+ </ObjectChild>
+ <ObjectChild key="ellipsoid">
+ <armarx::armem::vision::arondto::Ellipsoid/>
+ </ObjectChild>
+ <!-- <ObjectChild key="chain">
+ <armarx::armem::vision::arondto::Chain/>
+ </ObjectChild> -->
+ <ObjectChild key="points">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::vision::arondto::LaserScannerFeatures">
+ <ObjectChild key="frame">
+ <String/>
+ </ObjectChild>
+ <ObjectChild key="frameGlobalPose">
+ <Pose/>
+ </ObjectChild>
+ <ObjectChild key="features">
+ <List>
+ <armarx::armem::vision::arondto::LaserScannerFeature />
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
index 0c508a4e2138b4b04c126287bf46d8826fb3da6f..3f273cded23f6942d6be82f76521f929146fb21e 100644
--- a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::armem::arondto::OccupancyGrid'>
+ <Object name='armarx::armem::vision::arondto::OccupancyGrid'>
<ObjectChild key='resolution'>
<float />
</ObjectChild>
@@ -27,4 +27,4 @@
</GenerateTypes>
-</AronTypeDefinition>
\ No newline at end of file
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 4819a27abc53f7b10696da6d452bb2d133a82fd4..cadb4fdcda9f18695833a462e0bef2acfe91451d 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -5,8 +5,9 @@
#include <iterator>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
@@ -14,36 +15,38 @@
#include "types.h"
-namespace armarx::armem
+namespace armarx::armem::vision
{
/************ fromAron ************/
- SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
+ SensorHeader
+ fromAron(const arondto::SensorHeader& aronSensorHeader)
{
- return {.agent = aronSensorHeader.agent,
- .frame = aronSensorHeader.frame,
+ return {.agent = aronSensorHeader.agent,
+ .frame = aronSensorHeader.frame,
.timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScanStamped& laserScan)
+ void
+ fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan)
{
laserScan.header = fromAron(aronLaserScan.header);
// laserScan.data = fromAron(aronLaserScan.data);
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName)
+ void
+ fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName)
{
const auto header = fromAron(aronLaserScan.header);
// laserScan = fromAron(aronLaserScan.data);
timestamp = header.timestamp.toMicroSecondsSinceEpoch();
- frame = header.frame;
+ frame = header.frame;
agentName = header.agent;
}
@@ -54,46 +57,48 @@ namespace armarx::armem
// aronLaserScan.scan = toAron(laserScan);
// }
- int64_t toAron(const armem::Time& timestamp)
+ int64_t
+ toAron(const armem::Time& timestamp)
{
return timestamp.toMicroSecondsSinceEpoch();
}
- arondto::SensorHeader toAron(const SensorHeader& sensorHeader)
+ arondto::SensorHeader
+ toAron(const SensorHeader& sensorHeader)
{
arondto::SensorHeader aronSensorHeader;
- aronSensorHeader.agent = sensorHeader.agent;
- aronSensorHeader.frame = sensorHeader.frame;
+ aronSensorHeader.agent = sensorHeader.agent;
+ aronSensorHeader.frame = sensorHeader.frame;
toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
return aronSensorHeader;
}
- void toAron(const LaserScanStamped& laserScanStamped,
- arondto::LaserScanStamped& aronLaserScanStamped)
+ void
+ toAron(const LaserScanStamped& laserScanStamped,
+ arondto::LaserScanStamped& aronLaserScanStamped)
{
aronLaserScanStamped.header = toAron(laserScanStamped.header);
// toAron(laserScanStamped.data, aronLaserScanStamped.data);
}
- void toAron(const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScanStamped)
+ void
+ toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScanStamped)
{
- const SensorHeader header
- {
- .agent = agentName, .frame = frame, .timestamp = timestamp};
+ const SensorHeader header{.agent = agentName, .frame = frame, .timestamp = timestamp};
- const LaserScanStamped laserScanStamped{.header = header,
- .data = laserScan};
+ const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
toAron(laserScanStamped, aronLaserScanStamped);
}
- void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
+ void
+ toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
{
aron::toAron(dto.frame, bo.frame);
aron::toAron(dto.pose, bo.pose);
@@ -101,7 +106,8 @@ namespace armarx::armem
// bo.grid is NdArray -> need special handling.
}
- void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
+ void
+ fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
{
aron::fromAron(dto.frame, bo.frame);
aron::fromAron(dto.pose, bo.pose);
@@ -109,4 +115,70 @@ namespace armarx::armem
// bo.grid is NdArray -> need special handling.
}
-} // namespace armarx::armem
+ // LaserScannerFeatures
+
+
+ void
+ toAron(arondto::Circle& dto, const Circle& bo)
+ {
+ dto.center = bo.center;
+ dto.radius = bo.radius;
+ }
+
+ void
+ toAron(arondto::Ellipsoid& dto, const Ellipsoid& bo)
+ {
+ dto.globalPose = bo.pose.matrix();
+ dto.radii = bo.radii;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeature& dto, const LaserScannerFeature& bo)
+ {
+ toAron(dto.circle, bo.circle);
+ dto.convexHull = bo.convexHull;
+ toAron(dto.ellipsoid, bo.ellipsoid);
+ dto.points = bo.points;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::toAron(dto.features, bo.features);
+ }
+
+ void
+ fromAron(const arondto::Circle& dto, Circle& bo)
+ {
+ bo.center = dto.center;
+ bo.radius = dto.radius;
+ }
+ void
+ fromAron(const arondto::Ellipsoid& dto, Ellipsoid& bo)
+ {
+ bo.pose = dto.globalPose;
+ bo.radii = dto.radii;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeature& dto, LaserScannerFeature& bo)
+ {
+ bo.convexHull = dto.convexHull;
+ fromAron(dto.circle, bo.circle);
+ fromAron(dto.ellipsoid, bo.ellipsoid);
+
+ // bo.chain = dto.chain;
+ bo.points = dto.points;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::fromAron(dto.features, bo.features);
+ }
+
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
index 681049dac2250e30ad6a3ccee5f25cc6412b074e..bb1e525344c71ac33fabc7d8eda2102f69777107 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -21,15 +21,16 @@
#pragma once
-#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-namespace armarx::armem
+namespace armarx::armem::vision
{
namespace arondto
@@ -47,14 +48,13 @@ namespace armarx::armem
std::string& agentName);
template <typename T>
- auto fromAron(const aron::data::NDArrayPtr& navigator)
+ auto
+ fromAron(const aron::data::NDArrayPtr& navigator)
{
- return aron::converter::AronVectorConverter::ConvertToVector<T>(
- navigator);
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScanStamped& laserScan);
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
void toAron(const LaserScan& laserScan,
const armem::Time& timestamp,
@@ -79,4 +79,8 @@ namespace armarx::armem
return aron::converter::AronEigenConverter::ConvertFromArray(grid);
}
-} // namespace armarx::armem
+ // LaserScannerFeatures
+ void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
+ void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
+
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9cf78c698b31345a9fcc17262bb7a23a25cd7fc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
@@ -0,0 +1,218 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+// RobotAPI Aron
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+// RobotAPI Armem
+#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_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Reader::~Reader() = default;
+
+
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ // ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
+ // registerPropertyDefinitions(def);
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the mapping memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void
+ Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ try
+ {
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ armarx::armem::client::query::Builder
+ Reader::buildQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ qb.coreSegments()
+ .withName(properties.coreSegmentName)
+ .providerSegments()
+ .withName(query.providerName)
+ .entities()
+ .all()
+ .snapshots()
+ .beforeOrAtTime(query.timestamp);
+
+ // auto entitySel = [&]()
+ // {
+ // if (query.name.empty())
+ // {
+ // ARMARX_INFO << "querying all entities";
+ // return sel.entities().all();
+ // }
+ // return sel.entities().withName(query.name);
+ // }();
+
+ // entitySel.snapshots().beforeOrAtTime(query.timestamp);
+
+ return qb;
+ }
+
+ std::vector<LaserScannerFeatures>
+ asFeaturesList(const wm::ProviderSegment& providerSegment)
+ {
+ if (providerSegment.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ return {};
+ }
+
+ // 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"));
+
+ // ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ // laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
+ // ARMARX_IMPORTANT << "4";
+
+ // return laserScanStamped;
+ // };
+
+ std::vector<LaserScannerFeatures> laserScannerFeatures;
+
+ // loop over all entities and their snapshots
+ providerSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "Empty history for " << entity.id();
+ }
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScannerFeatures>(entityInstance))
+ {
+ LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
+ fromAron(o.value(), f);
+ }
+ return true;
+ });
+ return true;
+ });
+
+ return laserScannerFeatures;
+ }
+
+ 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());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.features = {},
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ // now create result from memory
+ const wm::ProviderSegment& providerSegment =
+ qResult.memory.getCoreSegment(properties.coreSegmentName)
+ .getProviderSegment(query.providerName);
+
+ const auto features = asFeaturesList(providerSegment);
+
+ // const auto laserScans = asLaserScans(providerSegment);
+ // std::vector<std::string> sensors;
+ // providerSegment.forEachEntity(
+ // [&sensors](const wm::Entity& entity)
+ // {
+ // sensors.push_back(entity.name());
+ // return true;
+ // });
+
+ return {.features = features,
+ // .sensors = sensors,
+ .status = Result::Status::Success,
+ .errorMessage = ""};
+ }
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..4752ca85cf3df286b3d1378c2a0526f5a89f1a31
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
@@ -0,0 +1,113 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+
+namespace armarx
+{
+ class ManagedIceObject;
+}
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Reader
+ {
+ public:
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader();
+
+ void connect();
+
+ struct Query
+ {
+ std::string providerName;
+
+ std::string name; // sensor name
+
+ armarx::core::time::DateTime timestamp;
+
+ // std::vector<std::string> sensorList;
+ };
+
+ struct Result
+ {
+ std::vector<LaserScannerFeatures> features;
+
+ // std::vector<std::string> sensors;
+
+ enum Status
+ {
+ Error,
+ Success
+ } status;
+
+ std::string errorMessage;
+ };
+
+ Result queryData(const Query& query) const;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+
+ protected:
+ armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+ private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Reader memoryReader;
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScannerFeatures";
+ } properties;
+
+ const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ };
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5746bffa1880c3031e183acf5cecd74f2d6d515a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
@@ -0,0 +1,111 @@
+#include "Writer.h"
+
+#include "RobotAPI/libraries/armem_vision/constants.h"
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Writer::~Writer() = default;
+
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ // def->optional(properties.coreSegmentName,
+ // prefix + "CoreSegment",
+ // "Name of the mapping memory core segment to use.");
+
+ // def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void
+ Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
+ << "' ...";
+ try
+ {
+ memoryWriter =
+ memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
+ }
+
+ bool
+ Writer::store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ // const auto result = memoryWriter.addSegment(constants::memoryName,
+ // constants::laserScannerFeaturesCoreSegment);
+
+ // if (not result.success)
+ // {
+ // ARMARX_ERROR << result.errorMessage;
+
+ // // TODO(fabian.reister): message
+ // return false;
+ // }
+
+ const auto entityID = armem::MemoryID()
+ .withMemoryName(constants::memoryName)
+ .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
+ .withProviderSegmentName(providerName)
+ .withEntityName(features.frame)
+ .withTimestamp(timestamp);
+
+ ARMARX_VERBOSE << "Memory id is " << entityID.str();
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ ARMARX_TRACE;
+
+ arondto::LaserScannerFeatures dto;
+ toAron(dto, features);
+
+ ARMARX_TRACE;
+
+ update.instancesData = {dto.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+
+ ARMARX_TRACE;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca58005772268d70f7eedf06189c02250c75fef6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
@@ -0,0 +1,85 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer
+ {
+ public:
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Writer();
+
+
+ void connect();
+
+ // MappingDataWriterInterface
+ /// to be called in Component::onConnectComponent
+ // void connect() override;
+
+ /// to be called in Component::addPropertyDefinitions
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+
+ bool store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const Time& timestamp);
+
+ private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Writer memoryWriter;
+
+ // Properties
+ struct Properties
+ {
+ // std::string memoryName = "Vision";
+ // std::string coreSegmentName = "LaserScannerFeatures";
+ } properties;
+
+ std::mutex memoryWriterMutex;
+
+ const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ };
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index 5faf563cfab3c4c380258268671f5c7268fe9dd1..396dd1bd8b4f40cfef062fa963e87c97f52f45b4 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -131,7 +131,7 @@ namespace armarx::armem::vision::laser_scans::client
}
const auto convert =
- [](const arondto::LaserScanStamped & aronLaserScanStamped,
+ [](const auto& aronLaserScanStamped,
const wm::EntityInstance & ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
@@ -177,7 +177,7 @@ namespace armarx::armem::vision::laser_scans::client
{
const auto qb = buildQuery(query);
- ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
index 138717c42257e02275a15785940783330126ee4d..349b65a5af3bea2fc6acb17324a42b87fbb3bf8d 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
@@ -113,7 +113,7 @@ namespace armarx::armem::vision::laser_scans::client
std::string coreSegmentName = "LaserScans";
} properties;
- const std::string propertyPrefix = "mem.vision.";
+ const std::string propertyPrefix = "mem.vision.laser_scans.";
};
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
index afc383634b87a31fc84a841a5658f83ec845ee27..b4f7502ef9493692fc82bf79740d2367fe165d4a 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
@@ -81,7 +81,7 @@ namespace armarx::armem::vision::laser_scans::client
std::mutex memoryWriterMutex;
- const std::string propertyPrefix = "mem.vision.";
+ const std::string propertyPrefix = "mem.vision.laser_scans.";
};
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
index bb3bdf5546650f2bb8c45db39b092c0f601c2c9a..91505d40e5ea0046fc2419ac532db85010062011 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -115,7 +115,7 @@ namespace armarx::armem::vision::occupancy_grid::client
{
const auto qb = buildQuery(query);
- ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
const armem::client::QueryResult qResult =
memoryReader().query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_vision/constants.h b/source/RobotAPI/libraries/armem_vision/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..653d6fda6fc9e787d00f570af5440a6c81c6108b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/constants.h
@@ -0,0 +1,33 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+
+namespace armarx::armem::vision::constants
+{
+ const inline std::string memoryName = "Vision";
+
+ // core segments
+ const inline std::string laserScannerFeaturesCoreSegment = "LaserScannerFeatures";
+
+} // namespace armarx::armem::vision::constants
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index 00fb545a104afb19c38c9d7b01fd864ed6ba1fe6..85322d188ce8fdbd9b065ce476e281408d38f254 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -23,10 +23,12 @@
#include <vector>
+#include <VirtualRobot/MathTools.h>
+
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-namespace armarx::armem
+namespace armarx::armem::vision
{
struct SensorHeader
@@ -52,10 +54,50 @@ namespace armarx::armem
// using ValueType = _ValueT;
using CellType = float;
- using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
+ using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
Grid grid;
};
+ struct Ellipsoid
+ {
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+
+ Eigen::Vector2f radii = Eigen::Vector2f::Zero();
+ };
+
+ struct Circle
+ {
+ Eigen::Vector2f center = Eigen::Vector2f::Zero();
+ float radius = 0.F;
+ };
+
+ struct LaserScannerFeature
+ {
+ using Points = std::vector<Eigen::Vector2f>;
+ using Chain = Points;
+
+ Points convexHull;
+
+ Circle circle;
+ Ellipsoid ellipsoid;
+
+ Chain chain;
+
+ Points points;
+ };
+
+ struct LaserScannerFeatures
+ {
+ // TODO(fabian.reister): framed pose
+ std::string frame;
+ Eigen::Isometry3f frameGlobalPose;
+
+ std::vector<LaserScannerFeature> features;
+
+
+ // std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
+ };
+
-} // namespace armarx::armem
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
index 0bf14ec90439e50a4869234b81449adaf068d173..deed74c9ada113f05d10286a370b703d6a712642 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
@@ -14,4 +14,14 @@ namespace armarx::aron
dto = bo.matrix();
}
+ void fromAron(const AronPose& dto, Eigen::Isometry3f& bo)
+ {
+ bo.matrix() = dto;
+ }
+
+ void toAron(AronPose& dto, const Eigen::Isometry3f& bo)
+ {
+ dto = bo.matrix();
+ }
+
} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
index 3f091dbfc40ee8e91b8d2922687273269ce790a8..3520b2dedd7c10f2d358cb7997269871979b52ed 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
@@ -9,4 +9,7 @@ namespace armarx::aron
void fromAron(const AronPose& dto, Eigen::Affine3f& bo);
void toAron(AronPose& dto, const Eigen::Affine3f& bo);
+ void fromAron(const AronPose& dto, Eigen::Isometry3f& bo);
+ void toAron(AronPose& dto, const Eigen::Isometry3f& bo);
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index e1213b86b1f2f6386fb22d318f4ed798a72eb040..8d6ccf594f8d0adcd168a066111b1ad2fecdcf39 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -111,8 +111,9 @@ namespace armarx::aron::converter
using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
- MatrixT ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ ARMARX_CHECK_EQUAL(dims.size(), 2); // for now ...
+ MatrixT ret(dims.at(0), dims.at(1));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<>()));
return ret;
}
@@ -128,7 +129,7 @@ namespace armarx::aron::converter
auto dims = nav.getShape();
Eigen::Matrix<T, Rows, Cols> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<int>()));
return ret;
}