Component.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    navigation::ArmarXObjects::room_editor
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
#include <algorithm>
#include <cstdint>
#include <memory>
#include <mutex>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
 
#include <armarx/navigation/components/navigation_memory/ComponentInterface.h>
#include <armarx/navigation/components/room_editor/RoomEditor.h>
#include <armarx/navigation/memory/client/rooms/Reader.h>
 
namespace armarx::navigation::components::room_editor
{
 
    const std::string Component::defaultName = "room_editor";
 
    Component::Component()
    {
        addPlugin(roomsReaderPlugin);
        addPlugin(roomsWriterPlugin);
    }
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->optional(properties.providerSegmentName,
                      "p.providerSegmentName",
                      "If set, only the rooms of this provider segment (i.e. navigation graph) "
                      "are edited. If empty, all rooms are edited.");
 
        def->optional(
            properties.updateRate, "p.updateRate", "Rate of the ArViz interaction loop [Hz].");
 
        def->optional(properties.layerPrefix,
                      "p.visu.layerPrefix",
                      "Prefix of the ArViz layers created by this component.");
 
        def->optional(properties.vertexHandleRadius,
                      "p.visu.vertexHandleRadius",
                      "Radius of the draggable polygon vertex handles [mm].");
 
        def->optional(properties.edgeHandleSize,
                      "p.visu.edgeHandleSize",
                      "Edge length of the draggable edge handles [mm].");
 
        def->optional(properties.exportPackageName,
                      "p.export.packageName",
                      "ArmarX package the rooms are exported to.");
 
        def->optional(properties.exportDirectory,
                      "p.export.directory",
                      "Directory relative to the package's data directory the rooms are "
                      "exported to. The `rooms.json` files are written to "
                      "`<directory>/<navigation graph>/rooms.json`, so this usually has to "
                      "include the dataset, e.g. `navigation-graphs/50_19`.");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
    }
 
    void
    Component::onConnectComponent()
    {
        RoomEditor::Parameters editorParameters;
        editorParameters.layerPrefix = properties.layerPrefix;
        editorParameters.vertexHandleRadius = properties.vertexHandleRadius;
        editorParameters.edgeHandleSize = properties.edgeHandleSize;
 
        editor = std::make_unique<RoomEditor>(arviz, editorParameters);
        editor->setRoomChangedCallback([this](const EditableRoom& room) { storeRoom(room); });
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
 
        task = new RunningTask<Component>(this, &Component::run, "RoomEditorTask");
        task->start();
    }
 
    void
    Component::onDisconnectComponent()
    {
        if (task)
        {
            const bool join = true;
            task->stop(join);
            task = nullptr;
        }
 
        editor.reset();
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    void
    Component::run()
    {
        ARMARX_CHECK_NOT_NULL(editor);
 
        viz::StagedCommit stage;
 
        reload();
        editor->redrawAll(stage);
 
        // The ArViz handles are manipulated with Coin draggers. Those only receive mouse
        // events while the 3D viewer is NOT in viewing (camera) mode, which is the default.
        // Selecting an element and using its context menu works either way, so "the context
        // menu works but nothing can be dragged" usually means viewing mode is still on.
        ARMARX_IMPORTANT << "Room editor is ready. Drag the handles in ArViz to reshape a room. "
                            "If the handles cannot be grabbed, disable the 3D viewer's viewing "
                            "mode (the toolbar toggle above the 3D view, shortcut `V`).";
 
        const float rate = std::max(properties.updateRate, 1.F);
        armarx::Metronome metronome(
            armarx::Duration::MilliSeconds(static_cast<std::int64_t>(1000.F / rate)));
 
        while (not task->isStopped())
        {
            const viz::CommitResult result = arviz.commit(stage);
 
            // The stage is rebuilt from scratch: only layers that actually changed are
            // re-committed by the editor.
            stage.reset();
 
            editor->update(result, stage);
 
            // Never pull the rug from underneath an ongoing interaction.
            if (not editor->isInteracting() and reloadRequested.exchange(false))
            {
                reload();
                editor->redrawAll(stage);
            }
 
            metronome.waitForNextTick();
        }
    }
 
    bool
    Component::reload()
    {
        ARMARX_CHECK_NOT_NULL(editor);
 
        memory::client::rooms::Reader::Query query;
        query.timestamp = armarx::Clock::Now();
        if (not properties.providerSegmentName.empty())
        {
            query.providerName = properties.providerSegmentName;
        }
 
        const auto result = roomsReaderPlugin->get().query(query);
 
        if (not result)
        {
            ARMARX_WARNING << "Could not read the rooms from the navigation memory: "
                           << result.errorMessage;
            editor->setRooms({});
            numRooms = 0;
            return false;
        }
 
        std::vector<EditableRoom> rooms;
        rooms.reserve(result.entries.size());
 
        for (const auto& entry : result.entries)
        {
            EditableRoom editable{.providerSegmentName = entry.instanceID.providerSegmentName,
                                  .room = entry.room};
 
            if (editable.room.name.empty())
            {
                // The entity name is the authoritative room name.
                editable.room.name = entry.instanceID.entityName;
            }
 
            rooms.push_back(std::move(editable));
        }
 
        // Stable ordering, so that the colors of the rooms do not jump around on reload.
        std::sort(rooms.begin(),
                  rooms.end(),
                  [](const EditableRoom& lhs, const EditableRoom& rhs)
                  {
                      return std::tie(lhs.providerSegmentName, lhs.room.name) <
                             std::tie(rhs.providerSegmentName, rhs.room.name);
                  });
 
        ARMARX_INFO << "Read " << rooms.size() << " room(s) from the navigation memory.";
 
        numRooms = static_cast<int>(rooms.size());
        editor->setRooms(rooms);
 
        return true;
    }
 
    void
    Component::storeRoom(const EditableRoom& editable)
    {
        if (editable.providerSegmentName.empty())
        {
            ARMARX_ERROR << "Cannot write room `" << editable.room.name
                         << "` back into the memory: the provider segment is unknown.";
            return;
        }
 
        if (not roomsWriterPlugin->get().store(
                editable.room, editable.providerSegmentName, armarx::Clock::Now()))
        {
            ARMARX_ERROR << "Failed to write room `" << editable.room.name
                         << "` back into the navigation memory.";
            return;
        }
 
        ARMARX_INFO << "Updated room `" << editable.room.name << "` in the navigation memory.";
    }
 
    bool
    Component::exportRooms()
    {
        std::string packageName;
        std::string directory;
        {
            std::scoped_lock lock(exportPathMutex);
            packageName = properties.exportPackageName;
            directory = properties.exportDirectory;
        }
 
        const auto navigationMemoryPrx = memory::NavigationMemoryInterfacePrx::uncheckedCast(
            roomsReaderPlugin->get().readingPrx());
 
        if (not navigationMemoryPrx)
        {
            ARMARX_ERROR << "Export failed: not connected to a navigation memory.";
            return false;
        }
 
        const armarx::PackagePath packagePath(packageName, directory);
 
        try
        {
            if (not navigationMemoryPrx->storeRooms(packagePath.serialize()))
            {
                ARMARX_ERROR << "Export failed: the navigation memory could not store the rooms.";
                return false;
            }
        }
        catch (const Ice::Exception& e)
        {
            ARMARX_ERROR << "Export failed: " << e.what();
            return false;
        }
 
        ARMARX_IMPORTANT << "Exported the rooms to package `" << packageName << "`, directory `"
                         << directory << "`.";
        return true;
    }
 
    void
    Component::reloadFromMemory(const Ice::Current& /*current*/)
    {
        reloadRequested = true;
    }
 
    bool
    Component::exportToJson(const Ice::Current& /*current*/)
    {
        return exportRooms();
    }
 
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        tab.statusLabel.setText("Editing " + std::to_string(numRooms.load()) + " room(s).");
 
        tab.reloadButton.setLabel("Reload from memory");
 
        tab.exportPackageName.setValue(properties.exportPackageName);
        tab.exportDirectory.setValue(properties.exportDirectory);
        tab.exportButton.setLabel("Export rooms to JSON");
        tab.exportLabel.setText("");
 
        GridLayout exportGrid;
        int row = 0;
        {
            exportGrid.add(Label("Package:"), {row, 0}).add(tab.exportPackageName, {row, 1});
            ++row;
 
            exportGrid.add(Label("Data path:"), {row, 0}).add(tab.exportDirectory, {row, 1});
            ++row;
 
            exportGrid.add(tab.exportButton, {row, 0}).add(tab.exportLabel, {row, 1});
            ++row;
        }
 
        GroupBox exportGroup;
        exportGroup.setLabel("Export");
        exportGroup.addChild(exportGrid);
 
        VBoxLayout root = {tab.statusLabel, tab.reloadButton, exportGroup, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    Component::RemoteGui_update()
    {
        tab.statusLabel.setText("Editing " + std::to_string(numRooms.load()) + " room(s).");
 
        if (tab.reloadButton.wasClicked())
        {
            reloadRequested = true;
        }
 
        if (tab.exportButton.wasClicked())
        {
            {
                std::scoped_lock lock(exportPathMutex);
                properties.exportPackageName = tab.exportPackageName.getValue();
                properties.exportDirectory = tab.exportDirectory.getValue();
            }
 
            tab.exportLabel.setText(exportRooms() ? "Exported the rooms."
                                                  : "Export failed. See the log for details.");
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::navigation::components::room_editor