occupancy grid reader

source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp

+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <utility>
+#include <vector>
+
+#include <type_traits>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include "RobotAPI/libraries/aron/converter/eigen/EigenConverter.h"
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+// RobotAPI Aron
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+// RobotAPI Armem
+#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/Entity.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/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+    armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
+    {
+        armarx::armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties().memoryName)
+        .providerSegments().withName(query.providerName)
+        .entities().all()
+        .snapshots().beforeOrAtTime(query.timestamp);
+        // clang-format on
+
+        return qb;
+    }
+
+    OccupancyGrid asOccupancyGrid(const std::map<std::string, wm::Entity>& entities)
+    {
+        ARMARX_CHECK(not entities.empty()) << "No entities";
+        ARMARX_CHECK(entities.size() == 1) << "There should be only one entity!";
+
+        const wm::Entity& entity = entities.begin()->second;
+        ARMARX_CHECK(not entity.empty()) << "No snapshots";
+
+        const auto& entitySnapshot = entity.getLatestSnapshot();
+        ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+        const auto& entityInstance = entitySnapshot.instances().front();
+
+        const auto aronDto = tryCast<arondto::OccupancyGrid>(entityInstance);
+        ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
+
+        OccupancyGrid occupancyGrid;
+        fromAron(*aronDto, occupancyGrid);
+
+        // direct access to grid data
+        const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(
+                                          entityInstance.data()->getElement("grid"));
+        ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+        occupancyGrid.grid = aron::converter::AronEigenConverter::ConvertToArray<float>(*ndArrayNavigator);
+
+        return occupancyGrid;
+    }
+
+    Reader::Result Reader::query(const Query& query) const
+    {
+        const auto qb = buildQuery(query);
+
+        ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+
+        const armem::client::QueryResult qResult =
+            memoryReader().query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+        if (not qResult.success)
+        {
+            ARMARX_WARNING << "Failed to query data from memory: "
+                           << qResult.errorMessage;
+            return {.occupancyGrid = std::nullopt,
+                    .status        = Result::Status::Error,
+                    .errorMessage  = qResult.errorMessage};
+        }
+
+        // now create result from memory
+        const auto& entities = qResult.memory.getCoreSegment(properties().memoryName)
+                               .getProviderSegment(query.providerName)
+                               .entities();
+
+        if (entities.empty())
+        {
+            ARMARX_WARNING << "No entities.";
+            return {.occupancyGrid = std::nullopt,
+                    .status        = Result::Status::NoData,
+                    .errorMessage  = "No entities"};
+        }
+
+        try
+        {
+            const auto occupancyGrid = asOccupancyGrid(entities);
+            return Result{.occupancyGrid = occupancyGrid,
+                          .status        = Result::Status::Success};
+        }
+        catch (...)
+        {
+            return Result{.status       = Result::Status::Error,
+                          .errorMessage = GetHandledExceptionString()};
+        }
+    }
+
+} // namespace armarx::armem::vision::occupancy_grid::client

source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h

+/*
+ * 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 <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleReaderBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+    class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+    {
+    public:
+        using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+        ~Reader() override;
+
+        struct Query
+        {
+            std::string providerName;
+            armem::Time timestamp;
+        };
+
+        struct Result
+        {
+            std::optional<OccupancyGrid> occupancyGrid = std::nullopt;
+
+            enum Status
+            {
+                Success,
+                NoData,
+                Error
+            } status;
+
+            std::string errorMessage = "";
+
+            operator bool() const noexcept
+            {
+                return status == Status::Success;
+            }
+        };
+
+        Result query(const Query& query) const;
+
+        ::armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+    protected:
+        std::string propertyPrefix() const override;
+        Properties defaultProperties() const override;
+    };
+
+} // namespace armarx::armem::vision::occupancy_grid::client