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/local/ExpressionException.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
 
// RobotAPI Interfaces
#include "RobotAPI/libraries/armem_laser_scans/constants.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/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/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 <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
 
namespace armarx::armem::laser_scans::client
{
 
    Reader::Reader()
    {
    }
 
    Reader::~Reader() = default;
 
    void
    Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
 
        const std::string prefix = propertyPrefix;
    }
 
    void
    Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
                         << "' ...";
        try
        {
            memoryReader =
                memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
            ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName
                             << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    armarx::armem::client::query::Builder
    Reader::buildQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        ARMARX_VERBOSE << "Query for agent: " << query.agent
                       << " memory name: " << constants::memoryName;
 
        if (query.sensorList.empty()) // all sensors
        {
            // clang-format off
            auto& snapshots = qb
            .coreSegments().withName(constants::memoryName)
            .providerSegments().withName(query.agent)
            .entities().all()
            .snapshots();
            // clang-format on
            if (query.timeRange.has_value())
            {
                snapshots.timeRange(query.timeRange->min, query.timeRange->max);
            }
            else
            {
                snapshots.latest();
            }
        }
        else
        {
            // clang-format off
            auto& snapshots = qb
            .coreSegments().withName(constants::memoryName)
            .providerSegments().withName(query.agent)
            .entities().withNames(query.sensorList)
            .snapshots();
            // clang-format on
            if (query.timeRange.has_value())
            {
                snapshots.timeRange(query.timeRange->min, query.timeRange->max);
            }
            else
            {
                snapshots.latest();
            }
        }
 
        return qb;
    }
 
    std::vector<LaserScanStamped>
    asLaserScans(const wm::ProviderSegment& providerSegment)
    {
        std::vector<LaserScanStamped> outV;
        if (providerSegment.empty())
        {
            ARMARX_WARNING << "No entities!";
        }
 
        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);
 
            return laserScanStamped;
        };
 
        // loop over all entities and their snapshots
        providerSegment.forEachEntity(
            [&outV, &convert](const 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(
                    [&outV, &convert](const wm::EntityInstance& entityInstance)
                    {
                        if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
                        {
                            outV.push_back(convert(*o, entityInstance));
                        }
                        return true;
                    });
                return true;
            });
 
        return outV;
    }
 
    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 {.laserScans = {},
                    .sensors = {},
                    .status = Result::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        // now create result from memory
        const wm::ProviderSegment& providerSegment =
            qResult.memory.getCoreSegment(constants::memoryName).getProviderSegment(query.agent);
 
        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 {.laserScans = laserScans,
                .sensors = sensors,
                .status = Result::Status::Success,
                .errorMessage = ""};
    }
 
} // namespace armarx::armem::laser_scans::client