diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
similarity index 68%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index fd8250c5b2b086f3869729e2bf6836e5e3c5dc32..286879b5f435a49602b70076b8a8456d37c66d27 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -1,31 +1,32 @@
-#include "MappingDataReader.h"
+#include "Reader.h"
// STD / STL
-#include <cstring>
-#include <vector>
#include <algorithm>
+#include <cstring>
#include <map>
#include <optional>
#include <ostream>
-#include <type_traits>
#include <utility>
+#include <vector>
+
+#include <type_traits>
// ICE
-#include <IceUtil/Time.h>
#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
// Simox
#include <SimoxUtility/algorithm/get_map_keys_values.h>
// ArmarXCore
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
// RobotAPI Interfaces
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#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/aron/core/Exception.h>
@@ -36,42 +37,42 @@
#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/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
-
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.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>
-#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>
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
- MappingDataReader::MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient)
- : memoryClient(memoryClient) {}
+ Reader::Reader(armem::ClientReaderComponentPluginUser& memoryClient) :
+ memoryClient(memoryClient)
+ {
+ }
- MappingDataReader::~MappingDataReader() = default;
+ Reader::~Reader() = default;
- void MappingDataReader::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
"Name of the mapping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- void MappingDataReader::connect()
+ void Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformReader: Waiting for memory '"
@@ -89,19 +90,18 @@ namespace armarx::armem
memoryReader.setReadingMemory(result.proxy);
}
- armem::client::query::Builder
- MappingDataReader::buildQuery(const Query& query) const
+ armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
{
- armem::client::query::Builder qb;
+ armarx::armem::client::query::Builder qb;
ARMARX_INFO << "Query for agent: " << query.agent
- << " memory name: " << properties.mappingMemoryName;
+ << " memory name: " << properties.memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().all()
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -111,7 +111,7 @@ namespace armarx::armem
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -119,10 +119,10 @@ namespace armarx::armem
}
return qb;
-
}
- std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, wm::Entity>& entities)
+ std::vector<LaserScanStamped>
+ asLaserScans(const std::map<std::string, wm::Entity>& entities)
{
std::vector<LaserScanStamped> outV;
@@ -131,25 +131,27 @@ namespace armarx::armem
ARMARX_WARNING << "No entities!";
}
- const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert =
+ [](const arondto::LaserScanStamped& aronLaserScanStamped,
+ const wm::EntityInstance& ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
- const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(ei.data()->getElement("scan"));
+ const auto ndArrayNavigator =
+ aron::datanavigator::NDArrayNavigator::DynamicCast(
+ ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
ARMARX_IMPORTANT << "4";
-
return laserScanStamped;
-
};
// loop over all entities and their snapshots
- for (const auto &[s, entity] : entities)
+ for (const auto& [s, entity] : entities)
{
if (entity.empty())
{
@@ -158,11 +160,12 @@ namespace armarx::armem
ARMARX_DEBUG << "History size: " << entity.size();
- for (const auto &[ss, entitySnapshot] : entity)
+ for (const auto& [ss, entitySnapshot] : entity)
{
for (const auto& entityInstance : entitySnapshot.instances())
{
- const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
+ const auto o =
+ tryCast<arondto::LaserScanStamped>(entityInstance);
if (o)
{
@@ -175,8 +178,7 @@ namespace armarx::armem
return outV;
}
- MappingDataReader::Result
- MappingDataReader::queryData(const Query& query) const
+ Reader::Result Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
@@ -191,25 +193,25 @@ namespace armarx::armem
{
ARMARX_WARNING << "Failed to query data from memory: "
<< qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
// now create result from memory
const auto& entities =
- qResult.memory.getCoreSegment(properties.mappingMemoryName)
- .getProviderSegment(query.agent)
- .entities();
+ qResult.memory.getCoreSegment(properties.memoryName)
+ .getProviderSegment(query.agent)
+ .entities();
const auto laserScans = asLaserScans(entities);
- const auto sensors = simox::alg::get_keys(entities);
+ const auto sensors = simox::alg::get_keys(entities);
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
similarity index 80%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
index aee783764f4b5720e4ae0d8d69fbf6faed297e98..f09e1e271827c1319112f3075d1f47866c238499 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
@@ -21,7 +21,7 @@
#pragma once
-#include <stdint.h>
+#include <cstdint>
#include <string>
#include <vector>
@@ -30,16 +30,14 @@
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
-
+#include <RobotAPI/libraries/armem_vision/types.h>
namespace armarx
{
class ManagedIceObject;
}
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
struct TimeRange
@@ -59,16 +57,15 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataReader
+ class Reader
{
- public:
- MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient);
+ public:
+ Reader(armem::ClientReaderComponentPluginUser& memoryClient);
- virtual ~MappingDataReader();
+ virtual ~Reader();
void connect();
-
struct Query
{
std::string agent;
@@ -77,7 +74,6 @@ namespace armarx::armem
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
-
};
struct Result
@@ -94,32 +90,27 @@ namespace armarx::armem
} status;
std::string errorMessage;
-
};
Result queryData(const Query& query) const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ armarx::armem::client::query::Builder buildQuery(const Query& query) const;
- client::query::Builder buildQuery(const Query& query) const ;
-
-
- private:
-
+ private:
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
-
- const std::string propertyPrefix = "mem.mapping.";
+ const std::string propertyPrefix = "mem.vision.";
armem::ClientReaderComponentPluginUser& memoryClient;
};
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
similarity index 63%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
index 351d71398a6959f346aadf45c6c604edcd865a53..e4c022bba4d0018bb30363ece2af8cb6b1f6e4da 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
@@ -1,35 +1,37 @@
-#include "MappingDataWriter.h"
+#include "Writer.h"
-#include "RobotAPI/libraries/armem_robot_mapping/aron_conversions.h"
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
- MappingDataWriter::MappingDataWriter(armem::ClientWriterComponentPluginUser& component)
- : component(component) {}
+ Writer::Writer(armem::ClientWriterComponentPluginUser& component) :
+ component(component)
+ {
+ }
- MappingDataWriter::~MappingDataWriter() = default;
+ Writer::~Writer() = default;
- void MappingDataWriter::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
"Name of the mapping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- void MappingDataWriter::connect()
+ void Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '"
+ << properties.memoryName << "' ...";
auto result = component.useMemory(properties.memoryName);
if (not result.success)
{
@@ -37,21 +39,21 @@ namespace armarx::armem
return;
}
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory '"
+ << properties.memoryName;
memoryWriter.setWritingMemory(result.proxy);
}
-
- bool MappingDataWriter::storeSensorData(const LaserScan& laserScan,
- const std::string& frame,
- const std::string& agentName,
- const std::int64_t& timestamp)
+ bool Writer::storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
{
std::lock_guard g{memoryWriterMutex};
const auto result =
- memoryWriter.addSegment(properties.mappingMemoryName, agentName);
+ memoryWriter.addSegment(properties.memoryName, agentName);
if (not result.success)
{
@@ -78,7 +80,7 @@ namespace armarx::armem
dict->addElement("scan", toAron(laserScan));
update.instancesData = {dict};
- update.timeCreated = iceTimestamp;
+ update.timeCreated = iceTimestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -93,4 +95,4 @@ namespace armarx::armem
return updateResult.success;
}
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem::vision::laser_scans::client
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
similarity index 70%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
index e5de996746b7d54b46faa891cf2cd26007fbaf02..34edeb55d2c4e69f191568b92ea6a190db98c2f2 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
@@ -25,12 +25,10 @@
#include <mutex>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-
-#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
/**
@@ -44,11 +42,11 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataWriter
+ class Writer
{
- public:
- MappingDataWriter(armem::ClientWriterComponentPluginUser& component);
- virtual ~MappingDataWriter();
+ public:
+ Writer(armem::ClientWriterComponentPluginUser& component);
+ virtual ~Writer();
void connect();
@@ -57,27 +55,30 @@ namespace armarx::armem
// void connect() override;
/// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+ void registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def) /*override*/;
- bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
+ bool storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp);
- private:
+ private:
armem::client::Writer memoryWriter;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
- std::mutex memoryWriterMutex;
+ std::mutex memoryWriterMutex;
- const std::string propertyPrefix = "mem.mapping.";
+ const std::string propertyPrefix = "mem.vision.";
armem::ClientWriterComponentPluginUser& component;
};
-
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client