diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index 4f7c222f5c8ebb3f6a5f65062033218e058a8ddb..b763c4301703bd83fe3af4b06a8fa9d96e4b6b1d 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -85,6 +85,11 @@ namespace armarx::viz
return this->position(position).orientation(orientation);
}
+ DerivedT& pose(const Eigen::Affine3f& pose)
+ {
+ return this->position(pose.translation()).orientation(pose.linear());
+ }
+
Eigen::Matrix4f pose() const
{
auto& p = data_->pose;
diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index cc00c9febfc9927d7a4985a63b1dabf2d8621300..328d1f59d793cbcc9a7d0febc0951775d200013a 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/source/RobotAPI/libraries/armem/core/Time.h
@@ -34,4 +34,4 @@ namespace armarx::armem
*/
Time timeFromStringMicroSeconds(const std::string& microSeconds);
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index f5577f9abf2c34e596cd47e28593f2ab7c8645a2..b9682ddebf691e850cc604c610a4d98d33610aa5 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -1,19 +1,47 @@
#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 <IceUtil/Handle.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <ArmarXCore/core/logging/Logging.h>
-
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.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/workingmemory/CoreSegment.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/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.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
{
@@ -70,9 +98,7 @@ namespace armarx::armem
.coreSegments().withName(properties.mappingMemoryName)
.providerSegments().withName(query.agent)
.entities().all()
- .snapshots().timeRange(
- IceUtil::Time::microSeconds(query.timeRange.min),
- IceUtil::Time::microSeconds(query.timeRange.max));
+ .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
// clang-format on
}
else
@@ -82,9 +108,7 @@ namespace armarx::armem
.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));
+ .snapshots().timeRange(query.timeRange.min,query.timeRange.max);
// clang-format on
}
@@ -92,6 +116,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 arondto::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<arondto::LaserScanStamped>(entityInstance);
+
+ if (o)
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ }
+ }
+ }
+
+ return outV;
+ }
+
MappingDataReader::Result
MappingDataReader::queryData(const Query& query) const
{
@@ -120,18 +197,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 3c700703ecc40708922580070d735ae9c3aaaffa..359b88fe8550d9b64f066fe942a5c6fda7dce172 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
@@ -40,8 +45,8 @@ namespace armarx::armem
struct TimeRange
{
- std::int64_t min;
- std::int64_t max;
+ Time min;
+ Time max;
};
/**
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
index 37f85182fd7339d013a6611d34157bc1d2c2f3dc..dd0299ece229eaac2fb4f44758fae23f763f612d 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 = 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);
+ arondto::LaserScanStamped aronSensorData;
+ // currently only sets the header
+ toAron(laserScan, iceTimestamp, 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 140e2aab83c6d8368ea39d1c2a031bce31f0ccd4..659df4a1536b0f81b81ba64f77e4049ba42fdd5c 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
@@ -8,22 +8,7 @@
<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'>
+ <Object name='armarx::arondto::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
</ObjectChild>
@@ -41,7 +26,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::aron::SensorHeader">
+ <Object name="armarx::arondto::SensorHeader">
<ObjectChild key="agent">
<string/>
</ObjectChild>
@@ -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'>
+ <Object name='armarx::arondto::LaserScanStamped'>
<ObjectChild key="header">
- <armarx::aron::SensorHeader />
+ <armarx::arondto::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 a7b8277c3783e518d10f76117b7a80311e1396ea..40471a632c66708ecb8085ea63a65acf6013d5a2 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
@@ -1,71 +1,51 @@
#include "aron_conversions.h"
#include <algorithm>
+#include <cstdint>
#include <iterator>
#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)
+ armem::Time timeFromAron(const int64_t timestamp)
{
- 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;
+ return armem::Time::microSeconds(timestamp);
}
- auto fromAron(const aron::LaserScan& aronLaserScan)
- {
- return fromAron(aronLaserScan.scan);
- }
-
- SensorHeader fromAron(const aron::SensorHeader& aronSensorHeader)
+ SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
{
+
return {.agent = aronSensorHeader.agent,
.frame = aronSensorHeader.frame,
- .timestamp = aronSensorHeader.timestamp};
+ .timestamp = timeFromAron(aronSensorHeader.timestamp)};
}
- void fromAron(const aron::LaserScanStamped& aronLaserScan,
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
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,
+ 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);
+ // laserScan = fromAron(aronLaserScan.data);
- timestamp = header.timestamp;
+ timestamp = header.timestamp.toMicroSeconds();
frame = header.frame;
agentName = header.agent;
}
@@ -74,58 +54,39 @@ 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)
+ int64_t toAron(const armem::Time& timestamp)
{
- aronLaserScan.scan = toAron(laserScan);
+ return timestamp.toMicroSeconds();
}
- aron::SensorHeader toAron(const SensorHeader& sensorHeader)
+ arondto::SensorHeader toAron(const SensorHeader& sensorHeader)
{
- aron::SensorHeader aronSensorHeader;
+ arondto::SensorHeader aronSensorHeader;
aronSensorHeader.agent = sensorHeader.agent;
aronSensorHeader.frame = sensorHeader.frame;
- aronSensorHeader.timestamp = sensorHeader.timestamp;
+ aronSensorHeader.timestamp = toAron(sensorHeader.timestamp);
return aronSensorHeader;
}
void toAron(const LaserScanStamped& laserScanStamped,
- aron::LaserScanStamped& aronLaserScanStamped)
+ arondto::LaserScanStamped& aronLaserScanStamped)
{
aronLaserScanStamped.header = toAron(laserScanStamped.header);
- toAron(laserScanStamped.data, aronLaserScanStamped.data);
+ // toAron(laserScanStamped.data, aronLaserScanStamped.data);
}
void toAron(const LaserScan& laserScan,
- const std::int64_t& timestamp,
+ const armem::Time& timestamp,
const std::string& frame,
const std::string& agentName,
- aron::LaserScanStamped& aronLaserScanStamped)
+ arondto::LaserScanStamped& aronLaserScanStamped)
{
const SensorHeader header
{
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
index 865e054b1fafca21516548e995133592a9991963..7e0d690a48da10f3d07290f6bf32281251a877e5 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,12 +23,14 @@
#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>
+#include <RobotAPI/libraries/armem/core/Time.h>
namespace armarx
{
- namespace aron
+ namespace arondto
{
struct LaserScanStamped;
} // namespace aron
@@ -38,19 +39,32 @@ namespace armarx
struct LaserScanStamped;
void fromAron(
- const aron::LaserScanStamped& aronLaserScan,
+ const arondto::LaserScanStamped& aronLaserScan,
LaserScan& laserScan,
std::int64_t& timestamp,
std::string& frame,
std::string& agentName);
- void fromAron(const aron::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
+
+ template<typename T>
+ auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
+ {
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
void toAron(
const LaserScan& laserScan,
- const std::int64_t& timestamp,
+ const armem::Time& timestamp,
const std::string& frame,
const std::string& agentName,
- aron::LaserScanStamped& aronLaserScan);
+ arondto::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 f3bfa33b76a5201465ce0bcccc3d53af9d284c91..d822597e103177b0833014d25f0530e25f2b2075 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
@@ -22,6 +21,7 @@
#pragma once
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
namespace armarx
@@ -31,7 +31,7 @@ namespace armarx
{
std::string agent;
std::string frame;
- std::int64_t timestamp;
+ armem::Time timestamp;
};
struct LaserScanStamped
diff --git a/source/RobotAPI/libraries/aron/converter/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
index 637345f2b3a4792f269dfd1cee0aed7861db323c..a99bfccfd49d736dc8d4c1ff55e41decb06029ee 100644
--- a/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
@@ -1,4 +1,19 @@
+add_subdirectory(common)
add_subdirectory(ivt)
add_subdirectory(pcl)
add_subdirectory(eigen)
add_subdirectory(opencv)
+
+
+add_library(AronConverter INTERFACE)
+
+target_link_libraries(AronConverter
+ INTERFACE
+ RobotAPI::aron::converter::common
+ RobotAPI::aron::converter::ivt
+ RobotAPI::aron::converter::pcl
+ RobotAPI::aron::converter::eigen
+ RobotAPI::aron::converter::opencv
+)
+
+add_library(RobotAPI::aron::converter ALIAS AronConverter)
diff --git a/source/RobotAPI/libraries/aron/converter/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/common/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ad1ddc3669b4672d4d9a2f9def8cb4643b8d6821
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/CMakeLists.txt
@@ -0,0 +1,23 @@
+set(LIB_NAME aroncommonconverter)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ aron
+)
+
+set(LIB_FILES
+ Converter.cpp
+ VectorConverter.cpp
+)
+
+set(LIB_HEADERS
+ Converter.h
+ VectorConverter.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+
+add_library(RobotAPI::aron::converter::common ALIAS aroncommonconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/common/Converter.cpp b/source/RobotAPI/libraries/aron/converter/common/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5161e17c00a2ab5dbe0d893a83ef29d46ac7da9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/Converter.cpp
@@ -0,0 +1 @@
+#include "Converter.h"
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron/converter/common/Converter.h b/source/RobotAPI/libraries/aron/converter/common/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..570344ab618f8a353327a716aaa9ba5f850eaca8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/Converter.h
@@ -0,0 +1,71 @@
+/*
+ * 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 <algorithm>
+#include <vector>
+
+namespace armarx::aron
+{
+
+ /**
+ * @brief Converter function for vector of aron elements to plain cpp type
+ *
+ * You have to provide a converter function for the element with the signature
+ *
+ * PlainCppType fromAron(const AronType&)
+ *
+ * @tparam T the aron vector element
+ * @param v the vector of elements
+ * @return the vector of aron elements
+ */
+ template <typename T>
+ auto fromAron(const std::vector<T>& v) -> std::vector<decltype(fromAron(T()))>
+ {
+ 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;
+ }
+
+
+ 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;
+ }
+
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.cpp b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f5406cc2be80d443623d0d406865b89279ad00e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.cpp
@@ -0,0 +1 @@
+#include "VectorConverter.h"
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..17475a3b9df0f39ee10e82443f24e5251db63c58
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
@@ -0,0 +1,69 @@
+/*
+ * 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
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <numeric>
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+
+namespace armarx::aron::converter
+{
+ class AronVectorConverter
+ {
+ public:
+ AronVectorConverter() = delete;
+
+ template<typename T>
+ static std::vector<T> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav)
+ {
+ ARMARX_CHECK_NOT_NULL(nav);
+
+ const auto& dims = nav->getDimensions();
+
+ if(dims.size() != 2)
+ {
+ throw error::AronException("AronVectorConverter", "ConvertToVector", "The NDArray must have two dimensions.", nav->getPath());
+ }
+
+ if(dims.at(1) != sizeof(T))
+ {
+ throw error::AronException("AronVectorConverter", "ConvertToVector", "Dimension 1 of the array has to match the element size.", nav->getPath());
+ }
+
+ const int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>());
+
+ std::vector<T> v(dims.at(0));
+ memcpy(v.data(), nav->getData(), size);
+
+ return v;
+ }
+ };
+
+} // namespace armarx::aron::converter
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt
index 44b70e2e88ffe15465aa558cb153d491f8ce62c4..d4f9008ea45cfb34d3a6056e2155907060e672f0 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt
@@ -8,6 +8,7 @@ armarx_build_if(Eigen3_FOUND "Eigen3 not available")
set(LIBS
aron
+ Eigen3::Eigen
)
set(LIB_FILES
@@ -20,6 +21,4 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-if(Eigen3_FOUND)
- include_directories(${Eigen3_INCLUDE_DIR})
-endif()
+add_library(RobotAPI::aron::converter::eigen ALIAS aroneigenconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
index a7651058d79736edb0797659365ddfc4b62782c6..376a1ae7bcbc17edf348c03d40b7c05602751e04 100644
--- a/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
@@ -7,7 +7,9 @@ find_package(IVT COMPONENTS ivt ivtopencv QUIET)
armarx_build_if(IVT_FOUND "IVT not available")
set(LIBS
- aron ivt ivtopencv
+ aron
+ ivt
+ ivtopencv
)
set(LIB_FILES
@@ -21,5 +23,10 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(IVT_FOUND)
- include_directories(${IVT_INCLUDE_DIRS})
+ target_include_directories(aronivtconverter
+ SYSTEM PUBLIC
+ ${IVT_INCLUDE_DIRS}
+ )
endif()
+
+add_library(RobotAPI::aron::converter::ivt ALIAS aronivtconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
index 1b54988b28c2e024b6035a8f722e2fe184ec78bf..5cbe0acad210ec314726aa204e59cefcb9fe23af 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
@@ -7,7 +7,8 @@ find_package(OpenCV QUIET)
armarx_build_if(OpenCV_FOUND "OpenCV not available")
set(LIBS
- aron ${OpenCV_LIBRARIES}
+ aron
+ ${OpenCV_LIBRARIES}
)
set(LIB_FILES
@@ -21,5 +22,11 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(OpenCV_FOUND)
- include_directories(${OpenCV_INCLUDE_DIRS})
+ target_include_directories(aronopencvconverter
+ SYSTEM PUBLIC
+ ${OpenCV_INCLUDE_DIRS}
+ )
endif()
+
+
+add_library(RobotAPI::aron::converter::opencv ALIAS aronopencvconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt
index 0a1c4532df8ad750b94056911c3cfd157ff937f4..c14d50a5f6f91db9b36088845b63d14ed217a6a4 100644
--- a/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt
@@ -7,7 +7,8 @@ find_package(PCL QUIET)
armarx_build_if(PCL_FOUND "PCL not available")
set(LIBS
- aron ${PCL_COMMON_LIBRARIES}
+ aron
+ ${PCL_COMMON_LIBRARIES}
)
set(LIB_FILES
@@ -21,5 +22,10 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(PCL_FOUND)
- include_directories(${PCL_INCLUDE_DIRS})
+ target_include_directories(aronpclconverter
+ SYSTEM PUBLIC
+ ${PCL_INCLUDE_DIRS}
+ )
endif()
+
+add_library(RobotAPI::aron::converter::pcl ALIAS aronpclconverter)
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 6a81faa502be6c0a667d66949d5bfa67624900c7..c2afd4f51bc1c18dd1c509a7328f31720eb519bf 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -8,9 +8,6 @@ armarx_build_if(Eigen3_FOUND "Eigen3 not available")
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
-if(Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIR})
-endif()
find_package(PCL QUIET COMPONENTS io common)
armarx_build_if(PCL_FOUND "PCL not available")
@@ -21,11 +18,11 @@ set(LIBS
ArmarXCore
RobotAPIInterfaces
cppgen
- SimoxUtility
- #${Simox_LIBS}
+ Simox::SimoxUtility
+
# System libraries
- #Eigen3::Eigen
+ Eigen3::Eigen
# PCLInterface
)
@@ -227,8 +224,8 @@ armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(PCL_FOUND)
-target_include_directories("${LIB_NAME}"
- SYSTEM PUBLIC
+target_include_directories("${LIB_NAME}"
+ SYSTEM PUBLIC
"${PCL_INCLUDE_DIRS}"
)
endif()
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
index 1d4c51c5480309d096420ef8af6e8af2176c428e..cd14f9221dce3541d1f2f2e59720077a8caed805 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
@@ -24,8 +24,6 @@
// Header
#include "PCLPointCloud.h"
-#include <RobotAPI/libraries/core/pcl_point_types.h>
-
namespace armarx::aron::cppcodegenerator::serializer
{
@@ -38,8 +36,7 @@ namespace armarx::aron::cppcodegenerator::serializer
{"PointXYZRGB", {"pcl::PointXYZRGB", 32}},
{"PointXYZRGBL", {"pcl::PointXYZRGBL", 32}},
{"PointXYZRGBA", {"pcl::PointXYZRGBA", 32}},
- {"PointXYZHSV", {"pcl::PointXYZHSV", 32}},
- {"PointPolar", {"pcl::PointPolar", 16}},
+ {"PointXYZHSV", {"pcl::PointXYZHSV", 32}}
};
// constructors
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
index bdd6a0c20e87ed893627f3f75e7ffbde7b84b689..6806d9dd41abe290d246d7a66a036f39a499461a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
@@ -243,4 +243,5 @@ case type::Descriptor::e##upperType: \
{
return 0;
}
-}
+
+} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
index 60d8361038c5d0aeba9f9d902f038dfd0386a96f..5e447b102d889f2ba91d95e23a60a89028cf1ff4 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
@@ -24,16 +24,22 @@
#pragma once
// STD/STL
+#include <cstddef>
+#include <functional>
#include <memory>
#include <map>
+#include <numeric>
+#include <vector>
// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
namespace armarx::aron::datanavigator
{
class NDArrayNavigator;
- typedef std::shared_ptr<NDArrayNavigator> NDArrayNavigatorPtr;
+ using NDArrayNavigatorPtr = std::shared_ptr<NDArrayNavigator>;
class NDArrayNavigator :
virtual public Navigator
@@ -41,7 +47,6 @@ namespace armarx::aron::datanavigator
public:
using PointerType = NDArrayNavigatorPtr;
- public:
// constructors
NDArrayNavigator(const Path& path = Path());
NDArrayNavigator(const data::AronNDArrayPtr&, const Path& path = Path());
@@ -63,11 +68,31 @@ namespace armarx::aron::datanavigator
/// Return dimensions in a readable string such as "(2, 3, 4)".
static std::string DimensionsToString(const std::vector<int>& dimensions);
+ // TODO(fabian.reister): move this to VectorConverter?
+ template<typename T>
+ static NDArrayNavigatorPtr FromVector(const std::vector<T>& data)
+ {
+ NDArrayNavigatorPtr ndArr(new NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(data.size()), sizeof(T)});
+ ndArr->setData(data);
+
+ return ndArr;
+ }
+
// public member functions
unsigned char* getData() const;
void setData(unsigned int, const unsigned char*);
+ // TODO(fabian.reister): move this to VectorConverter?
+ template<typename T>
+ void setData(const std::vector<T>& data)
+ {
+ using E = typename decltype(aron->data)::value_type;
+ setData(sizeof(T) * data.size(), reinterpret_cast < const E* >(data.data()));
+ }
+
std::vector<int> getDimensions() const;
void setDimensions(const std::vector<int>&);
void addDimension(int);
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
index a5e7f249f322d246cc3ce5af1a2f510904b472f6..2f62f1b579b5e84b3acc93aa7a72d70329492736 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
@@ -79,8 +79,7 @@ namespace armarx::aron::typenavigator
{"PointXYZRGB", {"pcl::PointXYZRGB"}},
{"PointXYZRGBL", {"pcl::PointXYZRGBL"}},
{"PointXYZRGBA", {"pcl::PointXYZRGBA"}},
- {"PointXYZHSV", {"pcl::PointXYZHSV"}},
- {"PointPolar", {"pcl::PointPolar"}}
+ {"PointXYZHSV", {"pcl::PointXYZHSV"}}
};
const unsigned int ACCEPTED_DIMENSION_MIN_SIZE = 2;
const unsigned int ACCEPTED_DIMENSION_MAX_SIZE = 2;
diff --git a/source/RobotAPI/libraries/core/pcl_point_types.h b/source/RobotAPI/libraries/core/pcl_point_types.h
deleted file mode 100644
index e610f72fed7ac5051c8ec43a842b2f60e811281d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/core/pcl_point_types.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * 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::Core
- * @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
-
-#define PCL_NO_PRECOMPILE
-#include <pcl/pcl_macros.h>
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-#include <pcl/io/pcd_io.h>
-// #include <pcl/memory.h>
-
-#include <Eigen/Core>
-
-namespace pcl
-{
-
- /**
- * @brief Point for polar coordinates
- *
- */
- struct PointPolar
- {
- // PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding
-
- //! radial distance
- float r;
-
- //! angle
- float phi;
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- // PCL_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
- } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
-
-
-
-} // namespace pcl
-
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointPolar,
- (float, r, r)
- (float, phi, phi)
- )
\ No newline at end of file