diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp index 083cb5977bfb370aee5cb95cd37fb1febc58137d..24029fa4c9422bd6da0618a8717f827fb081246f 100644 --- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp +++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp @@ -6,14 +6,12 @@ #include <RobotAPI/interface/units/LaserScannerUnit.h> #include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h> +#include <RobotAPI/libraries/aron/common/aron_conversions.h> #include <RobotAPI/libraries/aron/converter/common/Converter.h> #include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h> -#include <RobotAPI/libraries/aron/common/aron_conversions.h> - #include "types.h" - namespace armarx::armem { @@ -27,8 +25,8 @@ namespace armarx::armem SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader) { - return {.agent = aronSensorHeader.agent, - .frame = aronSensorHeader.frame, + return {.agent = aronSensorHeader.agent, + .frame = aronSensorHeader.frame, .timestamp = timeFromAron(aronSensorHeader.timestamp)}; } @@ -39,8 +37,10 @@ namespace armarx::armem // laserScan.data = fromAron(aronLaserScan.data); } - void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScan& laserScan, - std::int64_t& timestamp, std::string& frame, + void fromAron(const arondto::LaserScanStamped& aronLaserScan, + LaserScan& laserScan, + std::int64_t& timestamp, + std::string& frame, std::string& agentName) { const auto header = fromAron(aronLaserScan.header); @@ -48,14 +48,12 @@ namespace armarx::armem // laserScan = fromAron(aronLaserScan.data); timestamp = header.timestamp.toMicroSeconds(); - frame = header.frame; + frame = header.frame; agentName = header.agent; } - /************ toAron ************/ - // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan) // { // aronLaserScan.scan = toAron(laserScan); @@ -70,8 +68,8 @@ namespace armarx::armem { arondto::SensorHeader aronSensorHeader; - aronSensorHeader.agent = sensorHeader.agent; - aronSensorHeader.frame = sensorHeader.frame; + aronSensorHeader.agent = sensorHeader.agent; + aronSensorHeader.frame = sensorHeader.frame; aronSensorHeader.timestamp = toAron(sensorHeader.timestamp); return aronSensorHeader; @@ -94,20 +92,21 @@ namespace armarx::armem { .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); aron::toAron(dto.resolution, bo.resolution); // bo.grid is NdArray -> need special handling. } - - void fromAron(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); @@ -115,4 +114,4 @@ namespace armarx::armem // bo.grid is NdArray -> need special handling. } -} // namespace armarx::armem \ No newline at end of file +} // namespace armarx::armem \ No newline at end of file diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h index d917caacadef7f9db4c08b11b72098c503464a89..db97ad5076457805d953100201821f912a046291 100644 --- a/source/RobotAPI/libraries/armem_vision/aron_conversions.h +++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h @@ -26,6 +26,7 @@ #include <RobotAPI/libraries/armem/core/Time.h> #include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h> #include <RobotAPI/libraries/aron/converter/common/VectorConverter.h> +#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h> #include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h> namespace armarx::armem @@ -49,7 +50,7 @@ namespace armarx::armem auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator) { return aron::converter::AronVectorConverter::ConvertToVector<T>( - navigator); + navigator); } void fromAron(const arondto::LaserScanStamped& aronLaserScan, @@ -70,12 +71,12 @@ namespace armarx::armem // OccupancyGrid void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo); - void fromAron(arondto::OccupancyGrid& dto, OccupancyGrid& bo); + void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo); inline aron::datanavigator::NDArrayNavigatorPtr toAron(const OccupancyGrid::Grid& grid) { - return aron::converter::AronVectorConverter::ConvertFromVector(grid); + return aron::converter::AronEigenConverter::ConvertFromArray(grid); } } // namespace armarx::armem \ No newline at end of file