Navigator.cpp

Go to the documentation of this file.

#include "Navigator.h"
 
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <optional>
#include <thread>
 
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/subgraph.hpp>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
 
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/LocationUtils.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/server/GraphBuilder.h>
#include <armarx/navigation/server/StackResult.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
 
#include "range/v3/algorithm/reverse.hpp"
#include "range/v3/range/conversion.hpp"
#include <SemanticObjectRelations/Shapes/Shape.h>
#include <range/v3/view.hpp>
#include <range/v3/view/reverse.hpp>
 
namespace armarx::navigation::server
{
    void
    stopIfRunning(PeriodicTask<Navigator>::pointer_type& task)
    {
        if (task)
        {
            task->stop();
        }
    }
 
    Navigator::Navigator(const Config& config, const InjectedServices& services) :
        config{config}, srv{services}
    {
        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()));
 
                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::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 `" << 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 `" << 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 `" << goalVertex.attrib().getName() << "`";
 
        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().getName() << " couldn't be resolved ("
            << goal.errorMsg << ")";
        return goal.pose.value();
    }
 
    void
    Navigator::moveTo(const std::vector<client::WaypointTarget>& targets,
                      core::NavigationFrame navigationFrame)
    {
        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(), GoalReachedMonitorConfig());
 
        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().getName();
            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};
 
        // the following is similar to moveToAbsolute
        // TODO(fabian.reister): remove code duplication
 
        srv.executor->execute(globalPlan->trajectory);
 
        ARMARX_TRACE;
        srv.publisher->globalTrajectoryUpdated(globalPlan.value());
 
        ARMARX_TRACE;
        srv.introspector->onGlobalPlannerResult(globalPlan.value());
 
        ARMARX_INFO << "Global planning completed. Will now start all required threads";
        ARMARX_TRACE;
 
        startStack();
    }
 
    void
    Navigator::startStack()
    {
 
        ARMARX_TRACE;
 
        ARMARX_INFO << "Starting stack.";
 
        // FIXME instead of PeriodicTask, use RunningTask.
        runningTask = new PeriodicTask<Navigator>(this,
                                                  &Navigator::run,
                                                  config.general.tasks.replanningUpdatePeriod,
                                                  false,
                                                  "PeriodicTask");
        runningTask->start();
 
        // FIXME create separate function for this.
        if (hasLocalPlanner())
        {
            srv.executor->start(ExecutorInterface::ControllerType::LocalTrajectory);
        }
        else
        {
            srv.executor->start(ExecutorInterface::ControllerType::GlobalTrajectory);
        }
 
        // Could be required if pauseMovement() has been called in the past.
        resume();
        srv.publisher->movementStarted(core::MovementStartedEvent{
            {armarx::Clock::Now()}, core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
    }
 
    void
    Navigator::moveToAbsolute(const std::vector<core::Pose>& waypoints)
    {
        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);
 
        srv.sceneProvider->synchronize(Clock::Now());
 
        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(), GoalReachedMonitorConfig());
 
        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 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()}, {""}});
 
            srv.introspector->failure();
            return;
        }
 
        ARMARX_TRACE;
        srv.publisher->globalTrajectoryUpdated(globalPlan.value());
        srv.introspector->onGlobalPlannerResult(globalPlan.value());
 
        if (not hasLocalPlanner())
        {
            updateExecutor(globalPlan.value());
        }
 
        ARMARX_INFO << "Global planning completed. Will now start all required threads";
        ARMARX_TRACE;
 
        startStack();
 
        ARMARX_INFO << "Movement started.";
    }
 
    void
    Navigator::updateAbsolute(const std::vector<core::Pose>& waypoints)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_EMPTY(waypoints);
 
 
        // 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()}, {""}});
 
            srv.introspector->failure();
            return;
        }
 
        ARMARX_TRACE;
        srv.publisher->globalTrajectoryUpdated(globalPlan.value());
        srv.introspector->onGlobalPlannerResult(globalPlan.value());
        // srv.executor->execute(globalPlan->trajectory);
 
        ARMARX_INFO << "Global planning completed. Will now start all required threads";
        ARMARX_TRACE;
 
        startStack();
 
        ARMARX_INFO << "Movement started.";
    }
 
    void
    Navigator::moveTowards(const core::Direction& direction, core::NavigationFrame navigationFrame)
    {
    }
 
    void
    Navigator::moveToLocation(const std::string& location)
    {
        // update static scene including locations
        updateScene(true);
 
        const auto resolveLocation = [&](const std::string& location)
        {
            const auto locations = srv.sceneProvider->scene().staticScene->locations;
            ARMARX_CHECK(locations.count(location) > 0) << "Unknown location `" << location << "`.";
            return locations.at(location);
        };
 
        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[1];
        }
        const auto goal = core::resolveLocation(srv.sceneProvider->scene().staticScene->objectMap,
                                                srv.sceneProvider->scene().staticScene->objectInfo,
                                                resolveLocation(split[0]).framedPose,
                                                instanceID);
 
        if (goal.pose.has_value())
        {
            moveToAbsolute({goal.pose.value()});
        }
        else
        {
            ARMARX_ERROR << goal.errorMsg;
        }
    }
 
    void
    Navigator::moveTowardsAbsolute(const core::Direction& direction)
    {
    }
 
    void
    Navigator::run()
    {
        // TODO(fabian.reister): add debug observer logging
 
        // scene update
        {
            ARMARX_DEBUG << "Updating scene";
 
            const Duration duration =
                armarx::core::time::StopWatch::measure([&]() { updateScene(); });
 
            ARMARX_DEBUG << deactivateSpam(0.2)
                         << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
 
            srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]",
                                                               duration.toMilliSecondsDouble());
        }
 
        // eventually, draw
        if (srv.introspector and srv.sceneProvider 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
        {
            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(), GoalReachedMonitorConfig());
 
                    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());
                    }
                }
            }
        }
 
        // local planner update
        {
            ARMARX_DEBUG << "Local planner update";
 
            const Duration duration = armarx::core::time::StopWatch::measure(
                [&]()
                {
                    if (hasLocalPlanner())
                    {
                        const auto localPlannerResult = updateLocalPlanner();
                        updateExecutor(localPlannerResult);
                        updateIntrospector(localPlannerResult);
 
                        if (srv.executor != nullptr)
                        {
                            srv.executor->ensureIsActive(
                                ExecutorInterface::ControllerType::LocalTrajectory);
                        }
                    }
                    else if (srv.executor != nullptr)
                    {
                        srv.executor->ensureIsActive(
                            ExecutorInterface::ControllerType::GlobalTrajectory);
                    }
                });
            ARMARX_DEBUG << deactivateSpam(0.2)
                         << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
 
            srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]",
                                                               duration.toMilliSecondsDouble());
        }
 
        // monitor update
        {
            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());
        }
    }
 
    void
    Navigator::updateScene(const bool fullUpdate)
    {
        ARMARX_CHECK_NOT_NULL(srv.sceneProvider);
        srv.sceneProvider->synchronize(armarx::Clock::Now(), fullUpdate);
    }
 
    std::optional<local_planning::LocalPlannerResult>
    Navigator::updateLocalPlanner()
    {
        ARMARX_CHECK(hasLocalPlanner());
 
        ARMARX_VERBOSE << "Updating local plan";
 
        try
        {
            localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
            if (localPlan.has_value())
            {
                srv.publisher->localTrajectoryUpdated(localPlan.value());
            }
            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!";
            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;
 
        srv.executor->execute(localPlan->trajectory, true);
    }
 
    void
    Navigator::updateExecutor(const std::optional<global_planning::GlobalPlannerResult>& globalPlan)
    {
        if (srv.executor == nullptr)
        {
            return;
        }
 
        ARMARX_IMPORTANT << "Requested to execute global plan with "
                         << globalPlan->trajectory.points().size() << " points.";
 
 
        // if (isPaused() or isStopped())
        // {
        //     // [[unlikely]]
        //     ARMARX_VERBOSE << deactivateSpam(1) << "stopped or paused";
        //     return;
        // }
 
        if (not globalPlan.has_value())
        {
            ARMARX_INFO << "Global plan is invalid!";
            srv.executor->stop();
            return;
        }
 
        ARMARX_IMPORTANT << "Executing global plan with " << globalPlan->trajectory.points().size()
                         << " points.";
        srv.executor->execute(globalPlan->trajectory, 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 isPaused() and goalReachedMonitor->goalReached())
        {
            // [[unlikely]]
 
            ARMARX_INFO << "Goal " << goalReachedMonitor->goal().translation().head<2>()
                        << " reached!";
 
            stop();
 
            srv.publisher->goalReached(core::GoalReachedEvent{
                {armarx::Clock::Now()},
                {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
 
            srv.introspector->success();
 
            // stop all threads, including this one
            ARMARX_INFO << "Stopping all threads";
            stopAllThreads();
 
            goalReachedMonitor.reset();
            goalReachedMonitor = std::nullopt;
        }
    }
 
    void
    Navigator::stopAllThreads()
    {
        stopIfRunning(runningTask);
    }
 
    // 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::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
    {
        // TODO: Implement.
        return false;
    }
 
    Navigator::Navigator(Navigator&& other) noexcept :
        config{std::move(other.config)},
        srv{std::move(other.srv)},
        executorEnabled{other.executorEnabled.load()}
    {
    }
} // namespace armarx::navigation::server