Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
// STD / STL
#include <algorithm>
#include <cstring>
#include <map>
#include <optional>
#include <ostream>
#include <type_traits>
#include <utility>
#include <vector>
 
// 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/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.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/data/variant/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/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
#include <RobotAPI/libraries/armem_vision/types.h>
 
namespace armarx::armem::vision::occupancy_grid::client
{
    Reader::~Reader() = default;
 
    armarx::armem::client::query::Builder
    Reader::buildQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties().coreSegmentName)
        .providerSegments().withName(query.providerName)
        .entities().all()
        .snapshots().beforeOrAtTime(query.timestamp);
        // clang-format on
 
        return qb;
    }
 
    std::string
    Reader::propertyPrefix() const
    {
        return "mem.vision.occupancy_grid.";
    }
 
    armarx::armem::client::util::SimpleReaderBase::Properties
    Reader::defaultProperties() const
    {
        return {.memoryName = "Vision", .coreSegmentName = "OccupancyGrid"};
    }
 
    OccupancyGrid
    asOccupancyGrid(const wm::ProviderSegment& providerSegment)
    {
        ARMARX_CHECK(not providerSegment.empty()) << "No entities";
        ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
 
        const wm::EntityInstance* entityInstance = nullptr;
        providerSegment.forEachEntity(
            [&entityInstance](const wm::Entity& entity)
            {
                const auto& entitySnapshot = entity.getLatestSnapshot();
                ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
 
                entityInstance = &entitySnapshot.getInstance(0);
                return false;
            });
        ARMARX_CHECK_NOT_NULL(entityInstance);
 
        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::data::NDArray::DynamicCast(entityInstance->data()->getElement("grid"));
        ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
 
        occupancyGrid.grid =
            aron::data::converter::AronEigenConverter::ConvertToArray<float>(*ndArrayNavigator);
 
        return occupancyGrid;
    }
 
    Reader::Result
    Reader::query(const Query& query) const
    {
        const auto qb = buildQuery(query);
 
        ARMARX_DEBUG << "[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};
        }
 
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        if (not coreSegment.hasProviderSegment(query.providerName))
        {
            ARMARX_WARNING << "Provider segment `" << query.providerName
                           << "` does not exist (yet).";
            return {.occupancyGrid = std::nullopt, .status = Result::Status::NoData};
        }
 
        const wm::ProviderSegment& providerSegment =
            coreSegment.getProviderSegment(query.providerName);
 
        if (providerSegment.empty())
        {
            ARMARX_WARNING << "No entities.";
            return {.occupancyGrid = std::nullopt,
                    .status = Result::Status::NoData,
                    .errorMessage = "No entities"};
        }
 
        try
        {
            const auto occupancyGrid = asOccupancyGrid(providerSegment);
            return Result{.occupancyGrid = occupancyGrid, .status = Result::Status::Success};
        }
        catch (...)
        {
            return Result{.status = Result::Status::Error,
                          .errorMessage = GetHandledExceptionString()};
        }
    }
 
} // namespace armarx::armem::vision::occupancy_grid::client