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::navigation_evaluator
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
#include <cmath>
#include <cstdint>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/Component.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/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/navigation/memory/client/rooms/Reader.h>
#include <armarx/navigation/skills/constants.h>
#include <armarx/navigation/skills/skill_ids.h>
 
namespace armarx::navigation::components::navigation_evaluator
{
    const std::string Component::defaultName = "navigation_evaluator";
 
    Component::Component()
    {
        addPlugin(roomsReaderPlugin);
 
        std::random_device rd;
        randomGenerator.seed(rd());
    }
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->component(manager, "SkillMemory");
 
        def->optional(properties.roomsProviderName,
                      "p.roomsProviderName",
                      "Provider segment name for rooms in the navigation memory. "
                      "Leave empty to query all providers.");
 
        def->optional(properties.skillProviderName,
                      "p.skillProviderName",
                      "Name of the skill provider that offers NavigateTo.");
 
        def->optional(properties.failureTimeoutSeconds,
                      "p.failureTimeoutSeconds",
                      "Failures within this time [s] after skill start are tolerated; "
                      "later failures stop the evaluation.");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Nothing special to initialize.
    }
 
    void
    Component::onConnectComponent()
    {
        ARMARX_CHECK_NOT_NULL(manager) << "SkillMemory dependency is not configured.";
 
        const armarx::skills::SkillID navigateToSkillId{
            .providerId =
                armarx::skills::ProviderID{.providerName = properties.skillProviderName},
            .skillName = skills::constants::skill_names::NavigateTo};
 
        try
        {
            navigateToSkillProxy =
                std::make_unique<armarx::skills::SpecializedSkillProxy<skills::arondto::NavigateToParams>>(
                    manager, navigateToSkillId);
        }
        catch (const std::exception& e)
        {
            ARMARX_ERROR << "Failed to create NavigateTo skill proxy: " << e.what();
            return;
        }
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
 
        evaluationTask = new RunningTask<Component>(this, &Component::run, "NavigationEvaluator");
        evaluationTask->start();
    }
 
    void
    Component::onDisconnectComponent()
    {
        stopRequested.store(true, std::memory_order_release);
 
        if (evaluationTask)
        {
            evaluationTask->stop(true);
        }
 
        navigateToSkillProxy.reset();
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        tab.refreshRoomsButton.setLabel("Refresh Rooms");
        tab.roomSelector.setValue("");
        tab.availableRoomsLabel.setText("Available rooms: <click Refresh>");
        tab.algorithmSelector.setOptions({"SPFA", "AStar", "AStarWithOrientation", "Point2Point"});
        tab.algorithmSelector.setValue("SPFA");
        tab.startButton.setLabel("Start");
        tab.stopButton.setLabel("Stop");
        tab.successCounter.setText("Successful: 0");
        tab.totalCounter.setText("Total: 0");
        tab.failureCounter.setText("Failed: 0");
        tab.statusLabel.setText("Status: stopped");
 
        GridLayout grid;
        int row = 0;
 
        grid.add(Label("Room"), {row, 0}).add(tab.roomSelector, {row, 1});
        ++row;
        grid.add(tab.refreshRoomsButton, {row, 0}, {1, 2});
        ++row;
        grid.add(tab.availableRoomsLabel, {row, 0}, {1, 2});
        ++row;
 
        grid.add(Label("Algorithm"), {row, 0}).add(tab.algorithmSelector, {row, 1});
        ++row;
 
        grid.add(tab.startButton, {row, 0}).add(tab.stopButton, {row, 1});
        ++row;
 
        grid.add(tab.statusLabel, {row, 0}, {1, 2});
        ++row;
        grid.add(tab.successCounter, {row, 0}, {1, 2});
        ++row;
        grid.add(tab.totalCounter, {row, 0}, {1, 2});
        ++row;
        grid.add(tab.failureCounter, {row, 0}, {1, 2});
        ++row;
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    Component::RemoteGui_update()
    {
        if (tab.refreshRoomsButton.wasClicked())
        {
            const auto rooms = queryRooms();
            std::vector<std::string> roomNames;
            for (const auto& room : rooms)
            {
                roomNames.push_back(room.name);
            }
 
            if (roomNames.empty())
            {
                tab.availableRoomsLabel.setText("Available rooms: <none>");
            }
            else
            {
                std::string joined;
                for (std::size_t i = 0; i < roomNames.size(); ++i)
                {
                    if (i > 0)
                    {
                        joined += ", ";
                    }
                    joined += roomNames[i];
                }
                tab.availableRoomsLabel.setText("Available rooms: " + joined);
            }
        }
 
        if (tab.startButton.wasClicked() && not running.load(std::memory_order_acquire))
        {
            const std::string roomName = tab.roomSelector.getValue();
            if (roomName.empty())
            {
                tab.statusLabel.setText("Status: enter a room name");
                updateGuiCounters();
                return;
            }
 
            const auto room = findRoom(roomName);
            if (not room)
            {
                tab.statusLabel.setText("Status: room '" + roomName + "' not found");
                updateGuiCounters();
                return;
            }
 
            {
                std::lock_guard g{settingsMutex};
                selectedRoomName = roomName;
                selectedAlgorithm = algorithmFromString(tab.algorithmSelector.getValue());
            }
 
            successCount.store(0, std::memory_order_release);
            totalAttempts.store(0, std::memory_order_release);
            failureCount.store(0, std::memory_order_release);
            stopRequested.store(false, std::memory_order_release);
            running.store(true, std::memory_order_release);
 
            tab.statusLabel.setText("Status: running");
            updateGuiCounters();
        }
 
        if (tab.stopButton.wasClicked() && running.load(std::memory_order_acquire))
        {
            stopRequested.store(true, std::memory_order_release);
        }
 
        if (not running.load(std::memory_order_acquire))
        {
            tab.statusLabel.setText("Status: stopped");
        }
 
        updateGuiCounters();
    }
 
    void
    Component::updateGuiCounters()
    {
        tab.successCounter.setText("Successful: " + std::to_string(successCount.load(std::memory_order_acquire)));
        tab.totalCounter.setText("Total: " + std::to_string(totalAttempts.load(std::memory_order_acquire)));
        tab.failureCounter.setText("Failed: " + std::to_string(failureCount.load(std::memory_order_acquire)));
    }
 
    std::vector<algorithms::Room>
    Component::queryRooms() const
    {
        ARMARX_CHECK_NOT_NULL(roomsReaderPlugin);
 
        const std::optional<std::string> providerName =
            properties.roomsProviderName.empty()
                ? std::nullopt
                : std::optional<std::string>{properties.roomsProviderName};
 
        ARMARX_DEBUG << "Querying rooms from navigation memory"
                     << (providerName ? " for provider '" + *providerName + "'" : " (all providers)")
                     << ".";
 
        const memory::client::rooms::Reader::Query query{
            .providerName = providerName,
            .timestamp = armarx::Clock::Now()};
 
        const auto result = roomsReaderPlugin->get().query(query);
        if (not result)
        {
            if (providerName)
            {
                ARMARX_WARNING << "Failed to query rooms from provider '" << *providerName
                               << "': " << result.errorMessage;
            }
            else
            {
                ARMARX_WARNING << "Failed to query rooms from memory: " << result.errorMessage;
            }
            return {};
        }
 
        return result.rooms;
    }
 
    std::optional<algorithms::Room>
    Component::findRoom(const std::string& name) const
    {
        const auto rooms = queryRooms();
        for (const auto& room : rooms)
        {
            if (room.name == name)
            {
                return room;
            }
        }
        return std::nullopt;
    }
 
    std::optional<Eigen::Isometry3f>
    Component::samplePoseInRoom(const algorithms::Room& room) const
    {
        const auto [min, max] = room.aabb();
 
        std::uniform_real_distribution<float> xDist(min.x(), max.x());
        std::uniform_real_distribution<float> yDist(min.y(), max.y());
        std::uniform_real_distribution<float> angleDist(-static_cast<float>(M_PI),
                                                        static_cast<float>(M_PI));
 
        constexpr std::size_t maxSamples = 1000;
        for (std::size_t i = 0; i < maxSamples; ++i)
        {
            const Eigen::Vector2f candidate(xDist(randomGenerator), yDist(randomGenerator));
            if (room.isInside(candidate))
            {
                Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
                pose.translation() << candidate.x(), candidate.y(), 0.0F;
                pose.linear() = Eigen::AngleAxisf(angleDist(randomGenerator),
                                                  Eigen::Vector3f::UnitZ())
                                    .toRotationMatrix();
                return pose;
            }
        }
 
        ARMARX_ERROR << "Could not sample a valid position inside room '" << room.name
                     << "' after " << maxSamples << " attempts.";
        return std::nullopt;
    }
 
    skills::arondto::GlobalPlanningAlgorithm::ImplEnum
    Component::algorithmFromString(const std::string& name) const
    {
        using ImplEnum = skills::arondto::GlobalPlanningAlgorithm::ImplEnum;
        if (name == "AStar")
            return ImplEnum::AStar;
        if (name == "AStarWithOrientation")
            return ImplEnum::AStarWithOrientation;
        if (name == "Point2Point")
            return ImplEnum::Point2Point;
        return ImplEnum::SPFA;
    }
 
    std::string
    Component::algorithmToString(skills::arondto::GlobalPlanningAlgorithm::ImplEnum algorithm) const
    {
        using ImplEnum = skills::arondto::GlobalPlanningAlgorithm::ImplEnum;
        switch (algorithm)
        {
            case ImplEnum::AStar:
                return "AStar";
            case ImplEnum::AStarWithOrientation:
                return "AStarWithOrientation";
            case ImplEnum::Point2Point:
                return "Point2Point";
            case ImplEnum::SPFA:
                return "SPFA";
            default:
                return "SPFA";
        }
    }
 
    bool
    Component::executeSingleNavigation(
        const Eigen::Isometry3f& targetPose,
        skills::arondto::GlobalPlanningAlgorithm::ImplEnum algorithm)
    {
        const auto startTime = armarx::Clock::Now();
 
        skills::arondto::NavigateToParams params;
        params.targetPose = targetPose.matrix();
        params.navigatingSkillParams.globalPlanningAlgorithm = algorithm;
        params.navigatingSkillParams.enableLocalPlanning = false;
        params.navigatingSkillParams.enableSafetyGuard = true;
 
        armarx::skills::TerminatedSkillStatusUpdate finalStatus;
        try
        {
            finalStatus = navigateToSkillProxy->executeSkill(getName(), params);
        }
        catch (const std::exception& e)
        {
            ARMARX_ERROR << "Synchronous NavigateTo execution failed: " << e.what();
            return tolerateFailure(armarx::Clock::Now() - startTime);
        }
 
        const auto elapsed = armarx::Clock::Now() - startTime;
        const bool succeeded = finalStatus.status == armarx::skills::TerminatedSkillStatus::Succeeded;
 
        if (succeeded)
        {
            ARMARX_INFO << "Navigation succeeded after " << elapsed.toSeconds() << " s.";
            successCount.fetch_add(1, std::memory_order_acq_rel);
            return true;
        }
 
        ARMARX_WARNING << "Navigation failed after " << elapsed.toSeconds()
                       << " s with status " << static_cast<int>(finalStatus.status);
 
        return tolerateFailure(elapsed);
    }
 
    bool
    Component::tolerateFailure(const armarx::Duration& elapsed)
    {
        failureCount.fetch_add(1, std::memory_order_acq_rel);
 
        const bool tolerable =
            elapsed < armarx::Duration::SecondsDouble(properties.failureTimeoutSeconds);
 
        if (tolerable)
        {
            ARMARX_INFO << "Failure occurred within " << properties.failureTimeoutSeconds
                        << " s (elapsed " << elapsed.toSeconds()
                        << " s), continuing with next random location.";
            return true;
        }
 
        ARMARX_ERROR << "Failure occurred after " << properties.failureTimeoutSeconds
                     << " s (elapsed " << elapsed.toSeconds() << " s), stopping evaluation.";
        return false;
    }
 
    void
    Component::run()
    {
        while (not evaluationTask->isStopped())
        {
            if (not running.load(std::memory_order_acquire))
            {
                armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(100));
                continue;
            }
 
            std::string roomName;
            skills::arondto::GlobalPlanningAlgorithm::ImplEnum algorithm;
            {
                std::lock_guard g{settingsMutex};
                roomName = selectedRoomName;
                algorithm = selectedAlgorithm;
            }
 
            const auto room = findRoom(roomName);
            if (not room)
            {
                ARMARX_ERROR << "Room '" << roomName << "' not found in memory. Stopping.";
                running.store(false, std::memory_order_release);
                continue;
            }
 
            const auto targetPose = samplePoseInRoom(*room);
            if (not targetPose)
            {
                ARMARX_ERROR << "Failed to sample a pose in room '" << roomName
                             << "'. Stopping.";
                running.store(false, std::memory_order_release);
                continue;
            }
 
            totalAttempts.fetch_add(1, std::memory_order_acq_rel);
 
            const bool continueEvaluation = executeSingleNavigation(*targetPose, algorithm);
            if (not continueEvaluation)
            {
                running.store(false, std::memory_order_release);
            }
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::navigation::components::navigation_evaluator