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/algorithms/parametrization/Toppra.h>
#include <armarx/navigation/algorithms/parametrization/TrajectoryParametrization.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;
 
                // If the safety guard limits the platform velocity close to zero for a
                // sustained period, the robot is considered blocked (e.g. by an obstacle
                // that the global plan does not account for) and the global planner is run
                // again towards the current goal.
                struct
                {
                    bool enabled = true;
                    // limits below which the respective axis counts as blocked
                    float linearLimit = 1.F; // [mm/s]
                    float angularLimit = 0.01F; // [rad/s]
                    // how long the robot has to stay blocked before replanning [s]
                    float blockedTimeSeconds = 2.F;
                } blockedReplanning;
 
                // Which platform's drive parameters the TOPP-RA parametrization uses: the
                // suffix selecting `config/platform/PlatformDynamics<Platform>.json` inside the
                // `armarx_navigation` package, resolved by `algorithms::DriveParamsPath`. Same
                // convention as the analysis application's `--robot`, so a platform is named
                // once and the navigator, the analysis tool and the benchmark all agree.
                //
                // Spelled as the rest of the codebase does: `Armar7`, `Armar6`, `ArmarDE`.
                // Set `platformDynamicsEnabled` to false to disable TOPP-RA entirely.
                std::string platform{"Armar7"};
 
                // Whether to read drive parameters at all. False means TOPP-RA is unavailable
                // and the stack falls back to ramping -- the escape hatch for a platform that
                // has no config file yet.
                bool platformDynamicsEnabled{true};
 
                // Write a record of every parametrized navigation request, for offline
                // analysis with `plot-navigation-request`. Off by default -- this is a
                // debugging aid and it writes a file per request.
                bool parametrizationDump{false};
                std::string parametrizationDumpPath{
                    "/tmp/navigation-request-parametrization.json"};
            };
 
            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);
 
        // monitors the safety guard limits and re-runs the global planner if the robot
        // has been blocked (limits near zero) for a sustained period
        void checkRobotBlocked(const safety_guard::SafetyGuardResult& result);
 
        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;
 
        /// Parametrize `trajectory` in place, degrading to ramping rather than failing.
        ///
        /// Also emits the per-request record when `general.parametrizationDump` is set.
        void applyParametrization(core::GlobalTrajectory& trajectory, float startVelocity);
 
        /**
         * @brief Re-anchor the running trajectory at the robot's pose after an emergency stop.
         *
         * A `GlobalTrajectory` assigns a velocity to a *position*. The robot stands still where
         * the profile it was executing assigns cruise speed, so resuming on that trajectory
         * steps the commanded twist from zero and the device ramp answers at its own maximum --
         * far above the acceleration the path was parametrized for. Parametrizing the remainder
         * *from here* puts the profile's `boundaryVelocity` floor at the robot instead, and it
         * ramps up under the planned bound.
         *
         * Leaves `globalPlan` untouched: only the trajectory handed to the executor is replaced,
         * so segment bookkeeping and the goal monitor keep referring to the original plan.
         */
        void resumeAfterEmergencyStop();
 
        bool hasLocalPlanner() const noexcept;
        bool hasSafetyGuard() const;
 
        Config config;
 
        InjectedServices srv;
 
        /**
         * @brief Assigns the velocities along a freshly started path segment.
         *
         * Which variant this is comes from `GeneralConfig::parametrization`; the default,
         * `Ramping`, is what this class used to do inline.
         */
        /// The drive parameters TOPP-RA was built from, when it could be built at all.
        /// Kept so the per-request record can report the bounds that shaped the profile.
        /// Declared first: the initialisers below read it.
        std::optional<algorithms::Toppra::DriveParams> driveParams;
 
        /// What the parametrization actually is, which is not always what was configured:
        /// TOPP-RA degrades to ramping when it is unavailable or cannot be initialised.
        /// Declared before `parametrization` so it is written by its initialiser.
        core::TrajectoryParametrization parametrizationMode{
            core::TrajectoryParametrization::Ramping};
 
        algorithms::TrajectoryParametrizationPtr parametrization;
 
        /// Always available, for the per-request fallback when a solve fails.
        algorithms::TrajectoryParametrizationPtr rampingFallback;
 
        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;
 
        // timestamp since when the robot is continuously blocked by the safety guard
        // (nullopt while the robot is able to move)
        std::optional<armarx::DateTime> blockedSince;
 
        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