occupancy grid conversion

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

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