Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
#include <filesystem>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
 
namespace armarx::armem::server::obj::clazz
{
 
    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        SpecializedCoreSegment(memoryToIceAdapter,
                               objects::classSegmentID.coreSegmentName,
                               arondto::ObjectClass::ToAronType(),
                               -1)
    {
    }
 
    Segment::~Segment()
    {
    }
 
    void
    Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        SpecializedCoreSegment::defineProperties(defs, prefix);
 
        defs->optional(p.objectsPackage,
                       prefix + "ObjectsPackage",
                       "Name of the objects package to load from.");
        defs->optional(p.loadFromObjectsPackage,
                       prefix + "LoadFromObjectsPackage",
                       "If true, load the objects from the objects package on startup.");
 
        floorVis.defineProperties(defs, prefix + "Floor.");
    }
 
    void
    Segment::init()
    {
        SpecializedCoreSegment::init();
 
        if (p.loadFromObjectsPackage)
        {
            ARMARX_VERBOSE << "Loading objects from package `" << p.objectsPackage << "`.";
            loadByObjectFinder(p.objectsPackage);
        }
    }
 
    void
    Segment::reloadObjectClassesByObjectFinder()
    {
        if (p.loadFromObjectsPackage)
        {
            ARMARX_VERBOSE << "Loading objects from package `" << p.objectsPackage << "`.";
            loadByObjectFinder(p.objectsPackage);
        }
        else
        {
            ARMARX_INFO << "Loading object from package in `reloadObjectClassesByObjectFinder()` "
                           "is disabled via property. No object classes will be reloaded!";
        }
    }
 
    void
    Segment::connect(viz::Client arviz)
    {
        this->arviz = arviz;
 
        floorVis.setArViz(arviz);
        floorVis.updateFloorObject(*segmentPtr);
    }
 
    void
    Segment::loadByObjectFinder(const std::string& objectsPackage)
    {
        ARMARX_TRACE;
        loadByObjectFinder(ObjectFinder(objectsPackage));
    }
 
    void
    Segment::loadByObjectFinder(const ObjectFinder& finder)
    {
        this->objectFinder = finder;
        loadByObjectFinder();
    }
 
    void
    Segment::loadByObjectFinder()
    {
        ARMARX_TRACE;
 
        const Time now = Time::Now();
 
        const bool checkPaths = false;
        std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
 
        const MemoryID providerID =
            segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName());
        ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
                    << objectFinder.getPackageName() << "' ...";
        Commit commit;
        for (ObjectInfo& info : infos)
        {
            ARMARX_TRACE;
 
            ARMARX_VERBOSE << info.idStr();
            info.setLogError(false);
 
            EntityUpdate& update = commit.add();
            update.entityID = providerID.withEntityName(info.id().str());
            update.arrivedTime = update.referencedTime = update.sentTime = now;
 
            ARMARX_TRACE;
            ARMARX_VERBOSE << "object class from info";
            arondto::ObjectClass objectClass = objectClassFromInfo(info);
 
            armarx::ObjectID classID;
            fromAron(objectClass.id, classID);
            ARMARX_VERBOSE << VAROUT(classID);
            update.instancesData = {objectClass.toAron()};
        }
 
        ARMARX_TRACE;
 
        ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
                    << objectFinder.getPackageName() << "'.";
        iceMemory.commitLocking(commit);
    }
 
    void
    Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
    {
        const Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
 
        viz::Layer layerOrigin = arviz.layer("Origin");
        layerOrigin.add(viz::Pose("Origin"));
 
        viz::Layer layerObject = arviz.layer("Class Model");
        viz::Layer layerAABB = arviz.layer("Class AABB");
        viz::Layer layerOOBB = arviz.layer("Class OOBB");
 
        if (segmentPtr)
        {
            try
            {
                std::optional<arondto::ObjectClass> aron = doLocked(
                    [this, &entityID]()
                    {
                        return segmentPtr->findLatestInstanceDataAs<arondto::ObjectClass>(entityID,
                                                                                          0);
                    });
                if (not aron.has_value())
                {
                    return;
                }
 
                if (not aron->simoxXmlPath.package.empty())
                {
                    layerObject.add(viz::Object(entityID.str())
                                        .file(aron->simoxXmlPath.package, aron->simoxXmlPath.path)
                                        .pose(pose));
                }
 
                if (showAABB)
                {
                    layerAABB.add(viz::Box("AABB")
                                      .pose(pose * simox::math::pose(aron->aabb.center))
                                      .size(aron->aabb.extents)
                                      .color(simox::Color::cyan(255, 64)));
                }
                if (showOOBB)
                {
                    layerOOBB.add(viz::Box("OOBB")
                                      .pose(pose * simox::math::pose(aron->oobb.center,
                                                                     aron->oobb.orientation))
                                      .size(aron->oobb.extents)
                                      .color(simox::Color::lime(255, 64)));
                }
            }
            catch (const armem::error::ArMemError& e)
            {
                ARMARX_INFO << "Failed to visualize object class " << entityID << "."
                            << "\nReason: " << e.what();
            }
            catch (const aron::error::AronException& e)
            {
                ARMARX_INFO << "Failed to visualize object class " << entityID << "."
                            << "\nReason: " << e.what();
            }
        }
 
        arviz.commit({layerObject, layerOrigin, layerAABB, layerOOBB});
    }
 
    arondto::ObjectClass
    Segment::objectClassFromInfo(const ObjectInfo& info)
    {
        namespace fs = std::filesystem;
 
        // ARMARX_VERBOSE << info.idStr();
 
        arondto::ObjectClass data;
        toAron(data.id, info.id());
 
        auto setPathIfExists = [](armarx::arondto::PackagePath& aron, PackageFileLocation location)
        {
            // if the location relative path starts withtthe package name, this must be removed. The armarx::PackagePath
            // automatically adds the package when creating the absolute path
            if (simox::alg::starts_with(location.relativePath, location.package))
            {
                // strip the package name and the "/"
                location.relativePath = location.relativePath.substr(location.package.size() + 1);
            }
 
            if (fs::is_regular_file(location.absolutePath))
            {
                toAron(aron, location);
            }
            else
            {
                toAron(aron, PackageFileLocation());
            }
        };
        setPathIfExists(data.simoxXmlPath, info.simoxXML());
        setPathIfExists(data.urdfPath, info.urdf());
        setPathIfExists(data.sdfPath, info.sdf());
        setPathIfExists(data.articulatedSimoxXmlPath, info.articulatedSimoxXML());
        setPathIfExists(data.articulatedUrdfPath, info.articulatedUrdf());
        setPathIfExists(data.articulatedSdfPath, info.articulatedSdf());
        setPathIfExists(data.meshObjPath, info.wavefrontObj());
        setPathIfExists(data.meshWrlPath, info.meshWrl());
 
        ARMARX_TRACE;
        ARMARX_VERBOSE << "AABB";
        auto aabb = info.loadAABB();
        toAron(data.aabb, aabb ? aabb.value() : simox::AxisAlignedBoundingBox());
 
        ARMARX_TRACE;
        auto oobb = info.loadOOBB();
        ARMARX_VERBOSE << "OOBB";
        toAron(data.oobb, oobb ? oobb.value() : simox::OrientedBoxf());
 
        if (auto recogNames = info.loadRecognizedNames())
        {
            data.names.recognized = recogNames.value();
        }
        if (auto spokenNames = info.loadSpokenNames())
        {
            data.names.spoken = spokenNames.value();
        }
 
        return data;
    }
 
    void
    Segment::RemoteGui::setup(const Segment& segment)
    {
        using namespace armarx::RemoteGui::Client;
 
        data.setup(segment);
        visu.setup(segment);
 
        VBoxLayout layout;
        layout.addChildren({data.group, visu.group});
 
        group = {};
        group.setLabel("Class");
        group.addChildren({layout, VSpacer()});
    }
 
    void
    Segment::RemoteGui::update(Segment& segment)
    {
        data.update(segment);
        visu.update(segment);
    }
 
    void
    Segment::RemoteGui::Data::setup(const Segment& segment)
    {
        using namespace armarx::RemoteGui::Client;
 
        reloadButton.setLabel("Reload");
 
        maxHistorySize.setValue(std::max(1, int(segment.properties.maxHistorySize)));
        maxHistorySize.setRange(1, 1e6);
        infiniteHistory.setValue(segment.properties.maxHistorySize == -1);
 
        GridLayout grid;
        int row = 0;
        grid.add(reloadButton, {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++;
 
        group = {};
        group.setLabel("Data");
        group.addChild(grid);
    }
 
    void
    Segment::RemoteGui::Data::update(Segment& segment)
    {
        if (reloadButton.wasClicked())
        {
            // Core segment mutex will be locked on commit.
            segment.loadByObjectFinder();
            rebuild = true;
        }
        if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
        {
            segment.doLocked(
                [this, &segment]()
                {
                    segment.properties.maxHistorySize =
                        infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
                    if (segment.segmentPtr)
                    {
                        segment.segmentPtr->setMaxHistorySize(
                            long(segment.properties.maxHistorySize));
                    }
                });
        }
    }
 
    void
    Segment::RemoteGui::Visu::setup(const Segment& segment)
    {
        using namespace armarx::RemoteGui::Client;
 
        showComboBox = {};
        showOptionsIndex.clear();
        segment.segmentPtr->forEachEntity(
            [this](const wm::Entity& entity)
            {
                std::stringstream option;
                option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
                showComboBox.addOption(option.str());
                showOptionsIndex.push_back(entity.id());
            });
        if (showOptionsIndex.empty())
        {
            showComboBox.addOption("<none>");
        }
        showButton.setLabel("Visualize Object Class");
 
        GridLayout grid;
        int row = 0;
        grid.add(showComboBox, {row, 0}, {1, 2});
        row++;
        grid.add(showButton, {row, 0}, {1, 2});
        row++;
 
        group = {};
        group.setLabel("Visualization");
        group.addChild(grid);
    }
 
    void
    Segment::RemoteGui::Visu::update(Segment& segment)
    {
        if (showButton.wasClicked())
        {
            const size_t index = static_cast<size_t>(showComboBox.getIndex());
            if (/*index >= 0 &&*/ index < showOptionsIndex.size())
            {
                segment.visualizeClass(showOptionsIndex.at(index));
            }
        }
    }
 
} // namespace armarx::armem::server::obj::clazz