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>
 
// ArmarXCore
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
 
// RobotAPI Armem
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
 
#include <armarx/navigation/memory/aron_conversions.h>
#include <armarx/navigation/memory/constants.h>
 
namespace armarx::navigation::memory::client::laser_scanner_features
{
 
    Reader::~Reader() = default;
 
    std::string
    Reader::propertyPrefix() const
    {
        return "mem.nav.laser_scanner_features.";
    }
 
    armarx::armem::client::util::SimpleReaderBase::Properties
    Reader::defaultProperties() const
    {
        return {.memoryName = memory::constants::NavigationMemoryName,
                .coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment};
    }
 
    armarx::armem::client::query::Builder
    Reader::buildQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        auto& sel = qb.coreSegments()
                        .withName(properties().coreSegmentName)
                        .providerSegments()
                        .withName(query.providerName);
 
        auto& entitySel = [&]() -> armem::client::query::EntitySelector&
        {
            if (query.name.empty())
            {
                ARMARX_DEBUG << "querying all entities";
                return sel.entities().all();
            }
            return sel.entities().withName(query.name);
        }();
 
        entitySel.snapshots().beforeOrAtTime(query.timestamp);
 
        return qb;
    }
 
    std::vector<LaserScannerFeatures>
    asFeaturesList(const armem::wm::ProviderSegment& providerSegment)
    {
        if (providerSegment.empty())
        {
            ARMARX_WARNING << "No entities!";
            return {};
        }
 
        // const auto convert = [](const auto& aronLaserScanStamped,
        //                         const wm::EntityInstance& ei) -> LaserScanStamped
        // {
        //     LaserScanStamped laserScanStamped;
        //     fromAron(aronLaserScanStamped, laserScanStamped);
 
        //     const auto ndArrayNavigator =
        //         aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
 
        //     ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
 
        //     laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
        //     ARMARX_IMPORTANT << "4";
 
        //     return laserScanStamped;
        // };
 
        std::vector<LaserScannerFeatures> laserScannerFeatures;
 
        // loop over all entities and their snapshots
        providerSegment.forEachEntity(
            [&](const armem::wm::Entity& entity)
            {
                // If we don't need this warning, we could directly iterate over the snapshots.
                if (entity.empty())
                {
                    ARMARX_WARNING << "Empty history for " << entity.id();
                }
                ARMARX_DEBUG << "History size: " << entity.size();
 
                entity.forEachInstance(
                    [&](const armem::wm::EntityInstance& entityInstance)
                    {
                        if (const auto o =
                                armem::tryCast<arondto::LaserScannerFeatures>(entityInstance))
                        {
                            LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
                            fromAron(o.value(), f);
                        }
                        return true;
                    });
                return true;
            });
 
        return laserScannerFeatures;
    }
 
    Reader::Result
    Reader::queryData(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 {.features = {},
                    .status = Result::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
        if (not coreSegment.hasProviderSegment(query.providerName))
        {
            ARMARX_DEBUG << "Provider segment `" << query.providerName
                           << "` does not exist (yet).";
            return {.features = {},
                    .status = Result::Status::Error,
                    .errorMessage = "Provider segment " + query.providerName + " does not exist"};
        }
 
        // now create result from memory
        const armem::wm::ProviderSegment& providerSegment =
            qResult.memory.getCoreSegment(properties().coreSegmentName)
                .getProviderSegment(query.providerName);
 
        const auto features = asFeaturesList(providerSegment);
 
        // const auto laserScans = asLaserScans(providerSegment);
        // std::vector<std::string> sensors;
        // providerSegment.forEachEntity(
        //     [&sensors](const wm::Entity& entity)
        //     {
        //         sensors.push_back(entity.name());
        //         return true;
        //     });
 
        return {.features = features,
                // .sensors = sensors,
                .status = Result::Status::Success,
                .errorMessage = ""};
    }
 
} // namespace armarx::navigation::memory::client::laser_scanner_features