From cd56109a0be029085a374c706b1e7d590d510c63 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 26 Apr 2021 08:27:15 +0200
Subject: [PATCH] armem robot mapping: performance improvements: laser scans
are now ndarrays
---
.../armem_robot_mapping/MappingDataReader.cpp | 100 +++++++++++++++---
.../armem_robot_mapping/MappingDataReader.h | 11 +-
.../armem_robot_mapping/MappingDataWriter.cpp | 99 +++++++++--------
.../armem_robot_mapping/aron/LaserScan.xml | 28 +----
.../armem_robot_mapping/aron_conversions.cpp | 71 ++-----------
.../armem_robot_mapping/aron_conversions.h | 17 ++-
.../libraries/armem_robot_mapping/types.h | 1 -
7 files changed, 176 insertions(+), 151 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 6f57909c6..e9fe167f3 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -1,21 +1,46 @@
#include "MappingDataReader.h"
+#include <cstring>
#include <vector>
+#include <algorithm>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <type_traits>
+#include <utility>
#include <IceUtil/Time.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <IceUtil/Handle.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.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/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/ProviderSegment.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
#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_localization/MemoryConnector.h"
+
+namespace armarx::armem
+{
+ class Entity;
+} // namespace armarx
namespace armarx::armem
{
@@ -94,6 +119,59 @@ namespace armarx::armem
}
+ std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, Entity>& entities)
+ {
+ std::vector<LaserScanStamped> outV;
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ }
+
+ const auto convert = [](const aron::LaserScanStamped & aronLaserScanStamped, const EntityInstance & ei) -> LaserScanStamped
+ {
+ LaserScanStamped laserScanStamped;
+ fromAron(aronLaserScanStamped, laserScanStamped);
+
+ 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)
+ {
+ 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)
+ {
+ const auto o = tryCast<aron::LaserScanStamped>(entityInstance);
+
+ if (o)
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ }
+ }
+ }
+
+ return outV;
+ }
+
MappingDataReader::Result
MappingDataReader::queryData(const Query& query) const
{
@@ -122,18 +200,10 @@ namespace armarx::armem
.getProviderSegment(query.agent)
.entities;
- auto laserScans = transformAllOfType<aron::LaserScanStamped>(
- entities,
- [](const aron::LaserScanStamped & aronLaserScan) -> LaserScanStamped
- {
- LaserScanStamped laserScan;
- fromAron(aronLaserScan, laserScan);
- return laserScan;
- });
-
+ const auto laserScans = asLaserScans(entities);
const auto sensors = simox::alg::get_keys(entities);
- return {.laserScans = std::move(laserScans),
+ return {.laserScans = laserScans,
.sensors = sensors,
.status = Result::Status::Success,
.errorMessage = ""};
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
index 3c700703e..da3bcc6ee 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
@@ -13,7 +13,6 @@
* 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
@@ -22,12 +21,18 @@
#pragma once
-#include "RobotAPI/libraries/armem_robot_mapping/types.h"
-#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <stdint.h>
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
// TODO(fabian.reister): move MemoryConnector to armem library
#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem_robot_mapping/types.h>
+
namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
index 37f85182f..86e203cce 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
@@ -4,28 +4,30 @@
#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-namespace armarx::armem {
+namespace armarx::armem
+{
-MappingDataWriter::MappingDataWriter(ManagedIceObject &component)
- : MemoryConnector(component) {}
+ MappingDataWriter::MappingDataWriter(ManagedIceObject& component)
+ : MemoryConnector(component) {}
-MappingDataWriter::~MappingDataWriter() = default;
+ MappingDataWriter::~MappingDataWriter() = default;
-void MappingDataWriter::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr &def) {
- ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
+ void MappingDataWriter::registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
+ MemoryConnector::registerPropertyDefinitions(def);
- const std::string prefix = getPropertyPrefix();
+ const std::string prefix = getPropertyPrefix();
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
- "Name of the mapping memory core segment to use.");
+ def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ "Name of the mapping memory core segment to use.");
- def->optional(properties.memoryName, prefix + "MemoryName");
-}
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
- void MappingDataWriter::connect()
+ void MappingDataWriter::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
@@ -43,48 +45,55 @@ void MappingDataWriter::registerPropertyDefinitions(
}
-bool MappingDataWriter::storeSensorData(const LaserScan &laserScan,
- const std::string &frame,
- const std::string &agentName,
- const std::int64_t ×tamp) {
- std::lock_guard g{memoryWriterMutex};
+ bool MappingDataWriter::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);
+ const auto result =
+ memoryWriter.addSegment(properties.mappingMemoryName, agentName);
- if (not result.success) {
- ARMARX_ERROR << result.errorMessage;
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
- // TODO(fabian.reister): message
- return false;
- }
+ // TODO(fabian.reister): message
+ return false;
+ }
- const auto iceTimestamp = IceUtil::Time::microSeconds(timestamp);
+ const auto iceTimestamp = IceUtil::Time::microSeconds(timestamp);
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId.withEntityName(frame).withTimestamp(iceTimestamp);
- armem::EntityUpdate update;
- update.entityID = entityID;
- update.timeCreated = armem::Time::now();
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+ update.timeCreated = armem::Time::now();
- aron::LaserScanStamped aronSensorData;
- toAron(laserScan, timestamp, frame, agentName, aronSensorData);
+ aron::LaserScanStamped aronSensorData;
+ // currently only sets the header
+ toAron(laserScan, timestamp, frame, agentName, aronSensorData);
- update.instancesData = {aronSensorData.toAron()};
- update.timeCreated = iceTimestamp;
+ auto dict = aronSensorData.toAron();
+ dict->addElement("scan", toAron(laserScan));
- ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
- armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+ update.instancesData = {dict};
+ update.timeCreated = iceTimestamp;
- ARMARX_DEBUG << updateResult;
+ ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
- if (not updateResult.success) {
- ARMARX_ERROR << updateResult.errorMessage;
- }
+ ARMARX_DEBUG << updateResult;
- return updateResult.success;
-}
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
index 140e2aab8..860726583 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
@@ -8,21 +8,6 @@
<GenerateTypes>
- <Object name='armarx::aron::LaserScanStep'>
- <ObjectChild key='distance'>
- <float />
- </ObjectChild>
- <ObjectChild key='angle'>
- <float />
- </ObjectChild>
- <!--ObjectChild key='relativeTime'>
- <float />
- </ObjectChild-->
- <!--ObjectChild key='intensity'>
- <float />
- </ObjectChild-->
- </Object>
-
<Object name='armarx::aron::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
@@ -53,21 +38,16 @@
</ObjectChild>
</Object>
- <Object name="armarx::aron::LaserScan">
- <ObjectChild key='scan'>
- <List>
- <armarx::aron::LaserScanStep />
- </List>
- </ObjectChild>
- </Object>
<Object name='armarx::aron::LaserScanStamped'>
<ObjectChild key="header">
<armarx::aron::SensorHeader />
</ObjectChild>
+
+ <!--
<ObjectChild key='data'>
- <armarx::aron::LaserScan />
- </ObjectChild>
+ <NdArray />
+ </ObjectChild> -->
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
index a7b8277c3..57cb0873c 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
@@ -5,44 +5,17 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/converter/common/Converter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include "types.h"
+
namespace armarx
{
/************ fromAron ************/
-
- LaserScanStep fromAron(const aron::LaserScanStep& aronLaserScanStep)
- {
- LaserScanStep laserScanStep;
-
- laserScanStep.angle = aronLaserScanStep.angle;
- laserScanStep.distance = aronLaserScanStep.distance;
-
- return laserScanStep;
- }
-
- template <typename T> auto fromAron(const std::vector<T>& v)
- {
- std::vector<decltype(fromAron(T()))> r;
- r.reserve(v.size());
-
- std::transform(v.begin(), v.end(), std::back_inserter(r),
- [](const T & t)
- {
- return fromAron(t);
- });
-
- return r;
- }
-
- auto fromAron(const aron::LaserScan& aronLaserScan)
- {
- return fromAron(aronLaserScan.scan);
- }
-
SensorHeader fromAron(const aron::SensorHeader& aronSensorHeader)
{
return {.agent = aronSensorHeader.agent,
@@ -54,7 +27,7 @@ namespace armarx
LaserScanStamped& laserScan)
{
laserScan.header = fromAron(aronLaserScan.header);
- laserScan.data = fromAron(aronLaserScan.data);
+ // laserScan.data = fromAron(aronLaserScan.data);
}
void fromAron(const aron::LaserScanStamped& aronLaserScan, LaserScan& laserScan,
@@ -63,7 +36,7 @@ namespace armarx
{
const auto header = fromAron(aronLaserScan.header);
- laserScan = fromAron(aronLaserScan.data);
+ // laserScan = fromAron(aronLaserScan.data);
timestamp = header.timestamp;
frame = header.frame;
@@ -74,34 +47,10 @@ namespace armarx
/************ toAron ************/
- template <typename T> auto toAron(const std::vector<T>& v)
- {
- std::vector<decltype(toAron(T()))> r;
- r.reserve(v.size());
-
- std::transform(v.begin(), v.end(), std::back_inserter(r),
- [](const T & t)
- {
- return toAron(t);
- });
-
- return r;
- }
-
- aron::LaserScanStep toAron(const LaserScanStep& laserScanStep)
- {
- aron::LaserScanStep aronLaserScan;
-
- aronLaserScan.angle = laserScanStep.angle;
- aronLaserScan.distance = laserScanStep.distance;
-
- return aronLaserScan;
- }
-
- auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
- {
- aronLaserScan.scan = toAron(laserScan);
- }
+ // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+ // {
+ // aronLaserScan.scan = toAron(laserScan);
+ // }
aron::SensorHeader toAron(const SensorHeader& sensorHeader)
{
@@ -118,7 +67,7 @@ namespace armarx
aron::LaserScanStamped& aronLaserScanStamped)
{
aronLaserScanStamped.header = toAron(laserScanStamped.header);
- toAron(laserScanStamped.data, aronLaserScanStamped.data);
+ // toAron(laserScanStamped.data, aronLaserScanStamped.data);
}
void toAron(const LaserScan& laserScan,
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
index 865e054b1..a7b686fdb 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
@@ -13,7 +13,6 @@
* 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 RobotComponents::ArmarXObjects::
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -24,7 +23,8 @@
#pragma once
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
namespace armarx
{
@@ -44,6 +44,13 @@ namespace armarx
std::string& frame,
std::string& agentName);
+
+ template<typename T>
+ auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
+ {
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
+ }
+
void fromAron(const aron::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
void toAron(
@@ -53,4 +60,10 @@ namespace armarx
const std::string& agentName,
aron::LaserScanStamped& aronLaserScan);
+ inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
+ {
+ return aron::datanavigator::NDArrayNavigator::FromVector(laserScan);
+ }
+
+
} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/types.h b/source/RobotAPI/libraries/armem_robot_mapping/types.h
index f3bfa33b7..6e8a08414 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/types.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/types.h
@@ -13,7 +13,6 @@
* 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 RobotComponents::ArmarXObjects::
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
--
GitLab