ArvizIntrospector.cpp

Go to the documentation of this file.

#include "ArvizIntrospector.h"
 
#include <algorithm>
#include <cstddef>
#include <iterator>
#include <optional>
#include <string>
#include <utility>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <range/v3/algorithm/max_element.hpp>
 
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/ArViz/Client/elements/Path.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.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/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
 
namespace armarx::navigation::server
{
 
    namespace
    {
        /// How far apart the markers drawn along a path are. The trajectory carries one point
        /// per parametrization sample -- 500 of them -- which is the resolution the solver
        /// wants, not the resolution an observer can read.
        constexpr float markerSpacing = 200.F; // [mm]
 
        /**
         * @brief Indices into `points`, spaced at least `spacing` apart along the path.
         *
         * The first and the last point are always kept: the start and the goal are the two an
         * observer looks for, and the last one carries the final velocity.
         */
        std::vector<std::size_t>
        subsample(const std::vector<core::GlobalTrajectoryPoint>& points, const float spacing)
        {
            if (points.size() < 2)
            {
                return points.empty() ? std::vector<std::size_t>{} : std::vector<std::size_t>{0};
            }
 
            std::vector<std::size_t> indices{0};
            float sinceLast = 0.F;
 
            for (std::size_t i = 1; i + 1 < points.size(); i++)
            {
                sinceLast += (points.at(i).waypoint.pose.translation() -
                              points.at(i - 1).waypoint.pose.translation())
                                 .norm();
 
                if (sinceLast >= spacing)
                {
                    indices.push_back(i);
                    sinceLast = 0.F;
                }
            }
 
            indices.push_back(points.size() - 1);
 
            return indices;
        }
    } // namespace
 
    inline armarx::PackagePath
    asPackagePath(const std::string& absfilepath)
    {
        const std::vector<std::string> packages =
            armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
        const std::string package = armarx::ArmarXDataPath::getProject(packages, absfilepath);
 
        // make sure that the relative path is without the 'package/' prefix
        const std::string relPath = [&absfilepath, &package]() -> std::string
        {
            if (simox::alg::starts_with(absfilepath, package))
            {
                // remove "package" + "/"
                return absfilepath.substr(package.size() + 1, -1);
            }
 
            return absfilepath;
        }();
 
        return {package, relPath};
    }
 
    ArvizIntrospector::ArvizIntrospector(armarx::viz::Client arviz,
                                         const VirtualRobot::RobotConstPtr& robot,
                                         const objpose::ObjectPoseClient& objClient) :
        arviz(arviz), robot(robot), objClient(objClient)
    // visualization(arviz,
    //               util::Visualization::Params{.robotModelFileName = asPackagePath(robot->getFilename())})
    {
        robotPosesLayer = arviz.layer("robot_poses");
    }
 
    // TODO maybe run with predefined frequency instead of
 
    void
    ArvizIntrospector::onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result)
    {
        ARMARX_DEBUG << "ArvizIntrospector::onGlobalPlannerResult";
 
        drawGlobalTrajectory(result.trajectory);
        if (result.helperTrajectory)
        {
            drawGlobalHelperTrajectory(result.helperTrajectory.value());
        }
        else
        {
            // clear layer
            auto layer = arviz.layer("global_helper_trajectory");
            layers[layer.data_.name] = std::move(layer);
        }
        arviz.commit(simox::alg::get_values(layers));
 
 
        // visualization.visualize(result.trajectory);
    }
 
    void
    ArvizIntrospector::onGlobalPlannerSubdivision(const GlobalPathSubdivision& subdivision)
    {
        ARMARX_DEBUG << "ArvizIntrospector::onGlobalPlannerSubdivision";
 
        drawGlobalPathSubdivision(subdivision);
 
        if (subdivision.plan.helperTrajectory)
        {
            drawGlobalHelperTrajectory(subdivision.plan.helperTrajectory.value());
        }
        else
        {
            // clear layer
            auto layer = arviz.layer("global_helper_trajectory");
            layers[layer.data_.name] = std::move(layer);
        }
 
        arviz.commit(simox::alg::get_values(layers));
    }
 
    void
    ArvizIntrospector::onLocalPlannerResult(
        const std::optional<local_planning::LocalPlannerResult>& result)
    {
        if (result)
        {
            drawLocalTrajectory(result.value().trajectory);
        }
        else
        {
            drawLocalTrajectory(std::nullopt);
        }
 
        arviz.commit(simox::alg::get_values(layers));
    }
 
    // void
    // ArvizIntrospector::onTrajectoryControllerResult(
    //     const traj_ctrl::TrajectoryControllerResult& result)
    // {
    //     std::lock_guard g{mtx};
 
    //     drawRawVelocity(result.twist);
    // }
 
    // void
    // ArvizIntrospector::onSafetyGuardResult(const safety_guard::SafetyGuardResult& result)
    // {
    //     std::lock_guard g{mtx};
 
    //     drawSafeVelocity(result.twist);
    // }
 
    void
    ArvizIntrospector::onGoal(const core::Pose& goal)
    {
        auto layer = arviz.layer("goal");
        layer.add(viz::Pose("goal").pose(goal).scale(3));
 
        // A new goal starts a new travelled path -- otherwise the polyline would jump from
        // wherever the last request ended to wherever this one starts.
        travelled.clear();
        lastPose.reset();
        robotPosesLayer.clear();
 
        arviz.commit({layer, robotPosesLayer});
 
        // visualization.setTarget(goal);
    }
 
    void
    ArvizIntrospector::onRobotPose(const core::Pose& pose)
    {
        // Recorded finer than it is marked: the polyline is only as faithful to what the robot
        // did as the samples behind it, while the orientation markers are for reading.
        constexpr float recordingSpacing = 50.F; // [mm]
 
        if (lastPose and (lastPose->translation() - pose.translation()).norm() < recordingSpacing)
        {
            return;
        }
 
        lastPose = pose;
        travelled.push_back(pose);
 
        // Rebuilt rather than appended to: the path is one element covering every sample so far,
        // so it has to replace its predecessor. This runs once per `recordingSpacing` of motion,
        // not per control cycle.
        robotPosesLayer.clear();
 
        std::vector<core::Position> positions;
        positions.reserve(travelled.size());
        std::transform(travelled.begin(),
                       travelled.end(),
                       std::back_inserter(positions),
                       [](const core::Pose& p) -> core::Position { return p.translation(); });
 
        robotPosesLayer.add(
            viz::Path("travelled").points(positions).color(viz::Color::orange()).width(10));
 
        const auto markerStride =
            static_cast<std::size_t>(std::max(1.F, markerSpacing / recordingSpacing));
 
        for (std::size_t i = 0; i < travelled.size(); i += markerStride)
        {
            robotPosesLayer.add(viz::Pose("pose" + std::to_string(i)).pose(travelled.at(i)));
        }
 
        arviz.commit(robotPosesLayer);
    }
 
    void
    ArvizIntrospector::onGlobalShortestPath(const std::vector<core::Pose>& path)
    {
        auto layer = arviz.layer("graph_shortest_path");
 
        const auto toPosition = [](const core::Pose& pose) -> core::Position
        { return pose.translation(); };
 
 
        std::vector<core::Position> pts;
        pts.reserve(path.size());
        std::transform(path.begin(), path.end(), std::back_inserter(pts), toPosition);
 
 
        // layer.add(viz::Path("shortest_path").points(pts).color(viz::Color::purple()));
 
        for (size_t i = 0; i < (pts.size() - 1); i++)
        {
            layer.add(viz::Arrow("segment_" + std::to_string(i))
                          .fromTo(pts.at(i), pts.at(i + 1))
                          .color(viz::Color::purple()));
        }
 
 
        arviz.commit(layer);
    }
 
    // private methods
 
 
    void
    ArvizIntrospector::drawGlobalTrajectory(const core::GlobalTrajectory& trajectory)
    {
        drawGlobalTrajectory(trajectory, "global_planner", simox::Color::blue());
    }
 
    void
    ArvizIntrospector::drawGlobalHelperTrajectory(const core::GlobalTrajectory& trajectory)
    {
        drawGlobalTrajectory(trajectory, "global_helper_trajectory", simox::Color::gray());
    }
 
    void
    ArvizIntrospector::drawGlobalTrajectory(const core::GlobalTrajectory& trajectory,
                                            const std::string layerName,
                                            simox::color::Color color)
    {
        auto layer = arviz.layer(layerName);
 
        layer.add(viz::Path("path").points(trajectory.positions()).color(color));
 
        const auto cmap = simox::color::cmaps::viridis();
 
        const float maxVelocity = ranges::max_element(trajectory.points(),
                                                      std::less{},
                                                      &core::GlobalTrajectoryPoint::velocity)
                                      ->velocity;
 
 
        for (const std::size_t idx : subsample(trajectory.points(), markerSpacing))
        {
            const core::GlobalTrajectoryPoint& tp = trajectory.points().at(idx);
            const float scale = tp.velocity;
 
            const Eigen::Vector3f target =
                scale * tp.waypoint.pose.linear() * Eigen::Vector3f::UnitY();
 
            layer.add(
                viz::Arrow("velocity_" + std::to_string(idx))
                    .fromTo(tp.waypoint.pose.translation(), tp.waypoint.pose.translation() + target)
                    .color(cmap.at(tp.velocity / maxVelocity)));
        }
 
        layers[layer.data_.name] = std::move(layer);
    }
 
    void
    ArvizIntrospector::drawGlobalPathSubdivision(const GlobalPathSubdivision& subdivision)
    {
        if (subdivision.subdivision.empty())
        {
            // no actual subdivision
            drawGlobalTrajectory(subdivision.plan.trajectory);
            return;
        }
 
        auto layer = arviz.layer("global_path_subdivision");
 
        for (const auto& segment : subdivision.subdivision)
        {
            viz::Path path("segment_" + std::to_string(segment.s));
            // subTrajectory from [s,t] to overlap with next segment
            const auto& subTrajectory = subdivision.plan.trajectory.getSubTrajectory(
                segment.s, std::min(segment.t + 1, subdivision.plan.trajectory.points().size()));
            path.points(subTrajectory.positions());
            layer.add(
                path.color(segment.useLocalPlanner ? simox::Color::blue() : simox::Color::red()));
        }
 
        const auto cmap = simox::color::cmaps::viridis();
        const float maxVelocity = ranges::max_element(subdivision.plan.trajectory.points(),
                                                      std::less{},
                                                      &core::GlobalTrajectoryPoint::velocity)
                                      ->velocity;
 
        for (const std::size_t idx :
             subsample(subdivision.plan.trajectory.points(), markerSpacing))
        {
            const core::GlobalTrajectoryPoint& tp = subdivision.plan.trajectory.points().at(idx);
            const float scale = tp.velocity;
 
            const Eigen::Vector3f target =
                scale * tp.waypoint.pose.linear() * Eigen::Vector3f::UnitY();
 
            layer.add(
                viz::Arrow("velocity_" + std::to_string(idx))
                    .fromTo(tp.waypoint.pose.translation(), tp.waypoint.pose.translation() + target)
                    .color(cmap.at(tp.velocity / maxVelocity)));
        }
 
        layers[layer.data_.name] = std::move(layer);
    }
 
    void
    ArvizIntrospector::drawLocalTrajectory(const std::optional<core::LocalTrajectory>& input)
    {
        if (!input)
        {
            // no local trajectory found, remove old one
            auto layer = arviz.layer("local_planner");
            auto velLayer = arviz.layer("local_planner_velocity");
 
            layers[layer.data_.name] = std::move(layer);
            layers[velLayer.data_.name] = std::move(velLayer);
            return;
        }
 
        const auto trajectory = input.value();
 
        auto layer = arviz.layer("local_planner");
 
        const std::vector<Eigen::Vector3f> points =
            simox::alg::apply(trajectory.points(),
                              [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
                              { return pt.pose.translation(); });
 
        layer.add(viz::Path("path").points(points).color(simox::Color::green()));
 
 
        // Visualize trajectory speed
        auto velLayer = arviz.layer("local_planner_velocity");
 
        simox::ColorMap cm = simox::color::cmaps::inferno();
        cm.set_vmin(0);
        cm.set_vmax(0.6);
 
        for (size_t i = 0; i < trajectory.points().size() - 1; i++)
        {
            const core::LocalTrajectoryPoint start = trajectory.points().at(i);
            const core::LocalTrajectoryPoint end = trajectory.points().at(i + 1);
 
            const Duration dT = end.timestamp - start.timestamp;
            const Eigen::Vector3f distance = end.pose.translation() - start.pose.translation();
            const float speed = distance.norm() / 1000 / dT.toSecondsDouble();
 
            const Eigen::Vector3f pos = start.pose.translation() + distance / 2;
            const simox::Color color = cm.at(speed);
 
            velLayer.add(
                viz::Sphere("velocity_" + std::to_string(i)).position(pos).radius(50).color(color));
        }
 
        layers[layer.data_.name] = std::move(layer);
        layers[velLayer.data_.name] = std::move(velLayer);
    }
 
    void
    ArvizIntrospector::drawRawVelocity(const core::Twist& twist)
    {
        auto layer = arviz.layer("trajectory_controller");
 
        layer.add(viz::Arrow("linear_velocity")
                      .fromTo(robot->getGlobalPosition(),
                              core::Pose(robot->getGlobalPose()) * twist.linear)
                      .color(simox::Color::orange()));
 
        layers[layer.data_.name] = std::move(layer);
    }
 
    void
    ArvizIntrospector::drawSafeVelocity(const core::Twist& twist)
    {
        auto layer = arviz.layer("safety_guard");
 
        layer.add(viz::Arrow("linear_velocity")
                      .fromTo(robot->getGlobalPosition(),
                              core::Pose(robot->getGlobalPose()) * twist.linear)
                      .color(simox::Color::green()));
 
        layers[layer.data_.name] = std::move(layer);
    }
 
    ArvizIntrospector::ArvizIntrospector(ArvizIntrospector&& other) noexcept :
        arviz{other.arviz},
        robot{other.robot},
        layers(std::move(other.layers)),
        lastPose(other.lastPose),
        travelled(std::move(other.travelled)),
        // Not moving this left the moved-to introspector with a default-constructed layer,
        // whose component and name are empty -- so every `commit` of it published nothing the
        // GUI could show. The navigator move-constructs its introspector on connect, which is
        // why the travelled path was implemented but never appeared.
        robotPosesLayer(std::move(other.robotPosesLayer))
    // visualization(std::move(other.visualization))
    {
    }
 
    void
    ArvizIntrospector::clear()
    {
        // clear all layers
        for (auto& [name, layer] : layers)
        {
            layer.markForDeletion();
        }
        arviz.commit(simox::alg::get_values(layers));
        layers.clear();
 
        // some special internal layers of TEB
        arviz.commitDeleteLayer("local_planner_obstacles");
        arviz.commitDeleteLayer("local_planner_velocity");
        arviz.commitDeleteLayer("local_planner_path_alternatives");
    }
 
    ArvizIntrospector&
    ArvizIntrospector::operator=(ArvizIntrospector&&) noexcept
    {
        return *this;
    }
 
    void
    ArvizIntrospector::onGlobalGraph(const core::Graph& graph)
    {
        auto layer = arviz.layer("global_planning_graph");
 
        const objpose::ObjectPoseMap objects = objClient.fetchObjectPosesAsMap();
        const std::vector<ObjectInfo> info = objClient.getObjectFinder().findAllObjects();
 
        for (const auto& edge : graph.edges())
        {
 
            const auto sourcePose = graph.vertex(edge.sourceDescriptor()).attrib().getPose();
            const auto targetPose = graph.vertex(edge.targetDescriptor()).attrib().getPose();
 
 
            const viz::Color color = [&]()
            {
                if (edge.attrib().cost() > 100'000) // "NaN check"
                {
                    return viz::Color::red();
                }
 
                return viz::Color::green();
            }();
 
            const auto from = core::resolveLocation(objects, info, sourcePose);
            const auto to = core::resolveLocation(objects, info, sourcePose);
 
            if (from.pose.has_value() && to.pose.has_value())
            {
                layer.add(
                    viz::Arrow(std::to_string(edge.sourceObjectID().t) + " -> " +
                               std::to_string(edge.targetObjectID().t))
                        .fromTo(from.pose.value().translation(), to.pose.value().translation())
                        .color(color));
            }
        }
 
        arviz.commit(layer);
    }
 
    void
    ArvizIntrospector::callGenericDrawFunction(std::function<void(viz::Client&)> fn)
    {
        fn(arviz);
    }
 
    void
    ArvizIntrospector::success()
    {
        clear();
 
        // visualization.success();
    }
 
    void
    ArvizIntrospector::failure()
    {
        clear();
 
        // visualization.failed();
    }
 
 
} // namespace armarx::navigation::server