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/core/time/DateTime.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/DrawerInterface.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;
    }
 
    // Allow global path to be subdivided into segments that are executed independently
    // each segment can be executed by the global or local planner
    struct GlobalPathSubdivision
    {
        // A segment from index [s,t)
        struct Subdivision
        {
            std::size_t s;
            std::size_t t;
            bool useLocalPlanner;
        };
 
        global_planning::GlobalPlannerResult plan;
        std::vector<Subdivision> subdivision;
        std::size_t currentSegment;
        core::GlobalTrajectory currentSegmentTrajectory;
 
        bool useLocalPlanner(bool hasLocalPlanner);
        const core::GlobalTrajectory& currentGlobalSegment() const;
        core::GlobalTrajectory& currentGlobalSegment();
 
        GlobalPathSubdivision(const global_planning::GlobalPlannerResult& globalPlan);
    };
 
    class Navigator : virtual public core::NavigatorInterface
    {
 
    public:
        struct Config
        {
            struct General
            {
                struct
                {
                    int replanningUpdatePeriod{100}; // [ms]
                } tasks;
 
                // Whether to subdivide the global trajectory into segments that are executed independently.
                // Requires local planner
 
                // The global plan is divided into segments that can be executed by the local planner and those
                // where the global planner should be used:
                //   - a part of the trajectory is ineligible for the local planner if the corresponding costmap
                //     entry has a value less than the localPlannerCostmapThreshold parameter
                // Then, each segment of continuous trajectory points where the global planner should be used is expanded
                // by globalPlanExpansionDistance into both directions.
                // Then, each segment of continuous trajectory points where the local planner should be used that is less
                // than minSegmentDistance long will be remove and joined with the neighboring globalPlanner segments.
                struct
                {
                    bool enable{false};
                    // the minimum distance in the costmap to enable the local planner [mm]
                    float localPlannerCostmapThreshold = 500;
                    float globalPlanExpansionDistance = 300; // [mm]
                    float minSegmentDistance = 500; // [mm]
                } subdivision;
 
                // numer of seconds to wait before replanning alternatives after all
                // alternatives where impossible [s]
                float targetAlternativesFilterTimeSeconds = 5.F;
            };
 
            server::NavigationStack stack;
            General general;
 
            GoalReachedMonitorConfig goalReachedConfig;
        };
 
        struct InjectedServices
        {
            ExecutorInterface* executor;
            EventPublishingInterface* publisher;
            IntrospectorInterface* introspector = nullptr;
            DrawerInterface* drawer = 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 moveToAlternatives(const std::vector<core::TargetAlternative>& targets,
                                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,
                            const std::optional<std::string>& providerName) 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, bool = true);
        void moveTowardsAbsolute(const core::Direction& direction);
 
        void updateAbsolute(const std::vector<core::Pose>& waypoints);
 
        void moveToAbsoluteAlternatives(const std::vector<core::TargetAlternative>& targets);
        bool setupTargetAlternatives();
 
        void setupGlobalPlanSubvidision();
 
        // return true iff globalPath was already executed fully
        bool startGlobalPathSegment(bool incrementSegment, bool rampFromCurrentVelocity);
 
        void run();
 
        void updateScene(bool fullUpdate = false);
 
        void checkGlobalPathAlternatives();
        bool verifyGlobalPathPossible(const core::GlobalTrajectory& plan);
 
        std::optional<local_planning::LocalPlannerResult> updateLocalPlanner();
        void updateExecutor(const std::optional<local_planning::LocalPlannerResult>& localPlan);
        void updateExecutor(const core::GlobalTrajectory& globalTrajectory);
        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;
 
        void setupRamping(core::GlobalTrajectory& trajectory, float startVelocity) const;
 
        bool hasLocalPlanner() const noexcept;
        bool hasSafetyGuard() const;
 
        Config config;
 
        InjectedServices srv;
 
        std::atomic_bool executorEnabled = true;
        std::atomic_bool shouldRun = false;
 
        PeriodicTask<Navigator>::pointer_type runningTask = nullptr;
 
        std::optional<GoalReachedMonitor> goalReachedMonitor;
 
        std::vector<core::TargetAlternative> targetAlternatives;
        std::optional<armarx::DateTime> lastAllAlternativesImpossible;
 
        std::optional<GlobalPathSubdivision> 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;
 
        std::mutex runningTaskMtx;
    };
 
} // namespace armarx::navigation::server