diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp index 80dfd636de50f94003f673cb47114c86afdcb844..8d5c5444aaf72999d8a10e6d84eb3d1026996ffa 100644 --- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp +++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp @@ -1,5 +1,8 @@ #include "Visu.h" +#include <SimoxUtility/color/Color.h> +#include <SimoxUtility/color/KellyLUT.h> + #include <ArmarXCore/core/logging/Logging.h> namespace armarx::priorknowledge::util::location @@ -25,10 +28,28 @@ namespace armarx::priorknowledge::util::location const Eigen::Vector3f& extends) const { // Add global location to layer - layer.add(armarx::viz::Box(id) + 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 diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h index a237c48b3ee1ae78bce651ab887f37c3aa2569df..a7005ff3296f94fb606fef9c13e69f6e858accc3 100644 --- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h +++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h @@ -1,5 +1,8 @@ #pragma once +#include <regex> +#include <unordered_map> + #include <RobotAPI/components/ArViz/Client/Client.h> #include <RobotAPI/components/ArViz/Client/ScopedClient.h> @@ -60,5 +63,9 @@ namespace armarx::priorknowledge::util::location protected: viz::Client& arviz; + + private: + const std::regex pattern = std::regex(R"(^[^:]+\/([^:]+?):[^:]+?$)"); + mutable std::set<std::string> unique_affordances; }; } // namespace armarx::priorknowledge::util::location