Navigator.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/>.
 *
 * @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 <atomic>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
 
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/NavigatorInterface.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_guard/SafetyGuard.h>
#include <armarx/navigation/server/GraphBuilder.h>
#include <armarx/navigation/server/NavigationStack.h>
#include <armarx/navigation/server/event_publishing/EventPublishingInterface.h>
#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/IntrospectorInterface.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
 
#include "monitoring/GoalReachedMonitor.h"
#include <SemanticObjectRelations/Shapes/Shape.h>
 
namespace armarx::navigation::server
{
 
    namespace scene_provider
    {
        class SceneProviderInterface;
    }
 
    class Navigator : virtual public core::NavigatorInterface
    {
 
    public:
        struct Config
        {
            struct General
            {
                struct
                {
                    int replanningUpdatePeriod{100}; // [ms]
                } tasks;
            };
 
            server::NavigationStack stack;
            General general;
 
            GoalReachedMonitorConfig goalReachedConfig;
        };
 
        struct InjectedServices
        {
            ExecutorInterface* executor;
            EventPublishingInterface* publisher;
            IntrospectorInterface* introspector = nullptr;
 
            armarx::DebugObserverHelper* debugObserverHelper;
 
            scene_provider::SceneProviderInterface* sceneProvider;
        };
 
        Navigator(const Config& config, const InjectedServices& services);
 
        void moveTo(const std::vector<core::Pose>& waypoints,
                    core::NavigationFrame navigationFrame) override;
 
        void update(const std::vector<core::Pose>& waypoints,
                    core::NavigationFrame navigationFrame) override;
 
 
        void moveTo(const std::vector<client::WaypointTarget>& targets,
                    core::NavigationFrame navigationFrame) override;
 
        void moveTowards(const core::Direction& direction,
                         core::NavigationFrame navigationFrame) override;
 
        void moveToLocation(const std::string& location) override;
 
        void setVelocityFactor(float velocityFactor) override;
 
        void pause() override;
 
        void resume() override;
 
        bool isPaused() const noexcept override;
 
        void stop() override;
 
        bool isStopped() const noexcept override;
 
        // Non-API
    public:
        Navigator(Navigator&& other) noexcept;
 
        ~Navigator() override;
 
    private:
        using GraphPath = std::vector<semrel::ShapeID>;
 
        void moveToAbsolute(const std::vector<core::Pose>& waypoints);
        void moveTowardsAbsolute(const core::Direction& direction);
 
        void updateAbsolute(const std::vector<core::Pose>& waypoints);
 
        void run();
 
        void updateScene(bool fullUpdate = false);
 
        std::optional<local_planning::LocalPlannerResult> updateLocalPlanner();
        void updateExecutor(const std::optional<local_planning::LocalPlannerResult>& localPlan);
        void updateExecutor(const std::optional<global_planning::GlobalPlannerResult>& localPlan);
        void updateIntrospector(const std::optional<local_planning::LocalPlannerResult>& localPlan);
 
        void updateMonitor();
 
        safety_guard::SafetyGuardResult updateSafetyGuard();
 
        // const core::Trajectory& currentTrajectory() const;
        // bool isStackResultValid() const noexcept;
 
        void stopAllThreads();
 
        void startStack();
 
        // helper methods
        void setGraphEdgeCosts(core::Graph& graph) const;
        GraphBuilder convertToGraph(const std::vector<client::WaypointTarget>& targets) const;
 
        core::GlobalTrajectory convertToTrajectory(const GraphPath& shortestPath,
                                                   const core::Graph& graph) const;
 
        core::Pose resolveGraphVertex(const core::Graph::ConstVertex& vertex) const;
 
        bool hasLocalPlanner() const noexcept;
        bool hasSafetyGuard() const;
 
        Config config;
 
        InjectedServices srv;
 
        std::atomic_bool executorEnabled = true;
 
        PeriodicTask<Navigator>::pointer_type runningTask = nullptr;
 
        std::optional<GoalReachedMonitor> goalReachedMonitor;
 
 
        std::optional<global_planning::GlobalPlannerResult> globalPlan;
        std::optional<local_planning::LocalPlannerResult> localPlan;
 
        using Waypoints = std::vector<core::Pose>;
 
        std::mutex globalPlanningRequestMtx;
        std::optional<Waypoints> globalPlanningRequest;
 
        std::atomic<float> velocityFactor = 1.F;
 
        std::mutex updateLocalPlannerMtx;
 
        std::mutex runMtx;
    };
 
} // namespace armarx::navigation::server