diff --git a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h index d1c7757..4fc3ff7 100644 --- a/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h +++ b/hydra_visualizer/include/hydra_visualizer/color/colormap_utilities.h @@ -40,6 +40,23 @@ namespace hydra::visualizer { +enum class NamedColors { + BLACK, + WHITE, + RED, + GREEN, + BLUE, + YELLOW, + ORANGE, + PURPLE, + CYAN, + MAGENTA, + PINK, + 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 1205eeb..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] @@ -108,6 +110,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 +162,8 @@ 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; + 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 9f2b759..8816412 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); @@ -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)); } @@ -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..038a1d3 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 { @@ -71,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"); @@ -91,6 +108,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 +195,14 @@ LayerInfo& LayerInfo::graph(const DynamicSceneGraph& graph, LayerId layer) { return *this; } +Color LayerInfo::text_color() const { + return colorFromName(config.text.color); +} + +Color LayerInfo::edge_color() const { + return colorFromName(config.edges.color); +} + bool LayerInfo::shouldVisualize(const spark_dsg::SceneGraphNode& node) const { if (!config.visualize) { return false;