From 77f97de023ebf92d3207751076ba0f2c0d020927 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 8 Jun 2021 19:55:52 +0200
Subject: [PATCH] conversions for occupancy grid

---
 .../aron_conversions.cpp                      | 24 +++++++-
 .../aron_conversions.h                        | 57 +++++++++++--------
 2 files changed, 55 insertions(+), 26 deletions(-)
 rename source/RobotAPI/libraries/{armem_robot_mapping => armem_vision}/aron_conversions.cpp (79%)
 rename source/RobotAPI/libraries/{armem_robot_mapping => armem_vision}/aron_conversions.h (53%)

diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
similarity index 79%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
rename to source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index b37a88381..083cb5977 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -5,14 +5,16 @@
 #include <iterator>
 
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.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
+namespace armarx::armem
 {
 
     /************ fromAron ************/
@@ -96,5 +98,21 @@ namespace armarx
 
         toAron(laserScanStamped, aronLaserScanStamped);
     }
+    
+    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) 
+    {
+        aron::fromAron(dto.frame, bo.frame);
+        aron::fromAron(dto.pose, bo.pose);
+        aron::fromAron(dto.resolution, bo.resolution);
+        // bo.grid is NdArray -> need special handling.
+    }
 
-} // namespace armarx
\ No newline at end of file
+}  // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
similarity index 53%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
rename to source/RobotAPI/libraries/armem_vision/aron_conversions.h
index 7ecffc237..d917caaca 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -19,52 +19,63 @@
  *             GNU General Public License
  */
 
-
 #pragma once
 
+#include "RobotAPI/libraries/armem_vision/types.h"
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
+#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/core/navigator/data/complex/NDArray.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
 
-namespace armarx
+namespace armarx::armem
 {
 
     namespace arondto
     {
         struct LaserScanStamped;
-    } // namespace aron
+    } // namespace arondto
 
     // struct LaserScan;
     struct LaserScanStamped;
 
-    void fromAron(
-        const arondto::LaserScanStamped& aronLaserScan,
-        LaserScan& laserScan,
-        std::int64_t& timestamp,
-        std::string& frame,
-        std::string& agentName);
-
+    void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+                  LaserScan& laserScan,
+                  std::int64_t& timestamp,
+                  std::string& frame,
+                  std::string& agentName);
 
-    template<typename T>
+    template <typename T>
     auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
     {
-        return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
+        return aron::converter::AronVectorConverter::ConvertToVector<T>(
+            navigator);
     }
 
-    void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
+    void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+                  LaserScanStamped& laserScan);
 
-    void toAron(
-        const LaserScan& laserScan,
-        const armem::Time& timestamp,
-        const std::string& frame,
-        const std::string& agentName,
-        arondto::LaserScanStamped& aronLaserScan);
+    void toAron(const LaserScan& laserScan,
+                const armem::Time& timestamp,
+                const std::string& frame,
+                const std::string& agentName,
+                arondto::LaserScanStamped& aronLaserScan);
 
-    inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
+    inline aron::datanavigator::NDArrayNavigatorPtr
+    toAron(const LaserScan& laserScan)
     {
-        return aron::converter::AronVectorConverter::ConvertFromVector(laserScan);
+        using aron::converter::AronVectorConverter;
+        return AronVectorConverter::ConvertFromVector(laserScan);
     }
 
+    // OccupancyGrid
+    void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
+    void fromAron(arondto::OccupancyGrid& dto, OccupancyGrid& bo);
+
+    inline aron::datanavigator::NDArrayNavigatorPtr
+    toAron(const OccupancyGrid::Grid& grid)
+    {
+        return aron::converter::AronVectorConverter::ConvertFromVector(grid);
+    }
 
-} // namespace armarx
\ No newline at end of file
+} // namespace armarx::armem
\ No newline at end of file
-- 
GitLab