Navigator.cpp

Go to the documentation of this file.

#include "Navigator.h"
 
#include "ParametrizationDump.h"
 
#include <armarx/navigation/factories/TrajectoryParametrizationFactory.h>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <iterator>
#include <limits>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <utility>
#include <vector>
 
#include <boost/graph/detail/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>
 
#include <Eigen/Geometry>
 
#include <range/v3/algorithm/sort.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/reverse.hpp>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/spfa/smoothing/PathGeometry.h>
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/LocationUtils.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/core/location.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/SPFA.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/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/DrawerInterface.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
 
#include <SemanticObjectRelations/Shapes/Shape.h>
 
namespace armarx::navigation::server
{
 
    GlobalPathSubdivision::GlobalPathSubdivision(
        const global_planning::GlobalPlannerResult& globalPlan) :
        plan(globalPlan), subdivision(), currentSegment(0), currentSegmentTrajectory()
    {
    }
 
    bool
    GlobalPathSubdivision::useLocalPlanner(bool hasLocalPlanner)
    {
        if (subdivision.empty())
        {
            // no subdivision
            return hasLocalPlanner;
        }
 
        ARMARX_CHECK(hasLocalPlanner); // we always need local planner for segment subdivision
        const bool segmentLocalPlanner = subdivision[currentSegment].useLocalPlanner;
 
        return segmentLocalPlanner;
    }
 
    const core::GlobalTrajectory&
    GlobalPathSubdivision::currentGlobalSegment() const
    {
        if (subdivision.empty())
        {
            // no subdivision
            return plan.trajectory;
        }
        return currentSegmentTrajectory;
    }
 
    core::GlobalTrajectory&
    GlobalPathSubdivision::currentGlobalSegment()
    {
        if (subdivision.empty())
        {
            // no subdivision
            return plan.trajectory;
        }
        return currentSegmentTrajectory;
    }
 
    namespace
    {
        /// Build the configured parametrization, falling back to ramping rather than failing.
        ///
        /// The fallback lives here and deliberately *not* in
        /// `TrajectoryParametrizationFactory`: the analysis application calls the same factory
        /// and then reads the time parametrization back out of the `Toppra` instance. A silent
        /// substitution inside the factory would make it write a result labelled `toppra` that
        /// actually contains a ramping profile -- wrong data in the tool used to validate this.
        /// A navigation component must not crash; an offline analysis tool must not lie.
        ///
        /// Three things can go wrong, and all three land here:
        ///   1. TOPP-RA was not compiled in (no python environment at configure time).
        ///   2. No platform dynamics config, or it does not parse / the robot model fails.
        ///   3. Anything the `Toppra` constructor throws while loading the model.
        /// Load the drive parameters, or nothing if they are unavailable or unusable.
        std::optional<algorithms::Toppra::DriveParams>
        loadDriveParams(const core::GeneralConfig& generalConfig,
                        const Navigator::Config::General& general)
        {
            if (generalConfig.parametrization != core::TrajectoryParametrization::Toppra)
            {
                return std::nullopt;
            }
 
            if (not general.platformDynamicsEnabled)
            {
                return std::nullopt;
            }
 
            try
            {
                return algorithms::LoadDriveParams(
                    algorithms::DriveParamsPath(general.platform));
            }
            catch (const std::exception& e)
            {
                ARMARX_WARNING << "Cannot read the platform dynamics config for platform "
                               << QUOTED(general.platform) << ": " << e.what();
                return std::nullopt;
            }
        }
 
        algorithms::TrajectoryParametrizationPtr
        createParametrization(const core::GeneralConfig& generalConfig,
                              const Navigator::Config::General& general,
                              const std::optional<algorithms::Toppra::DriveParams>& driveParams,
                              core::TrajectoryParametrization* applied)
        {
            *applied = generalConfig.parametrization;
 
            const auto rampingInstead = [&](const std::string& reason)
            {
                ARMARX_WARNING << "TOPP-RA trajectory parametrization was requested but " << reason
                               << ". Falling back to ramping.";
 
                core::GeneralConfig ramping = generalConfig;
                ramping.parametrization = core::TrajectoryParametrization::Ramping;
                *applied = core::TrajectoryParametrization::Ramping;
 
                return fac::TrajectoryParametrizationFactory::create(ramping);
            };
 
            if (generalConfig.parametrization != core::TrajectoryParametrization::Toppra)
            {
                return fac::TrajectoryParametrizationFactory::create(generalConfig);
            }
 
            if (not algorithms::Toppra::available())
            {
                return rampingInstead("it is not available in this build: " +
                                      algorithms::Toppra::unavailableReason());
            }
 
            if (not general.platformDynamicsEnabled)
            {
                return rampingInstead("its drive parameters are disabled by "
                                      "`p.navigator.general.platformDynamicsEnabled`");
            }
 
            if (not driveParams.has_value())
            {
                return rampingInstead(
                    "its drive parameters could not be read for platform '" + general.platform +
                    "'. Check `p.navigator.general.platform` against the files in "
                    "data/armarx_navigation/config/platform/");
            }
 
            try
            {
                return fac::TrajectoryParametrizationFactory::create(generalConfig, *driveParams);
            }
            catch (const std::exception& e)
            {
                return rampingInstead(std::string{"it could not be initialised: "} + e.what());
            }
        }
        /// Report an accepted plan the base cannot follow, on the trajectory that reaches it.
        ///
        /// Both checks already existed and neither had a caller.
        /// `findGeometryViolations` does run inside the SPFA smoother, but on the smoothed
        /// *candidate*: when smoothing is rejected the raw resampled path ships unchecked, which
        /// is exactly what happened on ARMAR-7 on 2026-08-29. `findTurnRateViolations` had no
        /// production caller at all.
        ///
        /// Here rather than in a planner because every planner reaches this: SPFA, AStar,
        /// Point2Point and the graph route all converge on one accepted `globalPlan`, and each
        /// has early returns that bypass its own final block. It is also upstream of
        /// `applyParametrization`, so this is provably what TOPP-RA will be handed.
        ///
        /// Reporting only. A plan that is awkward to follow still beats no plan, and the
        /// reparametrization slows down for it rather than failing.
        void
        reportPathGeometry(const core::GlobalTrajectory& trajectory)
        {
            namespace smoothing = algorithms::spfa::smoothing;
 
            if (trajectory.points().size() < 2)
            {
                return;
            }
 
            const smoothing::GeometryLimits limits;
 
            const std::vector<std::size_t> degenerate =
                smoothing::findGeometryViolations(trajectory, limits);
 
            if (not degenerate.empty())
            {
                ARMARX_WARNING << "[nav-guard] plan-geometry-violation: " << degenerate.size()
                               << " of " << trajectory.points().size()
                               << " waypoints of the accepted plan reverse direction or sit on a "
                               << "segment shorter than " << limits.minSegmentLength
                               << " mm; the first is waypoint " << degenerate.front()
                               << ". A heading taken across such a segment is numerical noise.";
            }
 
            const std::vector<std::size_t> unturnable =
                smoothing::findTurnRateViolations(trajectory, limits);
 
            if (not unturnable.empty())
            {
                ARMARX_WARNING << "[nav-guard] plan-unturnable: " << unturnable.size() << " of "
                               << trajectory.points().size() - 1
                               << " segments of the accepted plan turn faster than "
                               << limits.maxTurnRate << " rad/mm, the worst at "
                               << smoothing::maxTurnRate(trajectory, limits)
                               << " rad/mm. The reparametrization will slow the whole trajectory "
                               << "down for them.";
            }
        }
    } // namespace
 
    Navigator::Navigator(const Config& config, const InjectedServices& services) :
        config{config},
        srv{services},
        driveParams{loadDriveParams(config.stack.generalConfig, config.general)},
        parametrization{createParametrization(config.stack.generalConfig,
                                              config.general,
                                              driveParams,
                                              &parametrizationMode)},
        rampingFallback{fac::TrajectoryParametrizationFactory::create(
            [&]
            {
                core::GeneralConfig ramping = config.stack.generalConfig;
                ramping.parametrization = core::TrajectoryParametrization::Ramping;
                return ramping;
            }())}
    {
        ARMARX_INFO << "Trajectory parametrization: " << core::ToString(parametrizationMode);
 
        ARMARX_CHECK_NOT_NULL(srv.sceneProvider) << "The scene provider must be set!";
        // ARMARX_CHECK_NOT_NULL(services.executor) << "The executor service must be set!";
        ARMARX_CHECK_NOT_NULL(services.publisher) << "The publisher service must be set!";
    }
 
    Navigator::~Navigator()
    {
        ARMARX_INFO << "Navigator destructor";
        stop();
 
        stopAllThreads();
    }
 
    void
    Navigator::setGraphEdgeCosts(core::Graph& graph) const
    {
        const auto cost = [](const core::Pose& p1, const core::Pose& p2)
        {
            const core::Pose diff = p1.inverse() * p2;
 
            // FIXME consider rotation as well
            return diff.translation().norm();
        };
 
        // graph -> trajectory (set costs)
        for (auto edge : graph.edges())
        {
            const core::Pose start = resolveGraphVertex(edge.source());
            const core::Pose goal = resolveGraphVertex(edge.target());
 
            switch (edge.attrib().strategy)
            {
                case armarx::navigation::client::GlobalPlanningStrategy::Free:
                {
                    ARMARX_VERBOSE << "Global planning from " << start.translation() << " to "
                                   << goal.translation();
                    ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
 
                    try
                    {
                        const auto globalPlan = config.stack.globalPlanner->plan(start, goal);
                        if (globalPlan.has_value())
                        {
                            ARMARX_CHECK(globalPlan->trajectory.isValid());
 
                            edge.attrib().trajectory = globalPlan->trajectory;
                            ARMARX_VERBOSE << "Free path length: "
                                           << globalPlan->trajectory.length();
                            edge.attrib().cost() = globalPlan->trajectory.length();
                        }
                        else
                        {
                            ARMARX_VERBOSE << "Global planning failed for this edge.";
                            edge.attrib().cost() = std::numeric_limits<float>::max();
                        }
                    }
                    catch (...)
                    {
                        ARMARX_VERBOSE << "Global planning failed due to exception "
                                       << GetHandledExceptionString();
                        edge.attrib().cost() = std::numeric_limits<float>::max();
                    }
 
 
                    break;
                }
                case armarx::navigation::client::GlobalPlanningStrategy::Point2Point:
                {
                    edge.attrib().cost() = cost(start, goal);
                    break;
                }
            }
 
            ARMARX_VERBOSE << "Edge cost: " << edge.attrib().cost();
        }
    }
 
    void
    Navigator::moveTo(const std::vector<core::Pose>& waypoints,
                      core::NavigationFrame navigationFrame)
    {
        ARMARX_INFO << "Received moveTo() request.";
 
        std::vector<core::Pose> globalWaypoints;
        switch (navigationFrame)
        {
            case core::NavigationFrame::Absolute:
                globalWaypoints = waypoints;
                break;
            case core::NavigationFrame::Relative:
                globalWaypoints.reserve(waypoints.size());
 
                ARMARX_CHECK(srv.sceneProvider->synchronize(Clock::Now(), true));
 
                const core::Pose global_T_robot(srv.sceneProvider->scene().robot->getGlobalPose());
                ARMARX_VERBOSE << "Initial robot pose: " << global_T_robot.matrix();
 
                std::transform(std::begin(waypoints),
                               std::end(waypoints),
                               std::back_inserter(globalWaypoints),
                               [&](const core::Pose& p) { return global_T_robot * p; });
                break;
        }
 
        ARMARX_TRACE;
        moveToAbsolute(globalWaypoints);
    }
 
    void
    Navigator::moveToAlternatives(const std::vector<core::TargetAlternative>& targets,
                                  core::NavigationFrame navigationFrame)
    {
        ARMARX_INFO << "Received moveToAlternatives() request.";
 
        std::vector<core::TargetAlternative> globalTargets;
        switch (navigationFrame)
        {
            case core::NavigationFrame::Absolute:
                globalTargets = targets;
                break;
            case core::NavigationFrame::Relative:
                globalTargets.reserve(targets.size());
 
                ARMARX_CHECK(srv.sceneProvider->synchronize(Clock::Now(), true));
 
                const core::Pose global_T_robot(srv.sceneProvider->scene().robot->getGlobalPose());
                ARMARX_VERBOSE << "Initial robot pose: " << global_T_robot.matrix();
 
                std::transform(std::begin(targets),
                               std::end(targets),
                               std::back_inserter(globalTargets),
                               [&](const core::TargetAlternative& p) {
                                   return core::TargetAlternative{
                                       .target = global_T_robot * p.target, .priority = p.priority};
                               });
                break;
        }
 
        ARMARX_TRACE;
        moveToAbsoluteAlternatives(globalTargets);
    }
 
    void
    Navigator::update(const std::vector<core::Pose>& waypoints,
                      core::NavigationFrame navigationFrame)
    {
        ARMARX_INFO << "Received update() request.";
 
        std::vector<core::Pose> globalWaypoints;
        switch (navigationFrame)
        {
            case core::NavigationFrame::Absolute:
                globalWaypoints = waypoints;
                break;
            case core::NavigationFrame::Relative:
                globalWaypoints.reserve(waypoints.size());
                std::transform(
                    std::begin(waypoints),
                    std::end(waypoints),
                    std::back_inserter(globalWaypoints),
                    [&](const core::Pose& p)
                    { return core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
                break;
        }
 
        ARMARX_TRACE;
        updateAbsolute(globalWaypoints);
    }
 
    using GraphPath = std::vector<semrel::ShapeID>;
 
    GraphPath
    findShortestPath(core::Graph graph,
                     const core::Graph::ConstVertex& startVertex,
                     const core::Graph::ConstVertex& goalVertex)
    {
        ARMARX_VERBOSE << "Graph consists of " << graph.numVertices() << " vertices and "
                       << graph.numEdges() << " edges.";
 
        std::vector<core::Graph::VertexDescriptor> predecessors(graph.numVertices());
        std::vector<int> d(num_vertices(graph));
 
        // FIXME ARMARX_CHECK(graphBuilder.startVertex.has_value());
 
        auto weightMap = boost::get(&core::EdgeAttribs::m_value, graph);
 
        core::Graph::VertexDescriptor start = startVertex.descriptor();
 
        auto predecessorMap = boost::make_iterator_property_map(
            predecessors.begin(), boost::get(boost::vertex_index, graph));
 
        auto params = boost::predecessor_map(predecessorMap)
                          .distance_map(boost::make_iterator_property_map(
                              d.begin(), boost::get(boost::vertex_index, graph)))
                          .weight_map(weightMap);
 
        ARMARX_TRACE;
        ARMARX_INFO << graph;
        ARMARX_TRACE;
 
        ARMARX_VERBOSE << "Edge weights:";
        for (const auto& edge : graph.edges())
        {
            ARMARX_VERBOSE << edge.sourceObjectID() << " -> " << edge.targetObjectID() << ": "
                           << edge.attrib().m_value;
        }
 
        std::vector<core::Graph::EdgeDescriptor> edgesToBeRemoved;
        for (const auto edge : graph.edges())
        {
            if (edge.attrib().m_value == std::numeric_limits<float>::max())
            {
                edgesToBeRemoved.push_back(edge.descriptor());
            }
        }
 
        for (const auto edge : edgesToBeRemoved)
        {
            boost::remove_edge(edge, graph);
        }
        ARMARX_VERBOSE << "Edge weights after removal of inf edges:";
 
        for (const auto& edge : graph.edges())
        {
            ARMARX_VERBOSE << edge.sourceObjectID() << " -> " << edge.targetObjectID() << ": "
                           << edge.attrib().m_value;
            ARMARX_VERBOSE << "Edge: " << edge << "cost: " << edge.attrib().cost()
                           << ", type: " << static_cast<int>(edge.attrib().strategy)
                           << " , has traj " << edge.attrib().trajectory.has_value();
        }
 
        ARMARX_VERBOSE << "Searching shortest path from vertex with id `"
                       << graph.vertex(start).objectID() << "` to vertex `"
                       << graph.vertex(goalVertex.descriptor()).objectID() << "`";
 
        boost::dijkstra_shortest_paths(graph, start, params);
 
        // find shortest path
        ARMARX_TRACE;
 
        // WARNING: shortest path will be from goal to start first
        GraphPath shortestPath;
 
        core::Graph::VertexDescriptor currentVertex = goalVertex.descriptor();
        while (currentVertex != startVertex.descriptor())
        {
            shortestPath.push_back(graph.vertex(currentVertex).objectID());
 
            ARMARX_TRACE;
 
            auto parent = predecessorMap[currentVertex];
            ARMARX_VERBOSE << "Parent id: " << parent;
 
            ARMARX_TRACE;
 
            // find edge between parent and currentVertex
            auto outEdges = graph.vertex(parent).outEdges();
            ARMARX_VERBOSE << "Parent has " << std::distance(outEdges.begin(), outEdges.end())
                           << " out edges";
 
            ARMARX_CHECK_GREATER(std::distance(outEdges.begin(), outEdges.end()), 0)
                << "Cannot reach another vertex from vertex `"
                << graph.vertex(parent).objectID(); // not empty
 
            auto edgeIt = std::find_if(outEdges.begin(),
                                       outEdges.end(),
                                       [&currentVertex](const auto& edge) -> bool
                                       { return edge.target().descriptor() == currentVertex; });
 
            ARMARX_CHECK(edgeIt != outEdges.end());
            // shortestPath.edges.push_back(edgeIt->descriptor());
 
            currentVertex = parent;
        }
 
        shortestPath.push_back(startVertex.objectID());
 
        // ARMARX_CHECK_EQUAL(shortestPath.vertices.size() - 1, shortestPath.edges.size());
 
        ARMARX_TRACE;
        // reverse the range => now it is from start to goal again
        shortestPath = shortestPath | ranges::views::reverse | ranges::to_vector;
        // shortestPath.edges = shortestPath.edges | ranges::views::reverse | ranges::to_vector;
 
        ARMARX_TRACE;
        return shortestPath;
    }
 
    core::GlobalTrajectory
    Navigator::convertToTrajectory(const GraphPath& shortestPath, const core::Graph& graph) const
    {
        ARMARX_TRACE;
 
        ARMARX_CHECK_GREATER_EQUAL(shortestPath.size(), 2)
            << "At least start and goal vertices must be available";
 
        // ARMARX_CHECK_EQUAL(shortestPath.size() - 1, shortestPath.edges.size());
 
        std::vector<core::GlobalTrajectoryPoint> trajectoryPoints;
 
        // TODO add the start
        // trajectoryPoints.push_back(core::TrajectoryPoint{
        //     .waypoint = {.pose = graph.vertex(shortestPath.front()).attrib().getPose()},
        //     .velocity = 0.F});
 
        ARMARX_VERBOSE << "Shortest path with " << shortestPath.size() << " vertices";
        ARMARX_VERBOSE << graph;
 
        for (size_t i = 0; i < shortestPath.size() - 1; i++)
        {
            // ARMARX_CHECK(graph.hasEdge(shortestPath.edges.at(i).m_source,
            //                            shortestPath.edges.at(i).m_target));
 
            // TODO add method edge(shapeId, shapeId)
            const core::Graph::ConstEdge edge =
                graph.edge(graph.vertex(shortestPath.at(i)), graph.vertex(shortestPath.at(i + 1)));
 
            ARMARX_TRACE;
            ARMARX_VERBOSE << "Index " << i;
            ARMARX_VERBOSE << static_cast<int>(edge.attrib().strategy);
            ARMARX_VERBOSE << edge;
            switch (edge.attrib().strategy)
            {
                case client::GlobalPlanningStrategy::Free:
                {
                    ARMARX_INFO << "Free navigation on edge";
 
                    ARMARX_TRACE;
                    ARMARX_CHECK(edge.attrib().trajectory.has_value());
 
                    ARMARX_TRACE;
                    ARMARX_INFO << "Length: " << edge.attrib().trajectory->length();
 
 
                    ARMARX_TRACE;
                    ARMARX_INFO << "Free navigation with "
                                << edge.attrib().trajectory->points().size() << " waypoints";
 
                    // we have a trajectory
                    // FIXME trajectory points can be invalid => more points than expected. Why?
                    ARMARX_TRACE;
                    const std::vector<core::GlobalTrajectoryPoint> edgeTrajectoryPoints =
                        edge.attrib().trajectory->points();
 
                    // if trajectory is being initialized, include the start, otherwise skip it
                    const int offset = trajectoryPoints.empty() ? 0 : 1;
 
                    if (edgeTrajectoryPoints.size() > 2) // not only start and goal position
                    {
                        ARMARX_TRACE;
                        // append `edge trajectory` to `trajectory` (without start and goal points)
                        trajectoryPoints.insert(trajectoryPoints.end(),
                                                edgeTrajectoryPoints.begin() + offset,
                                                edgeTrajectoryPoints.end());
                    }
 
 
                    ARMARX_TRACE;
                    // clang-format off
                    // trajectoryPoints.push_back(
                    //     core::TrajectoryPoint
                    //         {
                    //             .waypoint =
                    //             {
                    //                 .pose = graph.vertex(shortestPath.at(i+1)).attrib().getPose()
                    //             },
                    //             .velocity = std::numeric_limits<float>::max()
                    //         });
                    // clang-format on
 
                    break;
                }
 
                case client::GlobalPlanningStrategy::Point2Point:
                {
                    ARMARX_INFO << "Point2Point navigation on edge";
 
                    // FIXME variable
                    const float point2pointVelocity = 400;
 
                    const core::GlobalTrajectoryPoint currentTrajPt = {
                        .waypoint = {.pose = resolveGraphVertex(graph.vertex(shortestPath.at(i)))},
                        .velocity = point2pointVelocity};
 
                    const core::GlobalTrajectoryPoint nextTrajPt{
                        .waypoint = {.pose =
                                         resolveGraphVertex(graph.vertex(shortestPath.at(i + 1)))},
                        .velocity = 0};
 
                    // resample event straight lines
                    //                    ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
 
                    // const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
                    // ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
 
                    // const auto resampledTrajectory = segmentTraj.resample(500); // FIXME param
 
                    // ARMARX_INFO << "Resampled trajectory contains "
                    //           << resampledTrajectory.points().size() << " points";
 
                    // this is the same pose as the goal of the previous segment but with a different velocity
                    // FIXME MUST set velocity here.
                    // trajectoryPoints.push_back(
                    // core::TrajectoryPoint{.waypoint = trajectoryPoints.back().waypoint,
                    //   .velocity = std::numeric_limits<float>::max()});
 
                    ARMARX_TRACE;
                    // trajectoryPoints.insert(trajectoryPoints.end(),
                    //                         resampledTrajectory.points().begin(),
                    //                         resampledTrajectory.points().end());
                    trajectoryPoints.push_back(currentTrajPt);
                    trajectoryPoints.push_back(nextTrajPt);
 
                    break;
                }
                default:
                {
                    ARMARX_ERROR << "Boom.";
                }
            }
        }
 
        ARMARX_INFO << "Trajectory consists of " << trajectoryPoints.size() << " points";
 
        for (const auto& pt : trajectoryPoints)
        {
            ARMARX_INFO << pt.waypoint.pose.translation();
        }
 
        return {trajectoryPoints};
    }
 
    GraphBuilder
    Navigator::convertToGraph(const std::vector<client::WaypointTarget>& targets) const
    {
        //
        GraphBuilder graphBuilder;
        graphBuilder.initialize(core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()));
 
        // std::optional<Graph*> activeSubgraph;
        for (const auto& target : targets)
        {
            ARMARX_INFO << "Adding target " << QUOTED(target) << " to graph";
            // if the last location was on a subgraph, that we are about to leave
            // const bool leavingSubgraph = [&]() -> bool
            // {
            //     if (not activeSubgraph.has_value())
            //     {
            //         return false;
            //     }
 
            //     // if a user specifies a pose, then this pose is not meant to be part of a graph
            //     // -> can be reached directly
            //     if (target.pose.has_value())
            //     {
            //         return true;
            //     }
 
            //     if (not target.locationId->empty())
            //     {
            //         const auto& subgraph = core::getSubgraph(target.locationId.value(),srv.sceneProvider->scene().graph->subgraphs);
            //         return subgraph.name() != activeSubgraph;
            //     }
 
            //     throw LocalException("this line should not be reachable");
            // }();
 
            // if(leavingSubgraph)
            // {
            //     const auto activeNodes = graph.activeVertices();
            //     ARMARX_CHECK(activeNodes.size() == 1);
            //     graph.connect(activeSubgraph->getRoutesFrom(activeNodes.front()));
            // }
 
 
            // if a user specifies a pose, then this pose is not meant to be part of a graph
            // -> can be reached directly
            if (target.pose.has_value())
            {
                graphBuilder.connect(target.pose.value(), target.strategy);
                continue;
            }
 
            // if a user specified a vertex of a subgraph (aka location), then this vertex
            // might not be reachable directly. It might be needed to navigate on the graph
            // instead. Thus, we collect all routes to reach the node.
            if (not target.locationId->empty())
            {
                const auto& subgraph = core::getSubgraph(
                    target.locationId.value(), srv.sceneProvider->scene().graph->subgraphs);
 
                const auto vertex = core::getVertexByName(target.locationId.value(), subgraph);
 
                ARMARX_INFO << "Vertex " << QUOTED(target.locationId.value()) << " resolved to "
                            << vertex.attrib().getPose();
 
                const std::vector<core::GraphPath> routes = core::findPathsTo(vertex, subgraph);
                // const auto routes = subgraph->getRoutesTo(target.locationId);
 
                ARMARX_INFO << "Found " << routes.size() << " routes to location `"
                            << target.locationId.value();
 
                ARMARX_CHECK(not routes.empty()) << "The location `" << target.locationId.value()
                                                 << "` is not a reachable vertex on the graph!";
 
                // we now add all routes to the graph that take us to the desired location
                graphBuilder.connect(routes,
                                     target.strategy); // TODO: all routes to the same target
 
                continue;
            }
 
            ARMARX_ERROR << "Either `location_id` or `pose` has to be provided!";
        }
 
        const auto goalVertex = graphBuilder.getGraph().vertex(graphBuilder.goalVertex());
        ARMARX_INFO << "Goal vertex is " << QUOTED(goalVertex.attrib().getLocationName());
 
        return graphBuilder;
    }
 
    core::Pose
    Navigator::resolveGraphVertex(const core::Graph::ConstVertex& vertex) const
    {
        const auto goal = core::resolveLocation(srv.sceneProvider->scene().staticScene->objectMap,
                                                srv.sceneProvider->scene().staticScene->objectInfo,
                                                vertex.attrib().getPose());
        ARMARX_CHECK(goal.pose.has_value())
            << "The location of vertex " << vertex.attrib().getLocationName()
            << " couldn't be resolved (" << goal.errorMsg << ")";
        return goal.pose.value();
    }
 
    void
    Navigator::moveTo(const std::vector<client::WaypointTarget>& targets,
                      core::NavigationFrame navigationFrame)
    {
        // arlt: Is this function deprecated?
        ARMARX_CHECK(navigationFrame == core::NavigationFrame::Absolute)
            << "only absolute movement implemented atm.";
 
        ARMARX_TRACE;
 
        validate(targets);
 
        ARMARX_CHECK_NOT_EMPTY(targets) << "At least the goal has to be provided!";
        ARMARX_INFO << "Navigating to " << targets.back();
 
        // update static scene including locations
        updateScene(true);
 
        auto graphBuilder = convertToGraph(targets);
 
        ARMARX_TRACE;
 
        core::Graph graph = graphBuilder.getGraph();
        auto startVertex = graphBuilder.startVertex;
        auto goalVertex = graphBuilder.getGraph().vertex(graphBuilder.goalVertex());
 
        ARMARX_INFO << "Goal pose according to graph is " << graphBuilder.goalPose().matrix();
 
        ARMARX_CHECK(startVertex.has_value());
 
        ARMARX_TRACE;
 
        goalReachedMonitor = std::nullopt;
        goalReachedMonitor = GoalReachedMonitor(
            graphBuilder.goalPose(), srv.sceneProvider->scene(), config.goalReachedConfig);
 
        if (goalReachedMonitor->goalReached(false))
        {
            ARMARX_IMPORTANT << "Already at goal position "
                             << goalReachedMonitor->goal().translation().head<2>()
                             << ". Robot won't move.";
 
            srv.publisher->goalReached(core::GoalReachedEvent{
                {armarx::Clock::Now()},
                core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
 
            return;
        }
 
        ARMARX_TRACE;
        setGraphEdgeCosts(graph);
 
        ARMARX_VERBOSE << graph;
 
        srv.introspector->onGlobalGraph(graph);
 
        ARMARX_TRACE;
        const auto shortestPath = findShortestPath(graph, startVertex.value(), goalVertex);
 
        // print
        ARMARX_TRACE;
 
        std::vector<core::Pose> vertexPoses;
        vertexPoses.emplace_back(srv.sceneProvider->scene().robot->getGlobalPose());
 
        ARMARX_INFO << "Navigating along the following nodes:";
        for (const semrel::ShapeID& vertex : shortestPath)
        {
            ARMARX_INFO << " - " << graph.vertex(vertex).attrib().getLocationName();
            vertexPoses.push_back(resolveGraphVertex(graph.vertex(vertex)));
        }
 
        srv.introspector->onGlobalShortestPath(vertexPoses);
 
 
        // convert graph / vertex chain to trajectory
        ARMARX_TRACE;
        core::GlobalTrajectory globalPlanTrajectory = convertToTrajectory(shortestPath, graph);
 
        // globalPlanTrajectory.setMaxVelocity(1000); // FIXME
 
        // move ...
 
        // this is our `global plan`
        ARMARX_TRACE;
 
        globalPlan = global_planning::GlobalPlannerResult{.trajectory = globalPlanTrajectory,
                                                          .helperTrajectory = std::nullopt};
 
        // the following is similar to moveToAbsolute
        // TODO(fabian.reister): remove code duplication
 
        srv.executor->execute(globalPlan->currentGlobalSegment());
 
        ARMARX_TRACE;
        srv.publisher->globalTrajectoryUpdated(core::GlobalTrajectoryUpdatedEvent{
            {.timestamp = armarx::Clock::Now()}, globalPlan->currentGlobalSegment()});
 
        ARMARX_TRACE;
        srv.introspector->onGlobalPlannerResult(globalPlan->plan);
 
        ARMARX_INFO << "Global planning completed. Will now start all required threads";
        ARMARX_TRACE;
 
        startStack();
    }
 
    void
    Navigator::startStack()
    {
 
        ARMARX_TRACE;
 
        ARMARX_INFO << "Starting stack.";
 
        {
            // ensure running task is never started and stopped simultaneously
            const std::scoped_lock<std::mutex> lock{runningTaskMtx};
 
            shouldRun = true;
 
            // FIXME instead of PeriodicTask, use RunningTask.
            if (not runningTask)
            {
                runningTask =
                    new PeriodicTask<Navigator>(this,
                                                &Navigator::run,
                                                config.general.tasks.replanningUpdatePeriod,
                                                false,
                                                "PeriodicTask");
                runningTask->start();
            }
            else if (not runningTask->isRunning())
            {
                runningTask->start();
            }
        }
 
        // FIXME create separate function for this.
        if (globalPlan->useLocalPlanner(hasLocalPlanner()))
        {
            if (srv.executor != nullptr)
            {
                srv.executor->start(ExecutorInterface::ControllerType::LocalTrajectory);
            }
        }
        else
        {
            if (srv.executor != nullptr)
            {
                srv.executor->start(ExecutorInterface::ControllerType::GlobalTrajectory);
            }
        }
 
        // Could be required if pauseMovement() has been called in the past.
        resume();
        srv.publisher->movementStarted(core::MovementStartedEvent{
            {.timestamp = armarx::Clock::Now()},
            core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
    }
 
    void
    Navigator::moveToAbsolute(const std::vector<core::Pose>& waypoints, bool sceneUpdate)
    {
        ARMARX_TRACE;
 
        // if this navigator is in use, stop the movement ...
        pause();
        // ... and all threads
        stopAllThreads();
 
        // FIXME remove
        //std::this_thread::sleep_for(std::chrono::seconds(1));
 
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_EMPTY(waypoints);
 
        if (sceneUpdate)
        {
            updateScene(true);
        }
 
        ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose()
                    << " to " << waypoints.back().matrix();
 
        // first we check if we are already at the goal position
        goalReachedMonitor = std::nullopt;
        goalReachedMonitor = GoalReachedMonitor(
            waypoints.back(), srv.sceneProvider->scene(), config.goalReachedConfig);
 
        if (goalReachedMonitor->goalReached(false))
        {
            ARMARX_INFO << "Already at goal position. Robot won't move.";
            ARMARX_CHECK_NOT_NULL(srv.publisher);
 
            ARMARX_VERBOSE << "Finalizing";
            srv.publisher->goalReached(core::GoalReachedEvent{
                {armarx::Clock::Now()},
                core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
            ARMARX_VERBOSE << "Finalized";
 
            return;
        }
 
        // global planner
        ARMARX_INFO << "Planning global trajectory";
        ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
        // TODO plan on multiple waypoints, ignoring waypoints for now
        // idea: compute multiple global trajectories, one for each segment between waypoints.
 
        srv.introspector->onGoal(waypoints.back());
        globalPlan = config.stack.globalPlanner->plan(waypoints.back());
 
        if (srv.drawer != nullptr)
        {
            srv.drawer->callGenericDrawFunction(
                [&](viz::Client& client)
                { config.stack.globalPlanner->visualizeDebugInfo(client); });
        }
        else
        {
            ARMARX_WARNING << "No drawer available. Cannot visualize global planner debug info.";
        }
 
 
        ARMARX_TRACE;
 
        if (not globalPlan.has_value())
        {
            ARMARX_WARNING << "No global trajectory. Cannot move.";
            srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
                {.timestamp = armarx::Clock::Now()}, {"No global trajectory. Cannot move."}});
 
            srv.introspector->failure();
            return;
        }
 
        // create subdivision and start first segment
        setupGlobalPlanSubvidision();
 
        startGlobalPathSegment(false, false);
    }
 
    void
    Navigator::updateAbsolute(const std::vector<core::Pose>& waypoints)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_EMPTY(waypoints);
 
        // Assume nothing in static obstacles changed -> also don't update costmap
        updateScene(false);
 
        // global planner
        ARMARX_INFO << "Planning global trajectory";
        ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
        // TODO plan on multiple waypoints, ignoring waypoints for now
        // idea: compute multiple global trajectories, one for each segment between waypoints.
 
        srv.introspector->onGoal(waypoints.back());
        globalPlan = config.stack.globalPlanner->plan(waypoints.back());
 
        ARMARX_TRACE;
 
        if (not globalPlan.has_value())
        {
            ARMARX_WARNING << "No global trajectory. Cannot move.";
            srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
                {.timestamp = armarx::Clock::Now()}, {"No global trajectory. Cannot move."}});
 
            srv.introspector->failure();
            return;
        }
 
        // create subdivision and start first segment
        setupGlobalPlanSubvidision();
 
        startGlobalPathSegment(false, true);
    }
 
    void
    Navigator::moveToAbsoluteAlternatives(const std::vector<core::TargetAlternative>& targets)
    {
        ARMARX_TRACE;
 
        // if this navigator is in use, stop the movement ...
        pause();
        // ... and all threads
        stopAllThreads();
 
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_EMPTY(targets);
 
        updateScene(true);
 
        ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose()
                    << " to " << targets.size() << " alternatives";
 
        lastAllAlternativesImpossible = std::nullopt;
 
        // first, check whether we are already at any target
        for (const auto& t : targets)
        {
            GoalReachedMonitor monitor(
                t.target, srv.sceneProvider->scene(), config.goalReachedConfig);
 
            if (monitor.goalReached(false))
            {
                ARMARX_INFO << "Already at a possible goal position. Robot won't move.";
 
                srv.publisher->goalReached(core::GoalReachedEvent{
                    {armarx::Clock::Now()},
                    core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
 
                return;
            }
        }
 
        // sort alternatives by their priority (in descending order)
        targetAlternatives = targets;
        ranges::sort(targetAlternatives, std::greater{}, &core::TargetAlternative::priority);
 
        if (setupTargetAlternatives())
        {
            // create subdivision and start first segment
            setupGlobalPlanSubvidision();
 
            startGlobalPathSegment(false, false);
        }
    }
 
    bool
    Navigator::setupTargetAlternatives()
    {
        ARMARX_CHECK_NOT_EMPTY(targetAlternatives);
 
        globalPlan = std::nullopt;
 
        // Spfa planner plans into the "whole room", i.e. by executing the algorithm with
        // a single start position, a path to any goal can be constructed.
        // Thus, we only use alternatives with the spfa planner to avoid repeatedly replanning the
        // global path.
        const auto spfaPlanner =
            std::dynamic_pointer_cast<global_planning::SPFA>(config.stack.globalPlanner);
        if (spfaPlanner != nullptr)
        {
            const core::Pose start(srv.sceneProvider->scene().robot->getGlobalPose());
            // expensive spfa planning only once for all alternatives
            const auto planningResult = spfaPlanner->executePlanner(start);
 
            for (const auto& target : targetAlternatives)
            {
                // cheap path reconstruction for every alternative
                const auto path = spfaPlanner->calculatePath(planningResult, target.target);
                if (path.has_value() and verifyGlobalPathPossible(path->trajectory))
                {
                    globalPlan = path;
                    srv.introspector->onGoal(target.target);
                    goalReachedMonitor = std::nullopt;
                    goalReachedMonitor = GoalReachedMonitor(
                        target.target, srv.sceneProvider->scene(), config.goalReachedConfig);
 
                    ARMARX_INFO << "Valid path found for target " << target.target.matrix()
                                << " with priority " << target.priority;
                    break;
                }
                else
                {
                    ARMARX_VERBOSE << "Target with priority " << target.priority
                                   << " not reachable";
                }
            }
        }
        else
        {
            const auto goal = targetAlternatives.front().target;
            ARMARX_INFO
                << "Current global planner is not an SPFA planner, discarding alternatives!";
 
            globalPlan = config.stack.globalPlanner->plan(goal);
            srv.introspector->onGoal(goal);
            goalReachedMonitor = std::nullopt;
            goalReachedMonitor =
                GoalReachedMonitor(goal, srv.sceneProvider->scene(), config.goalReachedConfig);
 
            // we only use target alternatives with the spfa planner
            targetAlternatives.clear();
        }
 
        if (srv.drawer != nullptr)
        {
            srv.drawer->callGenericDrawFunction(
                [&](viz::Client& client)
                { config.stack.globalPlanner->visualizeDebugInfo(client); });
        }
        else
        {
            ARMARX_WARNING << "No drawer available. Cannot visualize global planner debug info.";
        }
 
 
        ARMARX_TRACE;
 
        if (not globalPlan.has_value())
        {
            ARMARX_WARNING << "No global trajectory. Cannot move.";
            srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
                {.timestamp = armarx::Clock::Now()}, {"No global trajectory. Cannot move."}});
 
            srv.introspector->failure();
            return false;
        }
 
        return true;
    }
 
    void
    Navigator::setupGlobalPlanSubvidision()
    {
        ARMARX_CHECK(globalPlan.has_value());
        ARMARX_CHECK(globalPlan->subdivision.empty());
        const auto& globalPlanPoses = globalPlan->plan.trajectory.poses();
 
        ARMARX_TRACE;
        if (not globalPlan->plan.trajectory.points().empty())
        {
            ARMARX_INFO << "Trajectory final pose " << globalPlanPoses.back().matrix();
 
            goalReachedMonitor->updateGoal(globalPlanPoses.back());
        }
        srv.publisher->globalTrajectoryUpdated(core::GlobalTrajectoryUpdatedEvent{
            {.timestamp = armarx::Clock::Now()}, globalPlan->plan.trajectory});
 
        reportPathGeometry(globalPlan->plan.trajectory);
 
        ARMARX_INFO << "Global planning completed.";
        ARMARX_TRACE;
        // only enable subdivision if local planner is actually enabled
        if ((not config.general.subdivision.enable) or (not hasLocalPlanner()) or
            globalPlan->plan.trajectory.points().empty())
        {
            // startGlobalPathSegment will hand trajectory over to introspection
            // after ramping has been performed; when subdivision is enabled
            // the introspection will receive the full subdivision (see below)
 
            return;
        }
 
        ARMARX_CHECK(hasLocalPlanner());
        ARMARX_INFO << "Starting subdividing global path.";
 
        ARMARX_CHECK(srv.sceneProvider->scene().staticScene.has_value());
        ARMARX_CHECK(
            srv.sceneProvider->scene().staticScene->distanceToObstaclesCostmap.has_value());
        const auto& costmap = srv.sceneProvider->scene().staticScene->distanceToObstaclesCostmap;
 
        std::vector<bool> localPlanEligibility;
        localPlanEligibility.reserve(globalPlan->plan.trajectory.points().size());
        for (const auto& pose : globalPlanPoses)
        {
            Eigen::Vector2f pt = conv::to2D(pose.translation());
            localPlanEligibility.push_back(costmap->value(pt).value_or(0) >
                                           config.general.subdivision.localPlannerCostmapThreshold);
        }
 
        // expand every sequence of false values in localPlanEligibility
        bool falseSequence = false;
        const int n = localPlanEligibility.size();
        const float expansion = config.general.subdivision.globalPlanExpansionDistance;
        if (expansion > 0)
        {
            for (int i = 0; i < n;)
            {
                if (falseSequence)
                {
                    if (localPlanEligibility[i])
                    {
                        // false sequence ended, expand it appropriately to the back
                        falseSequence = false;
 
                        // we expand until we have a distance >= expansion
                        float totalDistance = 0;
                        // false sequence ended -> there has to be an element before the current one
                        ARMARX_CHECK_GREATER(i, 0);
                        for (; (totalDistance < expansion) and (i < n); i++)
                        {
                            localPlanEligibility[i] = false;
                            totalDistance += (globalPlanPoses[i - 1].translation() -
                                              globalPlanPoses[i].translation())
                                                 .norm();
                        }
                        continue; // we already incremented i
                    }
 
                    // otherwise continue current false sequence
                }
                else
                {
                    if (not localPlanEligibility[i])
                    {
                        // false sequence started, expand it appropriately to the front
                        falseSequence = true;
 
                        // we expand until we have a distance >= expansion
                        float totalDistance = 0;
                        for (int j = i - 1; (totalDistance < expansion) and (j >= 0); j--)
                        {
                            localPlanEligibility[j] = false;
                            totalDistance += (globalPlanPoses[j].translation() -
                                              globalPlanPoses[j + 1].translation())
                                                 .norm();
                        }
                    }
 
                    // otherwise continue current true sequence
                }
 
                i++;
            }
        }
 
        ARMARX_TRACE;
        // identify each consecutive true/false sequence and create subdivision
        // also check that each localPlanner segment is long enough
 
        GlobalPathSubdivision::Subdivision segment;
        segment.s = 0;
        segment.useLocalPlanner = localPlanEligibility[0];
 
        // returns true, iff the segment should be terminated and appended to the list of subdivisions
        const auto checkSegmentLength = [&]()
        {
            // only check the length if the current segment uses the local planner
            if (not segment.useLocalPlanner)
            {
                return true;
            }
 
            const auto& subTrajectory =
                globalPlan->plan.trajectory.getSubTrajectory(segment.s, segment.t);
            const float segmentLength = subTrajectory.length();
            if (segmentLength >= config.general.subdivision.minSegmentDistance)
            {
                return true; // segment is long enough
            }
 
            if (globalPlan->subdivision.empty() and static_cast<int>(segment.t) >= n)
            {
                // segment is too short but the only segment -> valid
                ARMARX_VERBOSE << "Segment " << globalPlan->subdivision.size() << ": [" << segment.s
                               << ", " << segment.t
                               << "); is the only segment. length=" << segmentLength;
                return true;
            }
 
            ARMARX_VERBOSE << "Segment " << globalPlan->subdivision.size() << ": [" << segment.s
                           << ", " << segment.t
                           << "); was too short for local planner: " << segmentLength;
 
            if (globalPlan->subdivision.empty())
            {
                // this is the first (but not the only) segment, join it to the next one
                segment.useLocalPlanner = false;
                ARMARX_VERBOSE
                    << "First segment, convert to global planner segment and grow it further.";
            }
            else
            {
                if (static_cast<int>(segment.t) < n)
                {
                    // there is a preceding segment, remove it and continue to grow it
                    segment = globalPlan->subdivision.back();
                    globalPlan->subdivision.pop_back();
                    ARMARX_VERBOSE << "Removing preceding segment and grow it further.";
                }
                else
                {
                    // this is the last segment, expand the preceding segment to include this one
                    globalPlan->subdivision.back().t = segment.t;
                    ARMARX_VERBOSE << "Last segment; extent previous segment to the end.";
                }
            }
            return false;
        };
 
        for (int i = 0; i < n; i++)
        {
            // continue the current sequence until its end
            while (i < n and localPlanEligibility[i] == segment.useLocalPlanner)
            {
                i++;
            }
 
            segment.t = i;
            if (checkSegmentLength())
            {
                globalPlan->subdivision.emplace_back(segment);
                ARMARX_VERBOSE << "Segment " << globalPlan->subdivision.size() << ": [" << segment.s
                               << ", " << segment.t
                               << "); localPlanner=" << segment.useLocalPlanner;
 
                // start the next sequence
                segment.s = i;
                segment.useLocalPlanner = localPlanEligibility[i];
            }
        }
 
        ARMARX_TRACE;
 
        // check subdivision is valid
        std::size_t lastSegmentEnd = 0;
        for (const auto& segment : globalPlan->subdivision)
        {
            ARMARX_CHECK_EQUAL(lastSegmentEnd, segment.s);
            const std::ptrdiff_t segmentLength = segment.t - segment.s;
            ARMARX_CHECK_POSITIVE(segmentLength);
            lastSegmentEnd = segment.t;
        }
        ARMARX_CHECK_EQUAL(lastSegmentEnd, globalPlan->plan.trajectory.points().size());
 
        globalPlan->currentSegment = 0;
        srv.introspector->onGlobalPlannerSubdivision(globalPlan.value());
 
        ARMARX_INFO << "Divided global path into " << globalPlan->subdivision.size() << " segments";
    }
 
    void
    Navigator::applyParametrization(core::GlobalTrajectory& trajectory, const float startVelocity)
    {
        // Copied *before* apply(): in the non-subdivision branch `currentGlobalSegment()` hands
        // back `plan.trajectory` by reference, so apply() overwrites the planner's velocities in
        // place. Those velocities are the obstacle-aware limit, and the record below needs them.
        const core::GlobalTrajectory planned = trajectory;
 
        core::TrajectoryParametrization applied = parametrizationMode;
 
        // TOPP-RA fits a spline through the path and needs at least four distinct waypoints for
        // it. A pure rotation or a very short point-to-point motion has fewer, and would fail on
        // every invocation. Silent by design: this is an expected shape of request, not a fault.
        constexpr std::size_t minimumWaypointsForToppra = 4;
 
        if (applied == core::TrajectoryParametrization::Toppra and
            trajectory.points().size() < minimumWaypointsForToppra)
        {
            applied = core::TrajectoryParametrization::Ramping;
            rampingFallback->apply(trajectory, startVelocity);
        }
        else
        {
            try
            {
                parametrization->apply(trajectory, startVelocity);
            }
            catch (const std::exception& e)
            {
                // An unparametrizable path must not abort the navigation request: ramping is
                // always executable, just slower.
                ARMARX_WARNING << "Trajectory parametrization failed (" << e.what()
                               << "). Falling back to ramping for this request.";
 
                applied = core::TrajectoryParametrization::Ramping;
                trajectory = planned;
                rampingFallback->apply(trajectory, startVelocity);
            }
        }
 
        // Which parametrization a request actually got is not otherwise visible in the log: the
        // requested one is announced when the navigator is built, but every degradation path
        // above is per-request. The two durations say what it bought -- both are the same
        // Riemann sum over the same path, so they are directly comparable.
        ARMARX_INFO << "Parametrized " << trajectory.points().size() << " waypoints with "
                    << QUOTED(core::ToString(applied)) << ": "
                    << trajectory.duration(core::VelocityInterpolation::LinearInterpolation)
                    << " s (the planner's obstacle-aware velocities alone would take "
                    << planned.duration(core::VelocityInterpolation::LinearInterpolation)
                    << " s).";
 
        if (config.general.parametrizationDump)
        {
            writeParametrizationDump(
                {.requested = config.stack.generalConfig.parametrization,
                 .applied = applied,
                 .planned = planned,
                 .parametrized = trajectory,
                 .toppra = dynamic_cast<const algorithms::Toppra*>(parametrization.get()),
                 .driveParams = driveParams,
                 .maxVelocity = config.stack.generalConfig.maxVel.linear,
                 .maxAngularVelocity = config.stack.generalConfig.maxVel.angular},
                config.general.parametrizationDumpPath);
        }
    }
 
    void
    Navigator::resumeAfterEmergencyStop()
    {
        if (not globalPlan.has_value() or isPaused() or isStopped())
        {
            return;
        }
 
        if (globalPlan->useLocalPlanner(hasLocalPlanner()))
        {
            // The local planner re-plans from the robot every cycle, so its trajectory is
            // already anchored where the robot stands. Nothing to re-anchor.
            return;
        }
 
        const core::GlobalTrajectory& trajectory = globalPlan->currentGlobalSegment();
        if (trajectory.points().empty())
        {
            return;
        }
 
        const core::Position robotPosition =
            core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()).translation();
 
        // The whole remainder, not a lookahead window: this replaces what the executor runs
        // until the next segment starts, and cutting it short would end the motion early.
        auto [remaining, endsAtGoal] = trajectory.getSubTrajectory(
            robotPosition, std::numeric_limits<float>::infinity());
 
        if (remaining.points().size() < 2)
        {
            ARMARX_WARNING << "Cannot re-anchor the trajectory after the emergency stop: only "
                           << remaining.points().size()
                           << " waypoints remain. Resuming on the trajectory as planned.";
            return;
        }
 
        ARMARX_IMPORTANT << "Re-anchoring the trajectory at the robot's current pose after the "
                            "emergency stop: "
                         << remaining.points().size() << " of " << trajectory.points().size()
                         << " waypoints remain.";
 
        applyParametrization(remaining, config.stack.generalConfig.boundaryVelocity);
 
        updateExecutor(remaining);
    }
 
    bool
    Navigator::startGlobalPathSegment(bool incrementSegment, bool rampFromCurrentVelocity)
    {
        if (not globalPlan->subdivision.empty())
        {
            // subdivision enabled
 
            if (incrementSegment)
            {
                globalPlan->currentSegment++;
            }
 
            if (globalPlan->currentSegment >= globalPlan->subdivision.size())
            {
                // global path already finished
                return true;
            }
 
            ARMARX_INFO << "Starting segment " << globalPlan->currentSegment;
            const auto& currentSegment = globalPlan->subdivision[globalPlan->currentSegment];
 
            globalPlan->currentSegmentTrajectory =
                globalPlan->plan.trajectory.getSubTrajectory(currentSegment.s, currentSegment.t);
 
            applyParametrization(globalPlan->currentGlobalSegment(),
                                 rampFromCurrentVelocity
                                     ? srv.sceneProvider->scene().platformVelocity.linear.norm()
                                     : config.stack.generalConfig.boundaryVelocity);
 
            goalReachedMonitor->updateGoal(
                globalPlan->currentGlobalSegment().points().back().waypoint.pose);
        }
        else
        {
            if (incrementSegment)
            {
                return true;
            }
 
            applyParametrization(globalPlan->currentGlobalSegment(),
                                 rampFromCurrentVelocity
                                     ? srv.sceneProvider->scene().platformVelocity.linear.norm()
                                     : config.stack.generalConfig.boundaryVelocity);
 
            goalReachedMonitor->updateGoal(
                globalPlan->currentGlobalSegment().points().back().waypoint.pose);
 
            // only visualize global trajectory once ramping has been performed
            srv.introspector->onGlobalPlannerResult(globalPlan->plan);
        }
 
        if (globalPlan->useLocalPlanner(hasLocalPlanner()))
        {
            const auto localPlannerResult = updateLocalPlanner();
            updateExecutor(localPlannerResult);
            updateIntrospector(localPlannerResult);
        }
        else
        {
            updateExecutor(globalPlan->currentGlobalSegment());
        }
 
        ARMARX_INFO << "Start executing global plan segment (local planner="
                    << globalPlan->useLocalPlanner(hasLocalPlanner())
                    << "). Will now start all required threads.";
        ARMARX_TRACE;
 
        startStack();
        ARMARX_INFO << "Movement started.";
 
        return false;
    }
 
    void
    Navigator::moveTowards(const core::Direction& direction, core::NavigationFrame navigationFrame)
    {
    }
 
    void
    Navigator::moveToLocation(const std::string& location,
                              const std::optional<std::string>& providerName)
    {
        // update static scene including locations
        updateScene(true);
 
        const auto resolveLocation =
            [&](const std::string& location,
                const std::optional<std::string>& providerName) -> std::vector<core::Location>
        {
            const auto locations = srv.sceneProvider->scene().staticScene->locations;
 
            ARMARX_VERBOSE << "Available locations";
            for (const auto& location : locations)
            {
                ARMARX_VERBOSE << QUOTED(location.name) << " from provider "
                               << QUOTED(location.provider);
            }
 
            const auto matchingLocs =
                core::util::findMatchingLocations(locations, location, providerName);
 
            ARMARX_CHECK_NOT_EMPTY(matchingLocs)
                << "Unknown location " << QUOTED(location) << " and provider "
                << QUOTED(providerName.value_or("~unset~")) << ".";
            return matchingLocs;
        };
 
        const auto split = simox::alg::split(location, ":");
        ARMARX_CHECK(0 < split.size() && split.size() <= 2)
            << "The given location does not match the format <location>(:<instance-id>)? '"
            << location << "'.";
 
        std::string instanceID;
        if (split.size() == 2)
        {
            // An instance-id was given in the location
            instanceID = split.back();
        }
 
        const auto matchingLocations = resolveLocation(split.front(), providerName);
 
 
        if (matchingLocations.empty())
        {
            ARMARX_WARNING << "Failed to resolve location " << QUOTED(location) << " from provider "
                           << QUOTED(providerName.value_or("~unset~"));
        }
 
        if (matchingLocations.size() > 1)
        {
            ARMARX_WARNING << "Found more than one matching location for " << QUOTED(location)
                           << " from provider " << QUOTED(providerName.value_or("~unset~"));
            for (const auto& location : matchingLocations)
            {
                ARMARX_INFO << "- " << QUOTED(location.provider) << ": " << QUOTED(location.name);
            }
        }
 
 
        const auto goal = core::resolveLocation(srv.sceneProvider->scene().staticScene->objectMap,
                                                srv.sceneProvider->scene().staticScene->objectInfo,
                                                matchingLocations.front().framedPose,
                                                instanceID);
 
        if (goal.pose.has_value())
        {
            moveToAbsolute({goal.pose.value()}, false);
        }
        else
        {
            ARMARX_ERROR << goal.errorMsg;
        }
    }
 
    void
    Navigator::moveTowardsAbsolute(const core::Direction& direction)
    {
    }
 
    void
    Navigator::run()
    {
        // Ensure only one thread can call this method at a time (This is not ensured by the PeriodicTask).
        // Given the lock in updateLocalPlanner, this is currently not fixing any bugs on its own,
        // however, running this function from different threads simultaneously does not provide any benefits
        // and could lead to further bugs.
        const std::scoped_lock<std::mutex> lock{runMtx};
 
        if (not shouldRun)
        {
            ARMARX_WARNING
                << "Called Navigator::run() although shouldRun is false. Directly returning.";
            return;
        }
 
        // TODO(fabian.reister): add debug observer logging
 
        // scene update
        {
            ARMARX_DEBUG << "Updating scene";
 
            // TODO: remove full update and only update costmap when required!!!
            // this will slow down the navigation loop significantly (>1s)
            const Duration duration = armarx::core::time::StopWatch::measure(
                [&]() { updateScene(true /* false (only dynamic and costmap)*/); });
 
            ARMARX_DEBUG << deactivateSpam(0.2)
                         << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
 
            srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]",
                                                               duration.toMilliSecondsDouble());
        }
 
        // verify global path is still possible
        {
            // if we have target alternatives and the path is no longer valid, attempt to
            // navigate to a valid alternative
            if (globalPlan.has_value() and not targetAlternatives.empty())
            {
                checkGlobalPathAlternatives();
            }
        }
 
        // eventually, draw
        if ((srv.introspector != nullptr) and (srv.sceneProvider != nullptr) and
            srv.sceneProvider->scene().robot)
        {
            ARMARX_DEBUG << "Drawing robot pose";
            srv.introspector->onRobotPose(
                core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()));
        }
 
        // global planner update if goal has changed
        // niklas: symbol globalPlanningRequest is not used, so this code is dead
        /*{
            std::lock_guard g{globalPlanningRequestMtx};
 
            if (globalPlanningRequest.has_value())
            {
                const auto& waypoints = globalPlanningRequest.value();
 
                // recreate goal monitor
                {
                    // first we check if we are already at the goal position
                    goalReachedMonitor = std::nullopt;
                    goalReachedMonitor = GoalReachedMonitor(
                        waypoints.back(), srv.sceneProvider->scene(), config.goalReachedConfig);
 
                    if (goalReachedMonitor->goalReached(false))
                    {
                        ARMARX_INFO << "Already at goal position. Robot won't move.";
 
                        srv.publisher->goalReached(core::GoalReachedEvent{
                            {armarx::Clock::Now()},
                            core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
 
                        return;
                    }
                }
 
                // global planning
                {
                    // global planner
                    ARMARX_INFO << "Update/Planning global trajectory";
                    ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
                    // TODO plan on multiple waypoints, ignoring waypoints for now
                    // idea: compute multiple global trajectories, one for each segment between waypoints.
 
                    srv.introspector->onGoal(waypoints.back());
                    globalPlan = config.stack.globalPlanner->plan(waypoints.back());
 
                    ARMARX_TRACE;
 
                    if (not globalPlan.has_value())
                    {
                        ARMARX_WARNING << "No global trajectory. Cannot move.";
                        srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
                            {.timestamp = armarx::Clock::Now()}, {""}});
 
                        srv.introspector->failure();
                        return;
                    }
 
                    ARMARX_TRACE;
                    srv.publisher->globalTrajectoryUpdated(globalPlan.value());
                    srv.introspector->onGlobalPlannerResult(globalPlan.value());
 
                    if (not hasLocalPlanner())
                    {
                        updateExecutor(globalPlan.value());
                    }
                }
            }
        }*/
 
        // Before anything is activated again: the executor withdrew its controller request when
        // the emergency stop engaged, so this is the one chance to replace the trajectory the
        // robot would otherwise resume on.
        if (srv.executor != nullptr and srv.executor->consumeEmergencyStopRelease())
        {
            resumeAfterEmergencyStop();
        }
 
        // local planner update
        {
            ARMARX_VERBOSE << "Local planner update";
 
            const Duration duration = armarx::core::time::StopWatch::measure(
                [&]()
                {
                    if (globalPlan->useLocalPlanner(hasLocalPlanner()))
                    {
                        const auto localPlannerResult = updateLocalPlanner();
                        updateExecutor(localPlannerResult);
                        updateIntrospector(localPlannerResult);
 
                        if (srv.executor != nullptr && localPlannerResult.has_value() &&
                            not isPaused() && not isStopped())
                        {
                            srv.executor->ensureIsActive(
                                ExecutorInterface::ControllerType::LocalTrajectory);
                        }
                    }
                    else if (srv.executor != nullptr && not isPaused() && not isStopped())
                    {
                        srv.executor->ensureIsActive(
                            ExecutorInterface::ControllerType::GlobalTrajectory);
                    }
                });
            ARMARX_VERBOSE << deactivateSpam(0.2)
                           << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
 
            srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]",
                                                               duration.toMilliSecondsDouble());
        }
 
        // update velocity limits of underlying platform controller
        //   - apply safety guard, if present
        //   - ensure the limits of the currently active navigator are used
        {
            if (hasSafetyGuard())
            {
                ARMARX_VERBOSE << "Updating safety guard";
                const auto result = updateSafetyGuard();
                srv.debugObserverHelper->setDebugObserverDatafield("safety_guard.limit_linear",
                                                                   result.twistLimits.linear);
                srv.debugObserverHelper->setDebugObserverDatafield("safety_guard.limit_angular",
                                                                   result.twistLimits.angular);
 
                if (srv.executor != nullptr)
                {
                    srv.executor->updateVelocityLimits(result.twistLimits);
                }
 
                // if the safety guard keeps the robot blocked for too long, re-run the
                // global planner
                checkRobotBlocked(result);
            }
            else
            // apply the maximum velocity limit from the general config, in case a different navigator applied other limits in between
            {
                if (srv.executor != nullptr)
                {
                    srv.executor->updateVelocityLimits(config.stack.generalConfig.maxVel);
                }
            }
        }
 
        // request to scale the calculated velocity by the velocity factor
        {
            ARMARX_TRACE;
            srv.debugObserverHelper->setDebugObserverDatafield("velocity_factor",
                                                               velocityFactor.load());
 
            // scaling is done relatively; for typical absolute values: see SPFA, 500mm/s 0.4 rad/s
            if (srv.executor != nullptr)
            {
                srv.executor->updateVelocityFactor(velocityFactor);
            }
        }
 
        // monitor update
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "Monitor update";
 
            const Duration duration =
                armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
 
            ARMARX_DEBUG << deactivateSpam(0.2)
                         << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
 
            srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]",
                                                               duration.toMilliSecondsDouble());
        }
    }
 
    safety_guard::SafetyGuardResult
    Navigator::updateSafetyGuard()
    {
        ARMARX_CHECK(hasSafetyGuard());
 
        const core::Pose global_T_robot(srv.sceneProvider->scene().robot->getGlobalPose());
        const auto proj = globalPlan->currentGlobalSegment().getProjection(
            global_T_robot.translation(), core::VelocityInterpolation::LinearInterpolation);
        const Eigen::Vector3f global_V_movement = proj.wayPointAfter.waypoint.pose.translation() -
                                                  proj.projection.waypoint.pose.translation();
 
        return config.stack.safetyGuard->computeSafetyLimits(global_V_movement.head<2>());
    }
 
    void
    Navigator::checkRobotBlocked(const safety_guard::SafetyGuardResult& result)
    {
        const auto& cfg = config.general.blockedReplanning;
 
        if (not cfg.enabled)
        {
            return;
        }
 
        // only relevant while actively navigating towards a goal
        if (isPaused() or isStopped() or not globalPlan.has_value() or
            not goalReachedMonitor.has_value())
        {
            blockedSince = std::nullopt;
            return;
        }
 
        // the safety guard limits the platform velocity close to zero when the robot
        // cannot make progress (e.g. an obstacle blocks the planned path)
        const bool blocked = (result.twistLimits.linear < cfg.linearLimit) or
                             (result.twistLimits.angular < cfg.angularLimit);
 
        if (not blocked)
        {
            // robot is able to move again -> reset the timer
            blockedSince = std::nullopt;
            return;
        }
 
        const armarx::DateTime now = armarx::Clock::Now();
        if (not blockedSince.has_value())
        {
            // start measuring how long the robot stays blocked
            blockedSince = now;
            return;
        }
 
        const armarx::Duration blockedFor = now - blockedSince.value();
        srv.debugObserverHelper->setDebugObserverDatafield("safety_guard.blocked_for [ms]",
                                                           blockedFor.toMilliSecondsDouble());
 
        if (blockedFor < armarx::Duration::SecondsDouble(cfg.blockedTimeSeconds))
        {
            // not blocked long enough yet -> keep waiting (obstacle might clear)
            return;
        }
 
        ARMARX_IMPORTANT << "Robot blocked for " << blockedFor.toMilliSecondsDouble()
                         << "ms (safety guard limits near zero). Re-running the global planner.";
 
        // reset so the freshly planned trajectory gets a chance before we re-check
        blockedSince = std::nullopt;
 
        // re-run the global planner towards the current goal
        updateAbsolute({goalReachedMonitor->goal()});
    }
 
    bool
    Navigator::hasSafetyGuard() const
    {
        return config.stack.safetyGuard != nullptr;
    }
 
    bool
    Navigator::hasLocalPlanner() const noexcept
    {
        return config.stack.localPlanner != nullptr;
    }
 
    void
    Navigator::updateScene(const bool fullUpdate)
    {
        ARMARX_CHECK_NOT_NULL(srv.sceneProvider);
        srv.sceneProvider->synchronize(armarx::Clock::Now(), fullUpdate);
    }
 
    void
    Navigator::checkGlobalPathAlternatives()
    {
        ARMARX_CHECK_NOT_EMPTY(targetAlternatives);
 
        const auto replanAlternatives = [&]()
        {
            lastAllAlternativesImpossible = std::nullopt;
 
            // save current global plan to restore when no path is currently possible
            // i.e. if a person is standing in a doorway and no paths are possible anymore,
            // we restore the previous path and move along it until the safety guard stops the robot
            const auto previousGlobalPlan = globalPlan;
 
            if (setupTargetAlternatives())
            {
                // an alternative path was found
                ARMARX_INFO << "An alternative path was found.";
                setupGlobalPlanSubvidision();
            }
            else
            {
                ARMARX_INFO << "No possible alternatives, restoring previous global path.";
                globalPlan = std::move(previousGlobalPlan);
                lastAllAlternativesImpossible = armarx::Clock::Now();
            }
 
            // either start the new global path or continue on the old one
            startGlobalPathSegment(false, true);
        };
 
        if (not verifyGlobalPathPossible(globalPlan->currentGlobalSegment()))
        {
            // global path is not possible -> either wait for timeout or check alternatives
 
            if (lastAllAlternativesImpossible.has_value() and
                (armarx::Clock::Now() - lastAllAlternativesImpossible.value() <
                 armarx::Duration::SecondsDouble(
                     config.general.targetAlternativesFilterTimeSeconds)))
            {
                ARMARX_INFO << deactivateSpam(1)
                            << "Current globalPlan invalid, waiting for timeout before replanning.";
            }
            else
            {
                ARMARX_INFO << "Global path no longer possible, trying other alternatives!";
 
                replanAlternatives();
            }
        }
        else if (lastAllAlternativesImpossible.has_value())
        {
            // globalPlan was previously invalid but is valid now
            // -> recheck all possible alternatives and their priority
            ARMARX_INFO
                << "Previously invalid global plan valid again -> replanning all alternatives";
 
            replanAlternatives();
        }
    }
 
    bool
    Navigator::verifyGlobalPathPossible(const core::GlobalTrajectory& plan)
    {
        const auto& costmap = srv.sceneProvider->scene().staticScene->distanceToObstaclesCostmap;
        ARMARX_CHECK(costmap.has_value());
 
        for (const auto& pt : plan.points())
        {
            const auto& pose = pt.waypoint.pose;
            const Eigen::Vector2f position = conv::to2D(pose.translation());
            const auto vertex = costmap->toVertex(position);
            if (not costmap->isValid(vertex.index) or costmap->isInCollision(vertex.position))
            {
                return false;
            }
        }
 
        return true;
    }
 
    std::optional<local_planning::LocalPlannerResult>
    Navigator::updateLocalPlanner()
    {
        // We need to make sure the local planner is not simultaniously called from different threads
        const std::scoped_lock<std::mutex> lock{updateLocalPlannerMtx};
 
        ARMARX_CHECK(hasLocalPlanner());
 
        ARMARX_VERBOSE << "Updating local plan";
 
        try
        {
            const auto& globalTrajectory = globalPlan->currentGlobalSegment();
            ARMARX_VERBOSE << globalTrajectory.points().size() << " points in global plan";
            localPlan = config.stack.localPlanner->plan(globalTrajectory);
            ARMARX_VERBOSE << "Local planning finished";
            if (localPlan.has_value())
            {
                srv.publisher->localTrajectoryUpdated(core::LocalTrajectoryUpdatedEvent{
                    {.timestamp = armarx::Clock::Now()}, localPlan->trajectory});
                return localPlan;
            }
            // do not return here, so that localPlanningFailed event is published
            //return localPlan;
        }
        catch (...)
        {
            ARMARX_WARNING << "Failure in local planner: " << GetHandledExceptionString();
            srv.publisher->localPlanningFailed(core::LocalPlanningFailedEvent{
                {.timestamp = armarx::Clock::Now()}, {GetHandledExceptionString()}});
 
            return std::nullopt;
        }
 
        srv.publisher->localPlanningFailed(core::LocalPlanningFailedEvent{
            {.timestamp = armarx::Clock::Now()}, {"Unknown reason"}});
 
        return std::nullopt;
    }
 
    void
    Navigator::updateExecutor(const std::optional<local_planning::LocalPlannerResult>& localPlan)
    {
        if (srv.executor == nullptr)
        {
            return;
        }
 
 
        if (isPaused() or isStopped())
        {
            // [[unlikely]]
            ARMARX_VERBOSE << deactivateSpam(1) << "stopped or paused";
            return;
        }
 
        if (not localPlan.has_value())
        {
            ARMARX_INFO << "Local plan is invalid!";
            ARMARX_CHECK_NOT_NULL(srv.executor);
            srv.executor->execute(core::LocalTrajectory{}, false);
            srv.executor->stop();
            return;
        }
 
 
        // const core::Rotation robot_R_world(srv.sceneProvider->scene().robot->getGlobalOrientation().inverse());
 
        // ARMARX_VERBOSE
        //     << deactivateSpam(100) << "Robot orientation "
        //     << simox::math::mat3f_to_rpy(srv.sceneProvider->scene().robot->getGlobalOrientation()).z();
 
        // core::Twist robotFrameVelocity;
 
        // robotFrameVelocity.linear = robot_R_world * twist.linear;
        // // FIXME fix angular velocity
        // robotFrameVelocity.angular = twist.angular;
 
        // ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
        //                << robotFrameVelocity.linear;
 
        ARMARX_CHECK_NOT_NULL(srv.executor);
        srv.executor->execute(localPlan->trajectory, true);
    }
 
    void
    Navigator::updateExecutor(const core::GlobalTrajectory& globalTrajectory)
    {
        if (srv.executor == nullptr)
        {
            return;
        }
 
        ARMARX_IMPORTANT << "Requested to execute global plan with "
                         << globalTrajectory.points().size() << " points.";
 
 
        // if (isPaused() or isStopped())
        // {
        //     // [[unlikely]]
        //     ARMARX_VERBOSE << deactivateSpam(1) << "stopped or paused";
        //     return;
        // }
 
        if (globalTrajectory.points().empty())
        {
            ARMARX_INFO << "Global plan is invalid!";
            ARMARX_CHECK_NOT_NULL(srv.executor);
            srv.executor->stop();
            return;
        }
 
        ARMARX_IMPORTANT << "Executing global plan with " << globalTrajectory.points().size()
                         << " points.";
        ARMARX_CHECK_NOT_NULL(srv.executor);
        srv.executor->execute(globalTrajectory, false);
    }
 
    void
    Navigator::updateIntrospector(
        const std::optional<local_planning::LocalPlannerResult>& localPlan)
    {
        ARMARX_CHECK_NOT_NULL(srv.introspector);
 
        srv.introspector->onLocalPlannerResult(localPlan);
    }
 
    void
    Navigator::updateMonitor()
    {
        ARMARX_TRACE;
        //ARMARX_CHECK(goalReachedMonitor.has_value());
        if (not goalReachedMonitor.has_value())
        {
            // Note: I guess this can happen because of a race condition between the moveTo methods and the run method.
            // But I am not sure if a common mutex would work correctly.
            ARMARX_WARNING << "GoalReachedMonitor is not initialized.";
            return;
        }
 
        const auto status = goalReachedMonitor->status();
        armarx::DebugObserverHelper* debugObserver = srv.debugObserverHelper;
 
        if (debugObserver != nullptr)
        {
            debugObserver->setDebugObserverDatafield("goalPositionError", status.posError);
            debugObserver->setDebugObserverDatafield("goalOrientationError", status.oriError);
            debugObserver->setDebugObserverDatafield("goalPositionThreshold",
                                                     goalReachedMonitor->getConfig().posTh);
            debugObserver->setDebugObserverDatafield("goalOrientationThreshold",
                                                     goalReachedMonitor->getConfig().oriTh);
        }
 
        if (not isPaused() and goalReachedMonitor->goalReached())
        {
            // [[unlikely]]
            ARMARX_INFO << "Current global path segment finished!";
 
            // we finished the current segment -> start the new one
            if (startGlobalPathSegment(true, true))
            {
                // this segment was the last one, we are finished
                ARMARX_INFO << "Goal " << goalReachedMonitor->goal().translation().head<2>()
                            << " reached!";
 
                // Before `stop()`, and not relying on it: `stop()` deactivates the trajectory
                // controller only if it reports active, so the controller that holds this
                // request's execution record is not guaranteed to be told the request is over.
                // Asking here is what makes the record independent of that.
                if (srv.executor != nullptr)
                {
                    srv.executor->dumpDiagnostics();
                }
 
                stop();
 
                srv.publisher->goalReached(core::GoalReachedEvent{
                    {armarx::Clock::Now()},
                    {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
 
                srv.introspector->success();
            }
        }
    }
 
    void
    Navigator::stopAllThreads()
    {
        const std::scoped_lock<std::mutex> lock{runningTaskMtx};
 
        if (runningTask)
        {
            runningTask->stop();
        }
 
        shouldRun = false;
    }
 
    // bool
    // Navigator::isStackResultValid() const noexcept
    // {
 
    //     // global planner
    //     if (config.stack.globalPlanner != nullptr)
    //     {
    //         if (not globalPlan.has_value())
    //         {
    //             ARMARX_VERBOSE << deactivateSpam(1) << "Global trajectory not yet set.";
    //             return false;
    //         }
    //     }
 
    //     // local planner
    //     if (config.stack.localPlanner != nullptr)
    //     {
    //         if (not localPlan.has_value())
    //         {
    //             ARMARX_VERBOSE << deactivateSpam(1) << "Local trajectory not yet set.";
    //             return false;
    //         }
    //     }
 
    //     // [[likely]]
    //     return true;
    // }
 
    // const core::Trajectory& Navigator::currentTrajectory() const
    // {
    //     ARMARX_CHECK(isStackResultValid());
 
    //     if(localPlan.has_value())
    //     {
    //         return localPlan->trajectory;
    //     }
 
    //     return globalPlan->trajectory;
    // }
 
    void
    Navigator::setVelocityFactor(const float velocityFactor)
    {
        ARMARX_CHECK_POSITIVE(velocityFactor)
            << "Scaling factor for velocity may not be negative, but is " << velocityFactor;
        ARMARX_CHECK_LESS_EQUAL(velocityFactor, 1)
            << "Scaling factor for velocity may not be > 1, but is " << velocityFactor;
 
        this->velocityFactor = velocityFactor;
    }
 
    void
    Navigator::pause()
    {
        ARMARX_INFO << "Paused.";
 
        executorEnabled.store(false);
 
        if (srv.executor != nullptr)
        {
            ARMARX_INFO << "Stopping executor.";
            srv.executor->stop();
        }
    }
 
    void
    Navigator::resume()
    {
        ARMARX_INFO << "Resume.";
 
        executorEnabled.store(true);
 
        if (srv.executor != nullptr)
        {
            if (hasLocalPlanner())
            {
                srv.executor->start(ExecutorInterface::ControllerType::LocalTrajectory);
            }
            else
            {
                srv.executor->start(ExecutorInterface::ControllerType::GlobalTrajectory);
            }
        }
    }
 
    void
    Navigator::stop()
    {
        ARMARX_INFO << "Stopping.";
 
        pause();
 
        // stop all threads, including this one
        stopAllThreads();
 
        goalReachedMonitor.reset();
        goalReachedMonitor = std::nullopt;
    }
 
    bool
    Navigator::isPaused() const noexcept
    {
        return not executorEnabled.load();
    }
 
    bool
    Navigator::isStopped() const noexcept
    {
        return (not shouldRun) or (not runningTask) or (not runningTask->isRunning());
    }
 
    Navigator::Navigator(Navigator&& other) noexcept :
        config{std::move(other.config)},
        srv{std::move(other.srv)},
        parametrization{std::move(other.parametrization)},
        executorEnabled{other.executorEnabled.load()}
    {
    }
 
 
} // namespace armarx::navigation::server