Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
#include <sstream>
 
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.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/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.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