ArMemToPointCloudProvider.cpp

Go to the documentation of this file.

#include "ArMemToPointCloudProvider.h"
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/error.h>
 
#include <VisionX/libraries/PointCloudTools/FramedPointCloud.h>
 
namespace visionx
{
    ArMemToPointCloudProvider::ArMemToPointCloudProvider()
    {
        addPlugin(virtualRobotReaderPlugin);
    }
 
    std::string
    ArMemToPointCloudProvider::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    ArMemToPointCloudProvider::GetDefaultName()
    {
        return "ArMemToPointCloudProvider";
    }
 
    armarx::PropertyDefinitionsPtr
    ArMemToPointCloudProvider::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new visionx::PointCloudProviderPropertyDefinitions(getConfigIdentifier());
 
        defs->optional(p.memoryName, "mem.01_MemoryName", "Name of the memory.");
        defs->optional(
            p.coreSegmentName,
            "mem.02_CoreSegmentName",
            "Name of the core segment. If empty or (auto), this is derived from the point type.");
        defs->optional(p.providerSegmentName,
                       "mem.03_ProviderSegmentName",
                       "Name of the provider segment(s).");
        defs->optional(p.entityName, "mem.04_EntityName", "Name of the entity.");
 
        defs->optional(p.pointtype, "pc.01_PointType", "The type of the point cloud.");
        defs->optional(p.width, "pc.02_Width", "Width of the buffer.");
        defs->optional(p.height, "pc.03_Height", "Height of the buffer.");
        defs->optional(p.elementPath,
                       "pc.04_ElementPath",
                       "Path inside the ARON structure to the point cloud element (delimited by '" +
                           p.elementPathDelimiter + "').");
        defs->optional(p.robotName, "p.robotName", "Name of the robot.");
 
 
        defs->optional(p.convertToGlobalFrame, "p.convertToGlobalFrame", "");
 
        return defs;
    }
 
    void
    ArMemToPointCloudProvider::onInitPointCloudProvider()
    {
        memoryNameSystem().setComponent(this);
 
        switch (visionx::armem::pointcloud::String2PointType.at(simox::alg::to_lower(p.pointtype)))
        {
            case visionx::armem::pointcloud::PointType::PointXYZ:
                registerToUpdates<pcl::PointXYZ>();
                break;
            case visionx::armem::pointcloud::PointType::PointXYZL:
                registerToUpdates<pcl::PointXYZL>();
                break;
            case visionx::armem::pointcloud::PointType::PointXYZRGBA:
                registerToUpdates<pcl::PointXYZRGBA>();
                break;
            case visionx::armem::pointcloud::PointType::PointXYZRGBL:
                registerToUpdates<pcl::PointXYZRGBL>();
                break;
        }
    }
 
    void
    ArMemToPointCloudProvider::onConnectPointCloudProvider()
    {
        try
        {
            memoryReader = memoryNameSystem().useReader(armarx::armem::MemoryID(p.memoryName, ""));
        }
        catch (const armarx::armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
        }
 
        pollingTask = new armarx::SimpleRunningTask<>([this]() { this->runPolling(); });
        pollingTask->start();
    }
 
    void
    ArMemToPointCloudProvider::runPolling()
    {
        while (true)
        {
            ARMARX_INFO << "Processing initial point clouds";
            processUpdates(armarx::armem::MemoryID(), {});
            ARMARX_INFO << "Done processing initial point clouds";
            armarx::Clock::WaitFor(armarx::Duration::Seconds(1));
        }
    }
 
    void
    ArMemToPointCloudProvider::onExitPointCloudProvider()
    {
    }
 
    void
    ArMemToPointCloudProvider::processUpdates(
        const armarx::armem::MemoryID& entityID,
        const std::vector<armarx::armem::MemoryID>& updatedSnapshotIDs)
    {
        // Discard everything in between
        if (not processing.exchange(true))
        {
            ARMARX_INFO << deactivateSpam() << "Processing update and provide pointcloud...";
 
            switch (
                visionx::armem::pointcloud::String2PointType.at(simox::alg::to_lower(p.pointtype)))
            {
                case visionx::armem::pointcloud::PointType::PointXYZ:
                    queryAndProvide<pcl::PointXYZ, arondto::PointCloudXYZ>();
                    break;
                case visionx::armem::pointcloud::PointType::PointXYZL:
                    queryAndProvide<pcl::PointXYZL, arondto::PointCloudXYZL>();
                    break;
                case visionx::armem::pointcloud::PointType::PointXYZRGBA:
                    queryAndProvide<pcl::PointXYZRGBA, arondto::PointCloudXYZRGBA>();
                    break;
                case visionx::armem::pointcloud::PointType::PointXYZRGBL:
                    queryAndProvide<pcl::PointXYZRGBL, arondto::PointCloudXYZRGBL>();
                    break;
            }
 
            processing = false;
        }
    }
 
    MetaPointCloudFormatPtr
    ArMemToPointCloudProvider::getDefaultPointCloudFormat()
    {
        // Copied from AK PointCloudProvider
        MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
 
        switch (visionx::armem::pointcloud::String2PointType.at(simox::alg::to_lower(p.pointtype)))
        {
            case visionx::armem::pointcloud::PointType::PointXYZ:
                info->type = PointContentType::ePoints;
                info->capacity = p.width * p.height * sizeof(Point3D);
                break;
            case visionx::armem::pointcloud::PointType::PointXYZL:
                info->type = PointContentType::eLabeledPoints;
                info->capacity = p.width * p.height * sizeof(LabeledPoint3D);
                break;
            case visionx::armem::pointcloud::PointType::PointXYZRGBA:
                info->type = PointContentType::eColoredPoints;
                info->capacity = p.width * p.height * sizeof(ColoredPoint3D);
                break;
            case visionx::armem::pointcloud::PointType::PointXYZRGBL:
                info->type = PointContentType::eColoredLabeledPoints;
                info->capacity = p.width * p.height * sizeof(ColoredLabeledPoint3D);
                break;
        }
 
        //info->frameId = getProperty<std::string>("frameId").getValue();
        info->size = info->capacity;
        return info;
    }
 
    template <class PointT>
    void
    ArMemToPointCloudProvider::registerToUpdates()
    {
        // Connect to memory updates.
        auto callback = [this](const armarx::armem::MemoryID& entityID,
                               const std::vector<armarx::armem::MemoryID>& updatedSnapshotIDs)
        { this->processUpdates(entityID, updatedSnapshotIDs); };
        armarx::armem::MemoryID listenTo = armarx::armem::MemoryID(
            p.memoryName, p.getCoreSegmentName<PointT>(), p.providerSegmentName, p.entityName);
        ARMARX_INFO << "Listening to: " << listenTo.str();
        memoryNameSystem().subscribe(listenTo, callback);
    }
 
    template <class PointT, class AronT>
    void
    ArMemToPointCloudProvider::queryAndProvide()
    {
        // query latest pc from memory
        armarx::armem::MemoryID toQuery(
            p.memoryName, p.getCoreSegmentName<PointT>(), p.providerSegmentName, p.entityName);
        std::optional<armarx::armem::wm::EntitySnapshot> data =
            memoryReader.getLatestSnapshotIn(toQuery);
 
        if (data.has_value())
        {
            ARMARX_CHECK(data->size() == 1);
 
            armarx::aron::data::DictPtr aron = data->getInstance(0).data();
            const armarx::aron::Path elementPath = armarx::aron::Path::FromString(
                p.elementPath, armarx::aron::Path::DEFAULT_ROOT_IDENTIFIER, p.elementPathDelimiter);
 
            armarx::aron::data::VariantPtr aronPointCloud = aron->navigateRelative(elementPath);
            ARMARX_CHECK_NOT_NULL(aronPointCloud);
 
            armarx::aron::data::DictPtr aronAdapted = armarx::aron::make_dict();
            aronAdapted->setElement("pointcloud", aronPointCloud);
 
            AronT arondto = AronT::FromAron(aronAdapted);
 
            typename pcl::PointCloud<PointT>::Ptr toProvide(new pcl::PointCloud<PointT>());
            *toProvide = arondto.pointcloud;
 
            if (p.convertToGlobalFrame)
            {
 
                ARMARX_INFO << "Source frame " << p.frame;
 
                ARMARX_INFO << deactivateSpam(10) << "Converting to global frame";
                const auto robot =
                    virtualRobotReaderPlugin->get().getSynchronizedRobot(p.robotName);
                visionx::transformPointCloudFromTo<PointT>(
                    *toProvide, *toProvide, p.frame, armarx::GlobalFrame, robot);
            }
 
            ARMARX_VERBOSE << "Providing point cloud with " << toProvide->points.size()
                           << " points.";
 
 
            providePointCloud(toProvide);
        }
    }
 
    template <class PointT>
    std::string
    ArMemToPointCloudProvider::Properties::getCoreSegmentName() const
    {
        if (coreSegmentName.empty() or coreSegmentName == "(auto)")
        {
            return visionx::armem::pointcloud::GetCoreSegmentNameFor<PointT>();
        }
        else
        {
            return coreSegmentName;
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::visionx::ArMemToPointCloudProvider,
                                         ::visionx::ArMemToPointCloudProvider::GetDefaultName());
 
} // namespace visionx