Visu.cpp

Go to the documentation of this file.

#include "Visu.h"
 
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/KellyLUT.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
        std::smatch matches;
        if (std::regex_match(id, matches, pattern))
        {
            const std::string& affordance_name = matches[1];
            unique_affordances.insert(affordance_name);
            unsigned int index = std::distance(unique_affordances.begin(),
                                               unique_affordances.find(affordance_name)) %
                                 simox::color::KellyLUT::KELLY_COLORS_COLOR_BLIND.size();
            layer.add(armarx::viz::Text(id + "_name")
                          .text(affordance_name)
                          .color(simox::color::Color::black())
                          .scale(10.0f)
                          .pose(locationGlobalPose));
            layer.add(
                armarx::viz::Box(id)
                    .pose(locationGlobalPose)
                    .size(extends)
                    .color(simox::color::KellyLUT::KELLY_COLORS_COLOR_BLIND.at(index).with_alpha(
                        0.5f)));
        }
        else
        {
            layer.add(armarx::viz::Text(id + "_name").text(id).pose(locationGlobalPose));
            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