From 0e22c69a3acd7c183809e837edadd1eda50dcc62 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Wed, 22 Oct 2025 11:41:38 -0400 Subject: [PATCH 1/2] add text color selection --- .../color/colormap_utilities.h | 15 ++++++ .../include/hydra_visualizer/layer_info.h | 3 ++ hydra_visualizer/src/drawing.cpp | 4 +- hydra_visualizer/src/layer_info.cpp | 48 +++++++++++++++++++ 4 files changed, 68 insertions(+), 2 deletions(-) diff --git a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h index d1c7757..e429e0e 100644 --- a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h +++ b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h @@ -40,6 +40,21 @@ namespace hydra::visualizer { +enum class NamedColors { + BLACK, + WHITE, + RED, + GREEN, + BLUE, + YELLOW, + ORANGE, + PURPLE, + CYAN, + MAGENTA, + PINK, + GRAY, +}; + std_msgs::msg::ColorRGBA makeColorMsg(const spark_dsg::Color& color, std::optional alpha = std::nullopt); diff --git a/hydra_visualizer/include/hydra_visualizer/layer_info.h b/hydra_visualizer/include/hydra_visualizer/layer_info.h index 1205eeb..56580d2 100644 --- a/hydra_visualizer/include/hydra_visualizer/layer_info.h +++ b/hydra_visualizer/include/hydra_visualizer/layer_info.h @@ -108,6 +108,8 @@ struct LayerConfig { bool add_jitter = false; //! @brief amount of jitter to add double jitter_scale = 0.2; //[ 0.05, 5.0] + //! @brief Color to use for text + NamedColors color = NamedColors::BLACK; } text; struct BoundingBoxes { @@ -158,6 +160,7 @@ class LayerInfo { LayerInfo& graph(const spark_dsg::DynamicSceneGraph& graph, spark_dsg::LayerId layer); bool shouldVisualize(const spark_dsg::SceneGraphNode& node) const; + spark_dsg::Color text_color() const; const LayerConfig config; diff --git a/hydra_visualizer/src/drawing.cpp b/hydra_visualizer/src/drawing.cpp index 9f2b759..63d08d3 100644 --- a/hydra_visualizer/src/drawing.cpp +++ b/hydra_visualizer/src/drawing.cpp @@ -436,7 +436,7 @@ MarkerArray makeLayerNodeTextMarkers(const std_msgs::msg::Header& header, marker.text = name.empty() ? NodeSymbol(node->id).str() : name; marker.scale.z = info.config.text.scale; - marker.color = makeColorMsg(Color()); + marker.color = makeColorMsg(info.text_color()); fillPoseWithIdentity(marker.pose); tf2::convert(node->attributes().position, marker.pose.position); @@ -605,7 +605,7 @@ Marker makeLayerTextMarker(const std_msgs::msg::Header& header, marker.id = 0; marker.action = Marker::ADD; marker.scale.z = info.config.text.scale; - marker.color = makeColorMsg(Color()); + marker.color = makeColorMsg(info.text_color()); std::optional best_stamp; Eigen::Vector3d pos = Eigen::Vector3d::Zero(); diff --git a/hydra_visualizer/src/layer_info.cpp b/hydra_visualizer/src/layer_info.cpp index 9adfb22..7b49a32 100644 --- a/hydra_visualizer/src/layer_info.cpp +++ b/hydra_visualizer/src/layer_info.cpp @@ -35,6 +35,7 @@ #include "hydra_visualizer/layer_info.h" #include +#include #include namespace hydra::visualizer { @@ -91,6 +92,22 @@ void declare_config(LayerConfig::Text& config) { field(config.scale, "scale"); field(config.add_jitter, "add_jitter"); field(config.jitter_scale, "jitter_scale"); + enum_field(config.color, + "color", + { + {NamedColors::BLACK, "black"}, + {NamedColors::WHITE, "white"}, + {NamedColors::RED, "red"}, + {NamedColors::GREEN, "green"}, + {NamedColors::BLUE, "blue"}, + {NamedColors::YELLOW, "yellow"}, + {NamedColors::ORANGE, "orange"}, + {NamedColors::PURPLE, "purple"}, + {NamedColors::CYAN, "cyan"}, + {NamedColors::MAGENTA, "magenta"}, + {NamedColors::PINK, "pink"}, + {NamedColors::GRAY, "gray"}, + }); } void declare_config(LayerConfig::BoundingBoxes& config) { @@ -162,6 +179,37 @@ LayerInfo& LayerInfo::graph(const DynamicSceneGraph& graph, LayerId layer) { return *this; } +Color LayerInfo::text_color() const { + switch (config.text.color) { + case NamedColors::BLACK: + return spark_dsg::Color::black(); + case NamedColors::WHITE: + return spark_dsg::Color::white(); + case NamedColors::RED: + return spark_dsg::Color::red(); + case NamedColors::GREEN: + return spark_dsg::Color::green(); + case NamedColors::BLUE: + return spark_dsg::Color::blue(); + case NamedColors::YELLOW: + return spark_dsg::Color::yellow(); + case NamedColors::ORANGE: + return spark_dsg::Color::orange(); + case NamedColors::PURPLE: + return spark_dsg::Color::purple(); + case NamedColors::CYAN: + return spark_dsg::Color::cyan(); + case NamedColors::MAGENTA: + return spark_dsg::Color::magenta(); + case NamedColors::PINK: + return spark_dsg::Color::pink(); + case NamedColors::GRAY: + return spark_dsg::Color::gray(); + default: + return spark_dsg::Color::black(); + } +} + bool LayerInfo::shouldVisualize(const spark_dsg::SceneGraphNode& node) const { if (!config.visualize) { return false; From 8dd45f86395c10d6f2684b4a33c722ed1f8f7d6c Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Wed, 22 Oct 2025 11:56:56 -0400 Subject: [PATCH 2/2] also add edge color selection to visualizer --- .../color/colormap_utilities.h | 2 + .../include/hydra_visualizer/layer_info.h | 7 ++- .../src/color/colormap_utilities.cpp | 31 ++++++++++++ hydra_visualizer/src/drawing.cpp | 4 +- hydra_visualizer/src/layer_info.cpp | 49 ++++++++----------- 5 files changed, 61 insertions(+), 32 deletions(-) diff --git a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h index e429e0e..4fc3ff7 100644 --- a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h +++ b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h @@ -55,6 +55,8 @@ enum class NamedColors { GRAY, }; +spark_dsg::Color colorFromName(NamedColors color); + std_msgs::msg::ColorRGBA makeColorMsg(const spark_dsg::Color& color, std::optional alpha = std::nullopt); diff --git a/hydra_visualizer/include/hydra_visualizer/layer_info.h b/hydra_visualizer/include/hydra_visualizer/layer_info.h index 56580d2..40f3a84 100644 --- a/hydra_visualizer/include/hydra_visualizer/layer_info.h +++ b/hydra_visualizer/include/hydra_visualizer/layer_info.h @@ -74,7 +74,9 @@ struct LayerConfig { double scale = 0.03; //[ 0.001, 1.0] //! @brief intralayer edge alpha double alpha = 1.0; //[ 0.0, 1.0] - //! @brief show intralayer edge in color + //! @brief Color to use for edge + NamedColors color = NamedColors::BLACK; + //! @brief show intralayer edge using node colors bool use_color = true; //! @brief draw interlayer edges bool draw_interlayer = true; @@ -84,7 +86,7 @@ struct LayerConfig { double interlayer_scale = 0.03; // [0.001, 1.0] //! @brief interlayer edge alpha double interlayer_alpha = 1.0; // [0.0, 1.0] - //! @brief show interlayer edge in color + //! @brief show interlayer edge using node colors bool interlayer_use_color = true; //! @brief Number of edges to skip when drawing interlayer edges size_t interlayer_insertion_skip = 0; // [0, 1000] @@ -161,6 +163,7 @@ class LayerInfo { bool shouldVisualize(const spark_dsg::SceneGraphNode& node) const; spark_dsg::Color text_color() const; + spark_dsg::Color edge_color() const; const LayerConfig config; diff --git a/hydra_visualizer/src/color/colormap_utilities.cpp b/hydra_visualizer/src/color/colormap_utilities.cpp index 01cc2f6..570d79b 100644 --- a/hydra_visualizer/src/color/colormap_utilities.cpp +++ b/hydra_visualizer/src/color/colormap_utilities.cpp @@ -73,6 +73,37 @@ std::function lookupColormap(DiscretePalette cmap) { } // namespace +spark_dsg::Color colorFromName(NamedColors color) { + switch (color) { + case NamedColors::BLACK: + return spark_dsg::Color::black(); + case NamedColors::WHITE: + return spark_dsg::Color::white(); + case NamedColors::RED: + return spark_dsg::Color::red(); + case NamedColors::GREEN: + return spark_dsg::Color::green(); + case NamedColors::BLUE: + return spark_dsg::Color::blue(); + case NamedColors::YELLOW: + return spark_dsg::Color::yellow(); + case NamedColors::ORANGE: + return spark_dsg::Color::orange(); + case NamedColors::PURPLE: + return spark_dsg::Color::purple(); + case NamedColors::CYAN: + return spark_dsg::Color::cyan(); + case NamedColors::MAGENTA: + return spark_dsg::Color::magenta(); + case NamedColors::PINK: + return spark_dsg::Color::pink(); + case NamedColors::GRAY: + return spark_dsg::Color::gray(); + default: + return spark_dsg::Color::black(); + } +} + std_msgs::msg::ColorRGBA makeColorMsg(const Color& color, std::optional alpha) { std_msgs::msg::ColorRGBA msg; msg.r = static_cast(color.r) / 255.0; diff --git a/hydra_visualizer/src/drawing.cpp b/hydra_visualizer/src/drawing.cpp index 63d08d3..8816412 100644 --- a/hydra_visualizer/src/drawing.cpp +++ b/hydra_visualizer/src/drawing.cpp @@ -527,9 +527,9 @@ Marker makeLayerEdgeMarkers(const std_msgs::msg::Header& header, marker.points.push_back(target); const auto c_source = - info.config.edges.use_color ? info.node_color(source_node) : Color(); + info.config.edges.use_color ? info.node_color(source_node) : info.edge_color(); const auto c_target = - info.config.edges.use_color ? info.node_color(target_node) : Color(); + info.config.edges.use_color ? info.node_color(target_node) : info.edge_color(); marker.colors.push_back(makeColorMsg(c_source, info.config.edges.alpha)); marker.colors.push_back(makeColorMsg(c_target, info.config.edges.alpha)); } diff --git a/hydra_visualizer/src/layer_info.cpp b/hydra_visualizer/src/layer_info.cpp index 7b49a32..038a1d3 100644 --- a/hydra_visualizer/src/layer_info.cpp +++ b/hydra_visualizer/src/layer_info.cpp @@ -72,6 +72,22 @@ void declare_config(LayerConfig::Edges& config) { field(config.draw, "draw"); field(config.scale, "scale"); field(config.alpha, "alpha"); + enum_field(config.color, + "color", + { + {NamedColors::BLACK, "black"}, + {NamedColors::WHITE, "white"}, + {NamedColors::RED, "red"}, + {NamedColors::GREEN, "green"}, + {NamedColors::BLUE, "blue"}, + {NamedColors::YELLOW, "yellow"}, + {NamedColors::ORANGE, "orange"}, + {NamedColors::PURPLE, "purple"}, + {NamedColors::CYAN, "cyan"}, + {NamedColors::MAGENTA, "magenta"}, + {NamedColors::PINK, "pink"}, + {NamedColors::GRAY, "gray"}, + }); field(config.use_color, "use_color"); field(config.draw_interlayer, "draw_interlayer"); field(config.interlayer_use_source, "interlayer_use_source"); @@ -180,34 +196,11 @@ LayerInfo& LayerInfo::graph(const DynamicSceneGraph& graph, LayerId layer) { } Color LayerInfo::text_color() const { - switch (config.text.color) { - case NamedColors::BLACK: - return spark_dsg::Color::black(); - case NamedColors::WHITE: - return spark_dsg::Color::white(); - case NamedColors::RED: - return spark_dsg::Color::red(); - case NamedColors::GREEN: - return spark_dsg::Color::green(); - case NamedColors::BLUE: - return spark_dsg::Color::blue(); - case NamedColors::YELLOW: - return spark_dsg::Color::yellow(); - case NamedColors::ORANGE: - return spark_dsg::Color::orange(); - case NamedColors::PURPLE: - return spark_dsg::Color::purple(); - case NamedColors::CYAN: - return spark_dsg::Color::cyan(); - case NamedColors::MAGENTA: - return spark_dsg::Color::magenta(); - case NamedColors::PINK: - return spark_dsg::Color::pink(); - case NamedColors::GRAY: - return spark_dsg::Color::gray(); - default: - return spark_dsg::Color::black(); - } + return colorFromName(config.text.color); +} + +Color LayerInfo::edge_color() const { + return colorFromName(config.edges.color); } bool LayerInfo::shouldVisualize(const spark_dsg::SceneGraphNode& node) const {