From d0fe5ba5f1acc7218ef5794633d727f6bdee5eb6 Mon Sep 17 00:00:00 2001
From: Andre Meixner <andre.meixner@kit.edu>
Date: Mon, 28 Oct 2024 19:32:41 -0700
Subject: [PATCH] Updated visualization of framed box locations
---
.../util/LocationLoader/Visu.cpp | 23 ++++++++++++++++++-
.../PriorKnowledge/util/LocationLoader/Visu.h | 7 ++++++
2 files changed, 29 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
index 80dfd636d..8d5c5444a 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 a237c48b3..a7005ff32 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
--
GitLab