Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
#include <sstream>
 
#include <Eigen/Dense>
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
 
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.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/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx::armem::server::obj::instance
{
 
    void
    Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(robotName,
                       prefix + "robotName",
                       "Name of robot whose note can be calibrated.\n"
                       "If not given, the 'fallbackName' is used.");
        defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
        defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
    }
 
    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        SpecializedCoreSegment(memoryToIceAdapter,
                               objects::instaceSegmentID.coreSegmentName,
                               arondto::ObjectInstance::ToAronType(),
                               64)
    {
        oobbCache.setFetchFn(
            [this](const ObjectID& id) -> std::optional<simox::OrientedBoxf>
            {
                // Try to get OOBB from repository.
                if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
                {
                    try
                    {
                        objectInfo->setLogError(false); // Don't log missing files
                        return objectInfo->loadOOBB();
                    }
                    catch (const std::ios_base::failure& e)
                    {
                        // Give up - no OOBB information.
                        ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- "
                                       << e.what();
                        return std::nullopt;
                    }
                }
                else
                {
                    return std::nullopt;
                }
            });
 
        classNameToDatasetCache.setFetchFn(
            [this](const std::string& className)
            {
                std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
                return objectInfo ? objectInfo->dataset() : "";
            });
    }
 
    Segment::~Segment()
    {
    }
 
    void
    Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        SpecializedCoreSegment::defineProperties(defs, prefix);
 
        defs->optional(
            p.discardSnapshotsWhileAttached,
            prefix + "DiscardSnapshotsWhileAttached",
            "If true, no new snapshots are stored while an object is attached to a robot node.\n"
            "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
 
        defs->optional(robots.fallbackName,
                       prefix + "robots.FallbackName",
                       "Robot name to use as fallback if the robot name is not specified "
                       "in a provided object pose.");
 
        defs->optional(
            p.sceneSnapshotsPackage,
            prefix + "scene.10_Package",
            "ArmarX package containing the scene snapshots.\n"
            "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
        defs->optional(p.sceneSnapshotsDirectory,
                       prefix + "scene.11_Directory",
                       "Directory in Package/data/Package/ containing the scene snapshots.");
 
        std::vector<std::string> sceneSnapshotToLoadDescription = {
            "Scene(s) to load on startup.",
            "Specify multiple scenes in a ; separated list.",
            "Each entry must be one of the following:",
            "(1) A scene file in 'Package/scenes/' (with or without '.json' extension), "
            "e.g. 'MyScene', 'MyScene.json'",
            "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' "
            "extension), "
            "e.g. 'path/to/MyScene', 'path/to/MyScene.json'",
            "(3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'",
        };
        defs->optional(p.sceneSnapshotsToLoad,
                       prefix + "scene.12_SnapshotToLoad",
                       simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
 
        decay.defineProperties(defs, prefix + "decay.");
    }
 
    std::vector<std::string>
    Segment::Properties::getSceneSnapshotsToLoad() const
    {
        if (sceneSnapshotsToLoad.empty())
        {
            return {};
        }
        bool trim = true;
        return simox::alg::split(sceneSnapshotsToLoad, ";", trim);
    }
 
    void
    Segment::init()
    {
        SpecializedCoreSegment::init();
 
        const std::vector<std::string> scenes = p.getSceneSnapshotsToLoad();
        for (const std::string& scene : scenes)
        {
            const bool lockMemory = false;
            commitSceneSnapshotFromFilename(scene, lockMemory);
        }
 
        robots.setTag(Logging::tag);
    }
 
    void
    Segment::connect(viz::Client arviz)
    {
        (void)arviz;
        // ARMARX_INFO << "ArticulatedObjectVisu";
        // this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
        // visu->init();
    }
 
    Segment::CommitStats
    Segment::commitObjectPoses(const std::string& providerName,
                               const objpose::data::ProvidedObjectPoseSeq& providedPoses,
                               const Calibration& calibration,
                               std::optional<armem::Time> discardUpdatesUntil)
    {
        CommitStats stats;
 
        // Build new poses.
        objpose::ObjectPoseSeq newObjectPoses;
        stats.numUpdated = 0;
 
        // timestamp used to reduce the rpc calls for robot sync
        Time robotSyncTimestamp = Time::Invalid();
 
        for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
        {
            const Time timestamp = armarx::fromIce<Time>(provided.timestamp);
 
            // Check whether we have an old snapshot for this object.
            std::optional<objpose::ObjectPose> previousPose;
            const wm::Entity* entity =
                findObjectEntity(armarx::fromIce(provided.objectID), providerName);
            if (entity)
            {
                const arondto::ObjectInstance data = getLatestInstanceData(*entity);
 
                previousPose = objpose::ObjectPose();
                fromAron(data, *previousPose);
            }
 
            bool discard = false;
            if (discardUpdatesUntil && timestamp < discardUpdatesUntil.value())
            {
                // Dicard updates temporarily (e.g. due to head movement).
                discard = true;
            }
            else if (previousPose)
            {
                if (p.discardSnapshotsWhileAttached && previousPose->attachment)
                {
                    // Discard update due to active attachemnt.
                    discard = true;
                }
                else if (timestamp == previousPose->timestamp)
                {
                    // Discard update as it is not new.
                    discard = true;
                }
            }
 
            if (discard)
            {
                continue;
            }
 
            // Update the entity.
            stats.numUpdated++;
 
            VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
            if (not robot)
            {
                ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
                            << provided.robotName << "`.";
            }
 
            // robot may be null!
 
            bool synchronized = false;
 
            // Update the robot to obtain correct local -> global transformation
            if (robot and robotSyncTimestamp != timestamp)
            {
                synchronized = robots.reader->synchronizeRobot(*robot, timestamp);
                if (not synchronized)
                {
                    ARMARX_INFO << deactivateSpam(5) << "Failed to synchronize robot to timestamp "
                                << timestamp << " of provided object pose (" << provided.objectID
                                << "). This is " << (Clock::Now() - timestamp).toSecondsDouble()
                                << " seconds in the past.";
                }
 
                robotSyncTimestamp = timestamp;
 
                // Apply calibration offset after synchronizing the robot.
                {
                    if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
                    {
                        VirtualRobot::RobotNodePtr robotNode =
                            robot->getRobotNode(calibration.robotNode);
 
                        float value = robotNode->getJointValue();
                        float newValue = value + calibration.offset;
 
                        /*
                             * If newValue is out of the joint's limits, then the calibration would be ignored.
                             * In that case, we expand the joint value limits of the local robot model to allow
                             * for the calibrated value.
                             * As this is just for perception (and not for controlling the robot), this should be fine^TM.
                             */
                        VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
                        bool limitsChanged = false;
                        if (newValue < limits.low)
                        {
                            limits.low = newValue;
                            limitsChanged = true;
                        }
                        if (newValue > limits.high)
                        {
                            limits.high = newValue;
                            limitsChanged = true;
                        }
                        if (limitsChanged)
                        {
                            robotNode->setJointLimits(limits.low, limits.high);
                        }
 
                        robotNode->setJointValue(newValue);
                    }
                }
            }
            else if (robot and timestamp == robotSyncTimestamp)
            {
                synchronized = true;
            }
 
            if (not synchronized)
            {
                /*
                 * We know nothing about the current robot configuration. So the behvaiour of the
                 * following code is the same as if we have no robot model at all.
                 */
                ARMARX_INFO << deactivateSpam(5)
                            << "Robot could not be synchronized; discarding robot";
                robot = nullptr;
            }
 
            objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
            if (provided.objectPoseFrame.empty())
            {
                objpose::data::ProvidedObjectPose copy = provided;
                copy.objectPoseFrame = armarx::GlobalFrame;
                newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
            }
            else
            {
                newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
            }
 
            if (previousPose && previousPose->attachment)
            {
                // Keep current attachment.
                ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
                newPose.attachment = previousPose->attachment;
            }
 
            if (newPose.objectID.dataset().empty())
            {
                // Try to find the data set.
                const std::string dataset =
                    classNameToDatasetCache.get(newPose.objectID.className());
                if (!dataset.empty())
                {
                    newPose.objectID = {
                        dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
                }
            }
            if (!provided.localOOBB)
            {
                // Try to load oobb from disk.
                newPose.localOOBB = getObjectOOBB(newPose.objectID);
            }
        }
 
        commitObjectPoses(newObjectPoses, providerName);
 
        return stats;
    }
 
    void
    Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
    {
        Time now = Time::Now();
 
        ARMARX_CHECK_NOT_NULL(segmentPtr);
        const MemoryID coreSegmentID = segmentPtr->id();
 
        Commit commit;
        for (const objpose::ObjectPose& pose : objectPoses)
        {
            EntityUpdate& update = commit.updates.emplace_back();
 
            const MemoryID providerID = coreSegmentID.withProviderSegmentName(
                providerName.empty() ? pose.providerName : providerName);
 
            update.entityID = providerID.withEntityName(pose.objectID.str());
            update.arrivedTime = now;
            update.referencedTime = pose.timestamp;
            update.confidence = pose.confidence;
 
            arondto::ObjectInstance dto;
            toAron(dto, pose);
            // Search for object class.
            if (auto instance = findClassInstance(pose.objectID))
            {
                toAron(dto.classID, instance->id());
            }
            else
            {
                toAron(dto.classID, MemoryID());
            }
            toAron(dto.sourceID, MemoryID());
            update.instancesData.push_back(dto.toAron());
        }
        // Commit non-locking.
        iceMemory.commit(commit);
    }
 
    objpose::ObjectPoseMap
    Segment::getObjectPoses(const DateTime& now)
    {
        ObjectPoseMap objectPoses = getLatestObjectPoses();
        updateObjectPoses(objectPoses, now);
        return filterObjectPoses(objectPoses);
    }
 
    objpose::ObjectPoseMap
    Segment::getObjectPosesByProvider(const std::string& providerName, const DateTime& now)
    {
        ARMARX_CHECK_NOT_NULL(segmentPtr);
        ObjectPoseMap objectPoses =
            getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
        updateObjectPoses(objectPoses, now);
        return filterObjectPoses(objectPoses);
    }
 
    wm::Entity*
    Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
    {
        ARMARX_CHECK_NOT_NULL(segmentPtr);
        armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str());
        if (providerName.empty())
        {
            wm::Entity* result = nullptr;
            segmentPtr->forEachProviderSegment(
                [&result, &entityID](wm::ProviderSegment& prov)
                {
                    if (prov.hasEntity(entityID.entityName))
                    {
                        result = &prov.getEntity(entityID);
                        return false;
                    }
                    return true;
                });
            return result;
        }
        else
        {
            entityID.providerSegmentName = providerName;
            if (segmentPtr->hasProviderSegment(providerName))
            {
                wm::ProviderSegment& prov = segmentPtr->getProviderSegment(providerName);
                return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
            }
            else
            {
                return nullptr;
            }
        }
    }
 
    void
    Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
    {
        bool agentSynchronized = false;
 
        for (auto& [id, objectPose] : objectPoses)
        {
            VirtualRobot::RobotPtr robot =
                robots.get(objectPose.robotName, objectPose.providerName);
            updateObjectPose(objectPose, now, robot, agentSynchronized);
        }
    }
 
    void
    Segment::updateObjectPoses(ObjectPoseMap& objectPoses,
                               const DateTime& now,
                               VirtualRobot::RobotPtr agent,
                               bool& agentSynchronized) const
    {
        for (auto& [id, objectPose] : objectPoses)
        {
            updateObjectPose(objectPose, now, agent, agentSynchronized);
        }
    }
 
    void
    Segment::updateObjectPose(ObjectPose& objectPose,
                              const DateTime& now,
                              VirtualRobot::RobotPtr agent,
                              bool& agentSynchronized) const
    {
        updateAttachement(objectPose, agent, agentSynchronized);
 
        if (decay.enabled)
        {
            decay.updateConfidence(objectPose, now);
        }
    }
 
    Segment::ObjectPoseMap
    Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
    {
        ObjectPoseMap result;
        for (const auto& [id, objectPose] : objectPoses)
        {
            if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
            {
                result[id] = objectPose;
            }
        }
        return result;
    }
 
    void
    Segment::updateAttachement(ObjectPose& objectPose,
                               VirtualRobot::RobotPtr agent,
                               bool& synchronized) const
    {
        if (not objectPose.attachment.has_value())
        {
            // No attachment, nothing to do.
            return;
        }
        ARMARX_CHECK(objectPose.attachment);
 
        if (not agent)
        {
            // Without a robot model, we cannot update the object pose.
            return;
        }
        ARMARX_CHECK_NOT_NULL(agent);
 
        if (not synchronized) // Synchronize only once.
        {
            ARMARX_CHECK_NOT_NULL(robots.reader);
 
            const armarx::DateTime timestamp = armarx::Clock::Now();
 
            ARMARX_CHECK(robots.reader->synchronizeRobot(*agent, timestamp));
            synchronized = true;
        }
        objectPose.updateAttached(agent);
    }
 
    objpose::ObjectPoseMap
    Segment::getLatestObjectPoses() const
    {
        ARMARX_CHECK_NOT_NULL(segmentPtr);
        return getLatestObjectPoses(*segmentPtr);
    }
 
    objpose::ObjectPoseMap
    Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg)
    {
        ObjectPoseMap result;
        getLatestObjectPoses(coreSeg, result);
        return result;
    }
 
    objpose::ObjectPoseMap
    Segment::getLatestObjectPoses(const wm::ProviderSegment& provSeg)
    {
        ObjectPoseMap result;
        getLatestObjectPoses(provSeg, result);
        return result;
    }
 
    objpose::ObjectPose
    Segment::getLatestObjectPose(const wm::Entity& entity)
    {
        ObjectPose result;
        getLatestObjectPose(entity, result);
        return result;
    }
 
    void
    Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
    {
        coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment& provSegment)
                                       { getLatestObjectPoses(provSegment, out); });
    }
 
    void
    Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
    {
        provSegment.forEachEntity(
            [&out](const wm::Entity& entity)
            {
                if (!entity.empty())
                {
                    ObjectPose pose = getLatestObjectPose(entity);
                    // Try to insert. Fails and returns false if an entry already exists.
                    const auto [it, success] = out.insert({pose.objectID, pose});
                    if (!success)
                    {
                        // An entry with that ID already exists. We keep the newest.
                        if (it->second.timestamp < pose.timestamp)
                        {
                            it->second = pose;
                        }
                    }
                }
            });
    }
 
    void
    Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
    {
        entity.getLatestSnapshot().forEachInstance(
            [&out](const wm::EntityInstance& instance)
            {
                arondto::ObjectInstance dto;
                dto.fromAron(instance.data());
 
                fromAron(dto, out);
            });
    }
 
    arondto::ObjectInstance
    Segment::getLatestInstanceData(const wm::Entity& entity)
    {
        ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
        const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
 
        ARMARX_CHECK_EQUAL(snapshot.size(), 1);
        const wm::EntityInstance& instance = snapshot.getInstance(0);
 
        arondto::ObjectInstance data;
        data.fromAron(instance.data());
 
        return data;
    }
 
    ::armarx::armem::articulated_object::ArticulatedObjects
    Segment::getArticulatedObjects()
    {
        objpose::ObjectPoseMap objectPoses = getObjectPoses(Time::Now());
 
        ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
 
        ::armarx::armem::articulated_object::ArticulatedObjects objects;
        for (const auto& [objectId, objectPose] : objectPoses)
        {
            armem::articulated_object::ArticulatedObject articulatedObject;
            articulatedObject.config.jointMap = objectPose.objectJointValues;
            articulatedObject.config.globalPose = objectPose.objectPoseGlobal;
            articulatedObject.config.timestamp = objectPose.timestamp;
            articulatedObject.instance = objectPose.objectID.instanceName();
            articulatedObject.timestamp = objectPose.timestamp;
 
            ARMARX_INFO << "Object id is " << objectId.str();
            ARMARX_INFO << "Object id for objectPose is " << objectPose.objectID.str();
 
            // Search for object class.
            if (auto classInstance = findClassInstance(objectPose.objectID))
            {
                arondto::ObjectClass dto;
 
                try
                {
                    dto.fromAron(classInstance->data());
                    robot::RobotDescription description;
 
                    fromAron(dto, description);
                    articulatedObject.description = description;
                }
                catch (...)
                {
                    ARMARX_WARNING << "Conversion failed!";
                    continue;
                }
            }
            else
            {
                ARMARX_WARNING << "Class instance not found!";
                continue;
            }
 
            if (not articulatedObject.config.jointMap.empty())
            {
                objects.push_back(articulatedObject);
            }
        }
 
        return objects;
    }
 
    std::map<DateTime, objpose::ObjectPose>
    Segment::getObjectPosesInRange(const wm::Entity& entity,
                                   const DateTime& start,
                                   const DateTime& end)
    {
        std::map<DateTime, objpose::ObjectPose> result;
 
        entity.forEachSnapshotInTimeRange(
            start,
            end,
            [&result](const wm::EntitySnapshot& snapshot)
            {
                snapshot.forEachInstance(
                    [&result, &snapshot](const wm::EntityInstance& instance)
                    {
                        arondto::ObjectInstance dto;
                        dto.fromAron(instance.data());
 
                        ObjectPose pose;
                        fromAron(dto, pose);
 
                        result.emplace(snapshot.time(), pose);
                    });
            });
 
        return result;
    }
 
    std::optional<simox::OrientedBoxf>
    Segment::getObjectOOBB(const ObjectID& id)
    {
        return oobbCache.get(id);
    }
 
    objpose::ProviderInfo
    Segment::getProviderInfo(const std::string& providerName)
    {
        try
        {
            return providers.at(providerName);
        }
        catch (const std::out_of_range&)
        {
            std::stringstream ss;
            ss << "No provider with name '" << providerName << "' available.\n";
            ss << "Available are:\n";
            for (const auto& [name, _] : providers)
            {
                ss << "- '" << name << "'\n";
            }
            throw std::out_of_range(ss.str());
        }
    }
 
    objpose::AttachObjectToRobotNodeOutput
    Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
    {
        const armem::Time now = armem::Time::Now();
 
        objpose::AttachObjectToRobotNodeOutput output;
        output.success = false; // We are not successful until proven otherwise.
 
        ObjectID objectID = armarx::fromIce(input.objectID);
 
        VirtualRobot::RobotPtr agent = robots.get(input.agentName);
        if (not agent)
        {
            std::stringstream ss;
            ss << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName
               << "'."
               << "\n(You can leave the agent name empty if there is only one agent.)\n"
               << "\nKnown agents: ";
            for (const auto& [name, robot] : robots.loaded)
            {
                ss << "\n- '" << name << "'";
            }
            ARMARX_WARNING << ss.str();
            return output;
        }
        ARMARX_CHECK_NOT_NULL(agent);
 
        if (!agent->hasRobotNode(input.frameName))
        {
            ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '"
                           << input.frameName << "' of agent '" << agent->getName() << "'.";
            return output;
        }
        std::string frameName = input.frameName;
 
 
        // Find object pose (provider name can be empty).
        wm::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
        if (!objectEntity || objectEntity->empty())
        {
            ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
                           << "' of agent '" << agent->getName()
                           << "', but object is currently not provided.";
            return output;
        }
        arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
 
        objpose::ObjectAttachmentInfo info;
        info.agentName = agent->getName();
        info.frameName = frameName;
 
        if (input.poseInFrame)
        {
            info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
        }
        else
        {
            ARMARX_CHECK_NOT_NULL(robots.reader);
 
            const auto timestamp = armarx::Clock::Now();
            ARMARX_CHECK(robots.reader->synchronizeRobot(*agent, timestamp));
 
            armarx::FramedPose framed(
                data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
            if (frameName == armarx::GlobalFrame)
            {
                info.poseInFrame = framed.toGlobalEigen(agent);
            }
            else
            {
                framed.changeFrame(agent, info.frameName);
                info.poseInFrame = framed.toEigen();
            }
        }
 
        // Store attachment in new entity snapshot.
        {
            armem::Commit commit;
            armem::EntityUpdate& update = commit.add();
            update.entityID = objectEntity->id();
            update.referencedTime = now;
            {
                arondto::ObjectInstance updated = data;
                toAron(updated.pose.attachment, info);
                updated.pose.attachmentValid = true;
                update.instancesData = {updated.toAron()};
            }
            iceMemory.commit(commit);
        }
 
        ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName
                    << "' "
                    << "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
                    << "Object pose in frame: \n"
                    << info.poseInFrame;
 
        output.success = true;
        output.attachment = new objpose::data::ObjectAttachmentInfo();
        output.attachment->frameName = info.frameName;
        output.attachment->agentName = info.agentName;
        output.attachment->poseInFrame = new Pose(info.poseInFrame);
 
        return output;
    }
 
    objpose::DetachObjectFromRobotNodeOutput
    Segment::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input)
    {
        const armem::Time now = armem::Time::Now();
 
        ObjectID objectID = armarx::fromIce(input.objectID);
        std::string providerName = input.providerName;
 
        std::optional<objpose::arondto::ObjectAttachmentInfo> attachment;
        {
            // Remove from latest pose (if it was cached).
            // Find object pose (provider name can be empty).
            wm::Entity* entity = this->findObjectEntity(objectID, input.providerName);
            if (entity)
            {
                const arondto::ObjectInstance data = getLatestInstanceData(*entity);
                if (data.pose.attachmentValid)
                {
                    attachment = data.pose.attachment;
 
                    // Store non-attached pose in new snapshot.
                    storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
                }
                if (providerName.empty())
                {
                    providerName = data.pose.providerName;
                }
            }
        }
 
        objpose::DetachObjectFromRobotNodeOutput output;
        output.wasAttached = bool(attachment);
        if (attachment)
        {
            ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName
                        << "' from robot node '" << attachment->frameName << "' of agent '"
                        << attachment->agentName << "'.";
        }
        else
        {
            ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName
                        << "' "
                        << "from robot node, but it was not attached.";
        }
 
        return output;
    }
 
    objpose::DetachAllObjectsFromRobotNodesOutput
    Segment::detachAllObjectsFromRobotNodes(
        const objpose::DetachAllObjectsFromRobotNodesInput& input)
    {
        ARMARX_CHECK_NOT_NULL(segmentPtr);
 
        const armem::Time now = armem::Time::Now();
 
        objpose::DetachAllObjectsFromRobotNodesOutput output;
        output.numDetached = 0;
        segmentPtr->forEachEntity(
            [this, now, &input, &output](wm::Entity& entity)
            {
                const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
                if (data.pose.attachmentValid)
                {
                    ++output.numDetached;
                    // Store non-attached pose in new snapshot.
                    this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
                }
            });
 
        ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
 
        return output;
    }
 
    void
    Segment::storeDetachedSnapshot(wm::Entity& entity,
                                   const arondto::ObjectInstance& data,
                                   Time now,
                                   bool commitAttachedPose)
    {
        armem::Commit commit;
        armem::EntityUpdate& update = commit.add();
        update.entityID = entity.id();
        update.referencedTime = now;
        {
            arondto::ObjectInstance updated;
            if (commitAttachedPose and data.pose.attachmentValid)
            {
                ObjectPose objectPose;
                fromAron(data, objectPose);
 
                VirtualRobot::RobotPtr robot =
                    robots.get(objectPose.robotName, objectPose.providerName);
                bool agentSynchronized = false;
                updateAttachement(objectPose, robot, agentSynchronized);
 
                objectPose.attachment = std::nullopt;
                toAron(updated, objectPose);
            }
            else
            {
                updated = data;
                updated.pose.attachmentValid = false;
                toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
            }
 
            update.instancesData = {updated.toAron()};
        }
        iceMemory.commit(commit);
    }
 
    std::optional<wm::EntityInstance>
    Segment::findClassInstance(const ObjectID& objectID) const
    {
        const ObjectID classID = {objectID.dataset(), objectID.className()};
        try
        {
            std::optional<wm::EntityInstance> result;
            iceMemory.workingMemory->getCoreSegment("Class").forEachProviderSegment(
                [&classID, &result](const wm::ProviderSegment& provSeg)
                {
                    if (provSeg.hasEntity(classID.str()))
                    {
                        result =
                            provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
                        return false;
                    }
                    return true;
                });
 
            if (not result.has_value())
            {
                ARMARX_WARNING << deactivateSpam(120) << "No provider segment for classID "
                               << classID.str() << " found";
            }
            return result;
        }
        catch (const armem::error::ArMemError& e)
        {
            // Some segment or entity did not exist.
            ARMARX_WARNING << e.what();
            return std::nullopt;
        }
    }
 
    void
    Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
    {
        if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
        {
            ARMARX_INFO << "Storing scene snapshot at: \n" << path.value();
            try
            {
                simox::json::write(path.value(), scene, 2);
            }
            catch (const simox::json::error::JsonError& e)
            {
                ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
            }
        }
    }
 
    std::optional<armarx::objects::Scene>
    Segment::loadScene(const std::string& filename)
    {
        if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
        {
            return loadScene(path.value());
        }
        else
        {
            return std::nullopt;
        }
    }
 
    std::optional<armarx::objects::Scene>
    Segment::loadScene(const std::filesystem::path& path)
    {
        try
        {
            return simox::json::read<armarx::objects::Scene>(path);
        }
        catch (const simox::json::error::JsonError& e)
        {
            ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
            return std::nullopt;
        }
    }
 
    const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
 
    std::optional<std::filesystem::path>
    Segment::resolveSceneFilepath(const std::string& _filename)
    {
        std::stringstream log;
        std::filesystem::path filepath = _filename;
 
        filepath = simox::alg::replace_all(
            filepath, timestampPlaceholder, Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
        if (not simox::alg::ends_with(filepath, ".json"))
        {
            filepath += ".json";
        }
 
        if (filepath.is_absolute())
        {
            return filepath;
        }
 
        armarx::PackagePath packagePath = [&filepath, this]()
        {
            if (filepath.has_parent_path())
            {
                // Interpret the first component as package name.
                std::string packageName = *filepath.begin();
                return PackagePath(packageName, std::filesystem::relative(filepath, packageName));
            }
            else
            {
                // Only a filename => Interprete it as relative to 'Package/scenes/'.
                return PackagePath(p.sceneSnapshotsPackage, p.sceneSnapshotsDirectory / filepath);
            }
        }();
 
        try
        {
            std::filesystem::path systemPath = packagePath.toSystemPath();
            return systemPath;
        }
        catch (const armarx::PackageNotFoundException& e)
        {
            ARMARX_INFO << "Could not interpret '" << packagePath.serialize().package
                        << "' as ArmarX package name (" << e.what() << ").";
            return std::nullopt;
        }
    }
 
    armarx::objects::Scene
    Segment::getSceneSnapshot() const
    {
        using armarx::objects::SceneObject;
 
        // We only store the latest version of each objectID.
 
        struct StampedSceneObject
        {
            SceneObject object;
            Time timestamp;
        };
 
        std::map<ObjectID, StampedSceneObject> objects;
        segmentPtr->forEachEntity(
            [&objects](wm::Entity& entity)
            {
                const wm::EntityInstance* entityInstance = entity.findLatestInstance();
                if (entityInstance)
                {
                    std::optional<arondto::ObjectInstance> objectInstance =
                        tryCast<arondto::ObjectInstance>(*entityInstance);
                    if (objectInstance)
                    {
                        const ObjectID objectID =
                            ObjectID::FromString(objectInstance->classID.entityName);
 
                        auto it = objects.find(objectID);
                        if (it == objects.end() or
                            objectInstance->pose.timestamp > it->second.timestamp)
                        {
                            StampedSceneObject& stamped = objects[objectID];
                            stamped.timestamp = objectInstance->pose.timestamp;
 
                            SceneObject& object = stamped.object;
                            object.className = objectID.getClassID().str();
                            object.instanceName = objectID.instanceName();
                            object.collection = "";
 
                            object.position =
                                simox::math::position(objectInstance->pose.objectPoseGlobal);
                            object.orientation =
                                simox::math::orientation(objectInstance->pose.objectPoseGlobal);
 
                            object.isStatic = objectInstance->pose.isStatic;
                            object.jointValues = objectInstance->pose.objectJointValues;
                        }
                    }
                }
            });
 
        armarx::objects::Scene scene;
        for (const auto& [id, stamped] : objects)
        {
            scene.objects.emplace_back(std::move(stamped.object));
        }
        return scene;
    }
 
    void
    Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
    {
        const Time now = Time::Now();
        std::map<ObjectID, int> idCounters;
 
        objpose::ObjectPoseSeq objectPoses;
 
        for (const auto& object : scene.objects)
        {
            if (simox::alg::starts_with(object.className, "#"))
            {
                // marked to be ignored
                continue;
            }
 
            const ObjectID classID = object.getClassID(objectFinder);
 
            objpose::ObjectPose& pose = objectPoses.emplace_back();
 
            pose.providerName = sceneName;
            pose.objectType = objpose::ObjectType::KnownObject;
            // If not specified, assume loaded objects are static.
            pose.isStatic = object.isStatic.has_value() ? object.isStatic.value() : true;
            pose.objectID = classID.withInstanceName(object.instanceName.empty()
                                                         ? std::to_string(idCounters[classID]++)
                                                         : object.instanceName);
 
            pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
            pose.objectPoseRobot = pose.objectPoseGlobal;
            pose.objectPoseOriginal = pose.objectPoseGlobal;
            pose.objectPoseOriginalFrame = armarx::GlobalFrame;
 
            pose.objectJointValues = object.jointValues;
 
            pose.robotConfig = {};
            pose.robotPose = Eigen::Matrix4f::Identity();
            pose.robotName = "";
 
            pose.confidence = 1.0;
            pose.localOOBB = getObjectOOBB(classID);
            pose.timestamp = now;
        }
 
        commitObjectPoses(objectPoses);
    }
 
    void
    Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
    {
        std::stringstream ss;
        ss << "Loading scene '" << filename << "' ...";
        if (std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
        {
            ARMARX_INFO << ss.str() << "\nfrom " << path.value();
 
            if (const auto snapshot = loadScene(path.value()))
            {
                auto makeSceneName = [](const std::filesystem::path& path)
                {
                    std::filesystem::path filename = path.filename();
                    filename.replace_extension(); // Removes extension
                    return filename.string();
                };
                std::string sceneName = makeSceneName(path.value());
 
                // The check seems useless?
                if (lockMemory)
                {
                    segmentPtr->doLocked([this, &snapshot, &sceneName]()
                                         { commitSceneSnapshot(snapshot.value(), sceneName); });
                }
                else
                {
                    commitSceneSnapshot(snapshot.value(), sceneName);
                }
            }
        }
        else
        {
            ARMARX_INFO << ss.str() << " failed: Could not resolve scene name or path.";
        }
    }
 
    void
    Segment::RemoteGui::setup(const Segment& data)
    {
        using namespace armarx::RemoteGui::Client;
 
        maxHistorySize.setValue(std::max(1, int(data.properties.maxHistorySize)));
        maxHistorySize.setRange(1, 1e6);
        infiniteHistory.setValue(data.properties.maxHistorySize == -1);
        discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
 
        const std::string storeLoadPath = [&data]()
        {
            const std::vector<std::string> scenes = data.p.getSceneSnapshotsToLoad();
            if (scenes.empty())
            {
                std::string storeLoadFilename = "Scene_" + timestampPlaceholder;
                return data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsToLoad;
            }
            else
            {
                return scenes.front();
            }
        }();
 
        storeLoadLine.setValue(storeLoadPath);
        loadButton.setLabel("Load Scene");
        storeButton.setLabel("Store Scene");
 
        HBoxLayout storeLoadLineLayout({storeLoadLine, Label(".json")});
        HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
 
        detachAllObjectsButton.setLabel("Detach All Objects");
        detachAllObjectsCommitAttachedPoseCheckBox.setValue(true);
        HBoxLayout detachAllObjectsCommitAttachedPoseLayout(
            {Label("Commit Attached Pose"), detachAllObjectsCommitAttachedPoseCheckBox});
 
 
        GridLayout grid;
        int row = 0;
 
        grid.add(storeLoadLineLayout, {row, 0}, {1, 2});
        row++;
        grid.add(storeLoadButtonsLayout, {row, 0}, {1, 2});
        row++;
 
        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++;
 
        grid.add(detachAllObjectsButton, {row, 0})
            .add(detachAllObjectsCommitAttachedPoseLayout, {row, 1});
 
        group.setLabel("Data");
        group.addChild(grid);
    }
 
    void
    Segment::RemoteGui::update(Segment& segment)
    {
        if (loadButton.wasClicked())
        {
            bool lockMemory = true;
            segment.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
        }
 
        if (storeButton.wasClicked())
        {
            armarx::objects::Scene scene;
            segment.doLocked([&scene, &segment]() { scene = segment.getSceneSnapshot(); });
            segment.storeScene(storeLoadLine.getValue(), scene);
        }
 
        if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged() ||
            discardSnapshotsWhileAttached.hasValueChanged())
        {
            segment.doLocked(
                [this, &segment]()
                {
                    if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
                    {
                        segment.properties.maxHistorySize =
                            infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
                        if (segment.segmentPtr)
                        {
                            segment.segmentPtr->setMaxHistorySize(
                                long(segment.properties.maxHistorySize));
                        }
                    }
 
                    segment.p.discardSnapshotsWhileAttached =
                        discardSnapshotsWhileAttached.getValue();
                });
        }
 
        if (detachAllObjectsButton.wasClicked())
        {
            objpose::DetachAllObjectsFromRobotNodesInput input;
            input.commitAttachedPose = detachAllObjectsCommitAttachedPoseCheckBox.getValue();
 
            objpose::DetachAllObjectsFromRobotNodesOutput output = segment.doLocked(
                [&segment, &input]() { return segment.detachAllObjectsFromRobotNodes(input); });
            (void)output;
        }
    }
 
    VirtualRobot::RobotPtr
    Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
    {
        std::string robotName = _robotName;
 
        if (robotName.empty())
        {
            auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
 
            std::stringstream ss;
            if (providerName.empty())
            {
                ss << "The provided object pose";
            }
            else
            {
                ss << "The object pose provided by '" << providerName << "'";
            }
            ss << " does not specify a robot name";
 
            if (fallbackName.empty())
            {
                ss << ", and no fallback robot name was configured (e.g. via the properties).\n"
                   << "For these object poses, the object instance segment is not able "
                   << "to provide transformed object poses (global and in robot root frame).";
                ARMARX_INFO << antiSpam << ss.str();
 
                return nullptr;
            }
            else
            {
                ss << ". The object instance segment is falling back to '" << fallbackName << "'.";
                ARMARX_INFO << antiSpam << ss.str();
 
                robotName = fallbackName;
            }
        }
        ARMARX_CHECK_NOT_EMPTY(robotName);
 
        // Lookup in cache.
        if (auto it = loaded.find(robotName); it != loaded.end())
        {
            ARMARX_CHECK_NOT_NULL(it->second);
            return it->second;
        }
        else
        {
            // Try to fetch the robot.
            ARMARX_CHECK_NOT_NULL(reader);
            VirtualRobot::RobotPtr robot = reader->getRobot(
                robotName, Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure);
 
            if (robot)
            {
                bool synchronized = reader->synchronizeRobot(*robot, Clock::Now());
                if (not synchronized)
                {
                    ARMARX_INFO << "The robot '" << robotName << "' could be loaded, but not"
                                << " synchronized successfully (e.g., the global localization "
                                   "could be missing). "
                                << "Make sure to synchronize it before use if necessary.";
                }
                // Store robot if valid.
                loaded.emplace(robotName, robot);
            }
            return robot; // valid or nullptr
        }
    }
 
 
} // namespace armarx::armem::server::obj::instance