Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
#include <sstream>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/PluginAll.h>
 
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
 
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
 
 
namespace armarx::armem::server::obj::attachments
{
    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        iceMemory(memoryToIceAdapter)
    {
        Logging::setTag("Attachments");
    }
 
    Segment::~Segment() = default;
 
    void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
        defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
    }
 
    void Segment::init()
    {
        ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
 
        coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::Robot::ToAronType());
        coreSegment->setMaxHistorySize(p.maxHistorySize);
    }
 
    void Segment::connect()
    {
    }
 
    std::vector<armarx::armem::attachment::ObjectAttachment>
    Segment::getAttachments(const armem::Time& timestamp) const
    {
        ARMARX_CHECK_NOT_NULL(coreSegment);
        return coreSegment->doLocked([this]()
        {
            std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
            coreSegment->forEachEntity([this, &attachments](const wm::Entity & entity)
            {
                const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
                const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
                if (not aronAttachment)
                {
                    ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
                    return true;
                }
 
                ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
 
                armarx::armem::attachment::ObjectAttachment attachment;
                fromAron(*aronAttachment, attachment);
 
                attachments.push_back(attachment);
                return true;
            });
 
            return attachments;
        });
    }
 
}  // namespace armarx::armem::server::obj::attachments