Component.h

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
 */
 
#pragma once
 
#include <atomic>
#include <mutex>
#include <optional>
#include <random>
#include <string>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/components/navigation_evaluator/ComponentInterface.h>
#include <armarx/navigation/memory/client/rooms/Reader.h>
#include <armarx/navigation/skills/aron/NavigateTo.aron.generated.h>
 
namespace armarx::navigation::components::navigation_evaluator
{
 
    class Component :
        virtual public armarx::Component,
        virtual public armarx::navigation::components::navigation_evaluator::ComponentInterface,
        virtual public armarx::armem::ListeningClientPluginUser,
        virtual public armarx::LightweightRemoteGuiComponentPluginUser
    {
    public:
        Component();
 
        std::string getDefaultName() const override;
        static std::string GetDefaultName();
 
    protected:
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
 
        void createRemoteGuiTab();
        void RemoteGui_update() override;
 
        void run();
 
    private:
        static const std::string defaultName;
 
        struct Properties
        {
            std::string roomsProviderName;
            std::string skillProviderName = "navigation_skill_provider";
            float failureTimeoutSeconds = 5.0F;
        };
 
        Properties properties;
 
        // Skill manager proxy (SkillMemory component)
        ::armarx::skills::manager::dti::SkillManagerInterfacePrxPtr manager;
 
        // Skill proxy for NavigateTo
        std::unique_ptr<armarx::skills::SpecializedSkillProxy<skills::arondto::NavigateToParams>>
            navigateToSkillProxy;
 
        // Room reader plugin
        armem::client::plugins::ReaderWriterPlugin<memory::client::rooms::Reader>*
            roomsReaderPlugin = nullptr;
 
        // Worker thread
        RunningTask<Component>::pointer_type evaluationTask;
 
        // Random number generation
        mutable std::mt19937 randomGenerator;
 
        // Thread-safe state
        std::atomic<bool> running{false};
        std::atomic<bool> stopRequested{false};
        std::atomic<std::uint64_t> successCount{0};
        std::atomic<std::uint64_t> totalAttempts{0};
        std::atomic<std::uint64_t> failureCount{0};
 
        mutable std::mutex settingsMutex;
        std::string selectedRoomName;
        skills::arondto::GlobalPlanningAlgorithm::ImplEnum selectedAlgorithm =
            skills::arondto::GlobalPlanningAlgorithm::SPFA;
 
        // GUI
        struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
        {
            armarx::RemoteGui::Client::LineEdit roomSelector;
            armarx::RemoteGui::Client::Button refreshRoomsButton;
            armarx::RemoteGui::Client::Label availableRoomsLabel;
            armarx::RemoteGui::Client::ComboBox algorithmSelector;
            armarx::RemoteGui::Client::Button startButton;
            armarx::RemoteGui::Client::Button stopButton;
            armarx::RemoteGui::Client::Label successCounter;
            armarx::RemoteGui::Client::Label totalCounter;
            armarx::RemoteGui::Client::Label failureCounter;
            armarx::RemoteGui::Client::Label statusLabel;
        };
 
        RemoteGuiTab tab;
 
        // Helpers
        std::vector<algorithms::Room> queryRooms() const;
        std::optional<algorithms::Room> findRoom(const std::string& name) const;
        std::optional<Eigen::Isometry3f> samplePoseInRoom(const algorithms::Room& room) const;
        skills::arondto::GlobalPlanningAlgorithm::ImplEnum algorithmFromString(
            const std::string& name) const;
        std::string algorithmToString(
            skills::arondto::GlobalPlanningAlgorithm::ImplEnum algorithm) const;
        bool executeSingleNavigation(
            const Eigen::Isometry3f& targetPose,
            skills::arondto::GlobalPlanningAlgorithm::ImplEnum algorithm);
        bool tolerateFailure(const armarx::Duration& elapsed);
        void updateGuiCounters();
    };
 
} // namespace armarx::navigation::components::navigation_evaluator