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::Navigator
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <unordered_map>
#include <vector>
 
#include <Eigen/Core>
 
#include <Ice/Current.h>
#include <Ice/Object.h>
#include <IceUtil/Optional.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
 
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/memory/client/costmap3d/Reader.h>
#include <armarx/navigation/memory/client/events/Writer.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Writer.h>
#include <armarx/navigation/memory/client/stack_result/Writer.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
#include <armarx/navigation/server/execution/IceEmergencyStop.h>
#include <armarx/navigation/server/execution/PlatformControllerExecutor.h>
#include <armarx/navigation/server/introspection/ArvizIntrospector.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
#include <armarx/navigation/server/scene_provider/SceneProvider.h>
 
namespace armarx::navigation::components::navigator
{
 
    /**
     * @defgroup Component-Navigator Navigator
     * @ingroup armarx_navigation-Components
     * A description of the component Navigator.
     *
     * @class Navigator
     * @ingroup Component-Navigator
     * @brief Brief description of class Navigator.
     *
     * Detailed description of class Navigator.
     */
    class Component :
        virtual public armarx::Component,
        virtual public client::NavigatorInterface,
        virtual public ObjectPoseClientPluginUser,
        virtual public ArVizComponentPluginUser,
        virtual public armarx::control::client::ComponentPluginUser,
        virtual public armarx::LightweightRemoteGuiComponentPluginUser,
        virtual public armarx::DebugObserverComponentPluginUser
    // virtual public armem::ListeningClientPluginUser
    {
 
    public:
        Component();
        ~Component() override;
 
        /// @see armarx::ManagedIceObject::getDefaultName()
        std::string getDefaultName() const override;
 
        static std::string GetDefaultName();
 
        void createConfig(const aron::data::dto::DictPtr& stackConfig,
                          const std::string& callerId,
                          const Ice::Current& c = Ice::emptyCurrent) override;
 
        void removeConfig(const std::string& callerId,
                          const Ice::Current& c = Ice::emptyCurrent) override;
 
        void moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
                    const std::string& navigationMode,
                    const std::string& callerId,
                    const Ice::Current& c = Ice::emptyCurrent) override;
 
        void moveTo2(const client::detail::Waypoints& waypoints,
                     const std::string& navigationMode,
                     const std::string& callerId,
                     const Ice::Current& c = Ice::emptyCurrent) override;
 
        void moveToAlternatives(const client::detail::TargetAlternatives& targets,
                                const std::string& navigationMode,
                                const std::string& callerId,
                                const Ice::Current& c = Ice::emptyCurrent) override;
 
        void moveToLocation(const std::string& location,
                            const IceUtil::Optional<std::string>& providerName,
                            const std::string& callerId,
                            const Ice::Current& c = Ice::emptyCurrent) override;
 
        void updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
                          const std::string& navigationMode,
                          const std::string& callerId,
                          const Ice::Current& c = Ice::emptyCurrent) override;
 
        void moveTowards(const Eigen::Vector3f& direction,
                         const std::string& navigationMode,
                         const std::string& callerId,
                         const Ice::Current& c = Ice::emptyCurrent) override;
 
        void setVelocityFactor(float velocityFactor,
                               const std::string& callerId,
                               const Ice::Current& c = Ice::emptyCurrent) override;
 
        void pause(const std::string& callerId, const Ice::Current& c = Ice::emptyCurrent) override;
 
        void resume(const std::string& callerId,
                    const Ice::Current& c = Ice::emptyCurrent) override;
 
        void stop(const std::string& callerId, const Ice::Current& c = Ice::emptyCurrent) override;
 
        void stopAll(const Ice::Current& c = Ice::emptyCurrent) override;
 
        bool isPaused(const std::string& callerId,
                      const Ice::Current& c = Ice::emptyCurrent) override;
 
        bool isStopped(const std::string& callerId,
                       const Ice::Current& c = Ice::emptyCurrent) override;
 
        const core::Scene& scene() const;
 
    protected:
        /// @see PropertyUser::createPropertyDefinitions()
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        /// @see armarx::ManagedIceObject::onInitComponent()
        void onInitComponent() override;
 
        /// @see armarx::ManagedIceObject::onConnectComponent()
        void onConnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onDisconnectComponent()
        void onDisconnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onExitComponent()
        void onExitComponent() override;
 
        /// This function should be called once in onConnect() or when you
        /// need to re-create the Remote GUI tab.
        void createRemoteGuiTab();
 
        /// After calling `RemoteGui_startRunningTask`, this function is
        /// called periodically in a separate thread. If you update variables,
        /// make sure to synchronize access to them.
        void RemoteGui_update() override;
 
        bool initializeScene();
 
        void stopAllImpl(bool emitMemoryEvent);
 
 
        // core::StaticScene staticScene();
 
        // [[nodiscard]] VirtualRobot::RobotPtr getRobot();
        // void updateRobot();
 
        server::Navigator* activeNavigator();
        std::optional<std::string> activeNavigatorId() const;
 
    private:
        void visualizeSPFA();
 
        // core::Scene scene;
 
        /// Consulted by the executor before it re-activates a controller. Empty when no
        /// emergency-stop master could be resolved, which the executor reads as "this
        /// deployment has none" and proceeds. Declared before `executor`, which holds a pointer
        /// into it.
        std::optional<server::IceEmergencyStop> emergencyStop;
 
        std::optional<server::PlatformControllerExecutor> executor;
        std::optional<server::ArvizIntrospector> introspector;
        // std::optional<server::MemoryIntrospector> memoryIntrospector;
        std::optional<server::scene_provider::SceneProvider> sceneProvider;
 
        std::unordered_map<std::string, server::Navigator> navigators;
 
        std::mutex propertiesMutex;
 
        // unique_ptr to avoid dangling refs
        std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
 
        // `navigation` memory reader and writer
        armem::client::plugins::ReaderWriterPlugin<memory::client::param::Reader>*
            parameterizationReaderPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::param::Writer>*
            parameterizationWriterPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::events::Writer>*
            eventsWriterPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::stack_result::Writer>*
            resultsWriterPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::graph::Reader>*
            graphReaderPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
            costmapReaderPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::costmap3d::Reader>*
            costmap3dReaderPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::human::Reader>*
            humanReaderPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<memory::client::laser_scanner_features::Reader>*
            laserScannerFeaturesReaderPlugin = nullptr;
 
        // armem::vision::occupancy_grid::client::Reader occupancyGridReader;
 
        // `robot_state` memory reader and writer
        std::optional<armem::robot_state::description::RobotDescription> robotDescription;
        armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
            virtualRobotReaderPlugin = nullptr;
 
 
        server::MemoryParameterizationService parameterizationService;
        // server::MemoryPublisher publisher;
 
        // key is `callerId`
        std::unordered_map<std::string, std::unique_ptr<server::MemoryPublisher>> memoryPublishers;
 
        struct Parameters
        {
            float occupiedGridThreshold{0.55F};
            bool disableExecutor{false};
 
            server::scene_provider::SceneProvider::Config sceneCfg;
 
            bool rtUnitDynamicallyLoadLibraries = false;
 
            std::string emergencyStopMasterName{"EmergencyStopMaster"};
 
            server::Navigator::Config::General navigatorGeneralConfig;
            server::GoalReachedMonitorConfig navigatorGoalReachedConfig;
        };
 
        Parameters params;
 
        struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
        {
            armarx::RemoteGui::Client::Button stopAll;
        };
 
        RemoteGuiTab tab;
    };
} // namespace armarx::navigation::components::navigator