Segment.cpp

#include "Segment.h"

#include <sstream>

#include <ArmarXCore/core/time/TimeUtil.h>
#include "ArmarXCore/core/logging/Logging.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/workingmemory/Visitor.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_objects/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, std::mutex& memoryMutex) :
        iceMemory(memoryToIceAdapter),
        memoryMutex(memoryMutex)
    {
        Logging::setTag("Attachments");
    }

    Segment::~Segment() = default;

    void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(p.coreClassSegmentName, 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.coreClassSegmentName, arondto::Robot::toInitialAronType());
        coreSegment->setMaxHistorySize(p.maxHistorySize);
    }

    void Segment::connect(viz::Client arviz)
    {
        // this->visu = std::make_unique<Visu>(arviz, *this);
    }

    std::vector<armarx::armem::attachments::ObjectAttachment> Segment::getAttachments(const armem::Time& timestamp) const
    {
        std::vector<armarx::armem::attachments::ObjectAttachment> attachments;

        for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
        {
            for (const auto& [name, entity] :  provSeg.entities())
            {
                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
                const auto attachment = tryCast<armarx::arondto::attachment::ObjectAttachment>(entityInstance);

                if (not attachment)
                {
                    ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
                    continue;
                }

                ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
                attachments.push_back(attachment);
            }
        }

        return attachments;
    }


    // void Segment::RemoteGui::setup(const Segment& data)
    // {
    //     using namespace armarx::RemoteGui::Client;

    //     maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
    //     maxHistorySize.setRange(1, 1e6);
    //     infiniteHistory.setValue(data.p.maxHistorySize == -1);
    //     discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);

    //     GridLayout grid;
    //     int row = 0;
    //     grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
    //     row++;
    //     grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
    //     row++;
    //     grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
    //     row++;

    //     group.setLabel("Data");
    //     group.addChild(grid);
    // }

    // void Segment::RemoteGui::update(Segment& data)
    // {
    //     if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
    //         || discardSnapshotsWhileAttached.hasValueChanged())
    //     {
    //         std::scoped_lock lock(data.memoryMutex);

    //         if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
    //         {
    //             data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
    //             if (data.coreSegment)
    //             {
    //                 data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
    //             }
    //         }

    //         data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
    //     }
    // }

}  // namespace armarx::armem::server::obj::attachments