Visu.cpp

Go to the documentation of this file.

#include "Visu.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::priorknowledge::util::location
{
    void
    Visu::addFramedLocationToLayer(viz::Layer& layer,
                                   const std::string& id,
                                   const Eigen::Matrix4f& locationGlobalPose) const
    {
        // Add global location to layer
        layer.add(armarx::viz::Pose(id).pose(locationGlobalPose));
        layer.add(armarx::viz::Arrow(id + "_dir")
                      .pose(locationGlobalPose)
                      .length(110)
                      .width(7.5)
                      .color(this->settings.framedLocationArrowColor));
    }
 
    void
    Visu::addFramedBoxedLocationToLayer(viz::Layer& layer,
                                        const std::string& id,
                                        const Eigen::Matrix4f& locationGlobalPose,
                                        const Eigen::Vector3f& extends) const
    {
        // Add global location to layer
        layer.add(armarx::viz::Box(id)
                      .pose(locationGlobalPose)
                      .size(extends)
                      .color(this->settings.framedBoxedLocationColor));
    }
 
    viz::Layer
    Visu::locationsToLayer(const std::string& layerName,
                           const std::map<std::string, FramedLocationData>& locationData) const
    {
        auto layer = arviz.layer(layerName);
        for (auto& [id, data] : locationData)
        {
            if (data.extents.has_value())
            {
                addFramedBoxedLocationToLayer(layer, id, data.globalPose, data.extents.value());
            }
            else
            {
                addFramedLocationToLayer(layer, id, data.globalPose);
            }
        }
        return layer;
    }
 
    viz::Layer
    Visu::framedLocationsToLayer(
        const std::string& layerName,
        const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const
    {
        auto layer = arviz.layer(layerName);
        for (auto& [id, pose] : locationGlobalPoses)
        {
            // Add global location to layer
            addFramedLocationToLayer(layer, id, pose);
        }
        return layer;
    }
 
    viz::Layer
    Visu::framedBoxedLocationsToLayer(
        const std::string& layerName,
        const std::map<std::string, std::pair<Eigen::Matrix4f, Eigen::Vector3f>>&
            locationGlobalPosesAndExtends) const
    {
        auto layer = arviz.layer(layerName);
        for (auto& [id, pair] : locationGlobalPosesAndExtends)
        {
            const auto& pose = pair.first;
            const auto& extends = pair.second;
 
            // Add global location to layer
            addFramedBoxedLocationToLayer(layer, id, pose, extends);
        }
        return layer;
    }
} // namespace armarx::priorknowledge::util::location