diff --git a/hydra_visualizer/CMakeLists.txt b/hydra_visualizer/CMakeLists.txt index e6f9f5c..b11c936 100644 --- a/hydra_visualizer/CMakeLists.txt +++ b/hydra_visualizer/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(visualization_msgs REQUIRED) add_library( ${PROJECT_NAME} src/plugins/traversability_plugin.cpp + src/adapters/edge_color.cpp src/adapters/graph_color.cpp src/adapters/mesh_color.cpp src/adapters/text.cpp diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index a889601..b3ba352 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/hydra_visualizer/config/visualizer_config.yaml @@ -1,60 +1,69 @@ --- renderer: - layer_z_step: 5.5 # unit separation between layers - collapse_layers: false # whether or not to apply offsets to each of the layers + layer_z_step: 5.5 # unit separation between layers + collapse_layers: false # whether or not to apply offsets to each of the layers layers: 2: z_offset_scale: 2.0 visualize: true - nodes: {scale: 0.40, color: {type: LabelColorAdapter}, alpha: 0.8, use_sphere: false} - text: {draw: true, collapse: true, adapter: {type: LabelTextAdapter}, height: 0.5, scale: 0.45} - bounding_boxes: {draw: true, collapse: true, scale: 0.05, edge_scale: 0.05, alpha: 0.9, edge_break_ratio: 0.5} - edges: {interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.9, interlayer_use_color: true} + nodes: { scale: 0.40, color: { type: LabelColorAdapter }, alpha: 0.8, use_sphere: false } + text: { draw: true, collapse: true, adapter: { type: LabelTextAdapter }, height: 0.5, scale: 0.45 } + bounding_boxes: { draw: true, collapse: true, scale: 0.05, edge_scale: 0.05, alpha: 0.9, edge_break_ratio: 0.5 } + edges: { interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.9 } 3: z_offset_scale: 3.0 visualize: true - nodes: {scale: 0.2, color: {type: ParentColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.9, use_sphere: true} + nodes: + { + scale: 0.2, + color: { type: ParentColorAdapter, colormap: { palette: colorbrewer } }, + alpha: 0.9, + use_sphere: true, + } edges: scale: 0.01 alpha: 0.5 - use_color: false + color: { type: UniformEdgeColorAdapter } interlayer_use_source: false interlayer_scale: 0.08 interlayer_alpha: 0.4 - interlayer_use_color: true interlayer_insertion_skip: 0 4: z_offset_scale: 4.2 visualize: true - nodes: {scale: 0.6, color: {type: IdColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.8, use_sphere: false} - text: {draw: true, height: 1.25, scale: 1.0} + nodes: + { + scale: 0.6, + color: { type: IdColorAdapter, colormap: { palette: colorbrewer } }, + alpha: 0.8, + use_sphere: false, + } + text: { draw: true, height: 1.25, scale: 1.0 } edges: scale: 0.1 alpha: 0.2 - use_color: false + color: { type: UniformEdgeColorAdapter } interlayer_use_source: true interlayer_scale: 0.08 interlayer_alpha: 0.4 - interlayer_use_color: true interlayer_insertion_skip: 0 partitions: 2: z_offset_scale: 0.0 visualize: true - nodes: {scale: 0.15, alpha: 0.9, use_sphere: false, color: {type: PartitionColorAdapter}} - edges: {scale: 0.05, alpha: 0.9, draw_interlayer: false} - text: {draw_layer: true, height: 0.9, scale: 0.8} + nodes: { scale: 0.15, alpha: 0.9, use_sphere: false, color: { type: PartitionColorAdapter } } + edges: { scale: 0.05, alpha: 0.9, draw_interlayer: false } + text: { draw_layer: true, height: 0.9, scale: 0.8 } 3: z_offset_scale: 3.0 visualize: true - nodes: {scale: 0.2, color: {type: LabelColorAdapter}, alpha: 0.9, use_sphere: true} - boundaries: {draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0} + nodes: { scale: 0.2, color: { type: LabelColorAdapter }, alpha: 0.9, use_sphere: true } + boundaries: { draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0 } edges: scale: 0.01 alpha: 0.5 - use_color: false + color: { type: UniformEdgeColorAdapter } interlayer_use_source: false interlayer_scale: 0.08 interlayer_alpha: 0.4 - interlayer_use_color: true interlayer_insertion_skip: 0 diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h new file mode 100644 index 0000000..f08a1eb --- /dev/null +++ b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h @@ -0,0 +1,131 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include +#include +#include + +#include + +#include "hydra_visualizer/color/colormap_utilities.h" + +namespace hydra { + +#define REGISTER_COLOR_ADAPTER(adapter) \ + inline static const auto registration_ = \ + config::RegistrationWithConfig(#adapter) + +struct EdgeColorAdapter { + using Ptr = std::shared_ptr; + using EdgeColor = std::pair; + + virtual ~EdgeColorAdapter() = default; + + /** + * @brief Get color for an edge. + * @param graph Current scene graph node is from + * @param edge Edge to get color for + * @returns Visualizer color for source and target ends of the edge. + */ + virtual EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph, + const spark_dsg::SceneGraphEdge& edge) const = 0; + + /** + * @brief Set any pre-draw information + * @param graph Graph to get information for + * + * Allows color adapters to gather statistics about the scene graph before generating + * any edge colors when drawing the scene graph + */ + virtual void setGraph(const spark_dsg::DynamicSceneGraph& /* graph */, + spark_dsg::LayerId /* layer */) {} +}; + +struct UniformEdgeColorAdapter : EdgeColorAdapter { + struct Config { + // TODO(lschmid): Consider using the named colors, or even better integrate + // optionally named colors directly into the config utilities parsing. + spark_dsg::Color color; + } const config; + + explicit UniformEdgeColorAdapter(const Config& config); + EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph, + const spark_dsg::SceneGraphEdge& edge) const override; + + private: + REGISTER_COLOR_ADAPTER(UniformEdgeColorAdapter); +}; + +void declare_config(UniformEdgeColorAdapter::Config& config); + +struct EdgeValueFunctor { + virtual ~EdgeValueFunctor() = default; + virtual double eval(const spark_dsg::DynamicSceneGraph& graph, + const spark_dsg::SceneGraphEdge& edge) const = 0; +}; + +struct EdgeWeightFunctor : EdgeValueFunctor { + double eval(const spark_dsg::DynamicSceneGraph& graph, + const spark_dsg::SceneGraphEdge& edge) const override; + + inline static const auto registration = + config::Registration("weight"); +}; + +struct ValueEdgeColorAdapter : EdgeColorAdapter { + struct Config { + visualizer::RangeColormap::Config colormap; + std::string value_functor{"weight"}; + } const config; + + explicit ValueEdgeColorAdapter(const Config& config); + void setGraph(const spark_dsg::DynamicSceneGraph& graph, + spark_dsg::LayerId layer) override; + EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph, + const spark_dsg::SceneGraphEdge& edge) const override; + + private: + double min_value_; + double max_value_; + std::unique_ptr functor_; + const visualizer::RangeColormap colormap_; + REGISTER_COLOR_ADAPTER(ValueEdgeColorAdapter); +}; + +void declare_config(ValueEdgeColorAdapter::Config& config); + +#undef REGISTER_COLOR_ADAPTER + +} // namespace hydra diff --git a/hydra_visualizer/include/hydra_visualizer/layer_info.h b/hydra_visualizer/include/hydra_visualizer/layer_info.h index 40f3a84..ddf70a3 100644 --- a/hydra_visualizer/include/hydra_visualizer/layer_info.h +++ b/hydra_visualizer/include/hydra_visualizer/layer_info.h @@ -37,6 +37,7 @@ #include #include +#include "hydra_visualizer/adapters/edge_color.h" #include "hydra_visualizer/adapters/graph_color.h" #include "hydra_visualizer/adapters/text.h" @@ -74,10 +75,9 @@ struct LayerConfig { double scale = 0.03; //[ 0.001, 1.0] //! @brief intralayer edge alpha double alpha = 1.0; //[ 0.0, 1.0] - //! @brief Color to use for edge - NamedColors color = NamedColors::BLACK; - //! @brief show intralayer edge using node colors - bool use_color = true; + //! @brief Color to use for edge. Unspecified uses node colors. + config::VirtualConfig color{ + UniformEdgeColorAdapter::Config()}; //! @brief draw interlayer edges bool draw_interlayer = true; //! @brief use edge source layer for config @@ -86,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 using node colors + //! @brief If true color dsg-mesh edges bool interlayer_use_color = true; //! @brief Number of edges to skip when drawing interlayer edges size_t interlayer_insertion_skip = 0; // [0, 1000] @@ -155,6 +155,8 @@ class LayerInfo { using FilterFunction = std::function; using ColorFunction = std::function; + using EdgeColorFunction = std::function( + const spark_dsg::SceneGraphEdge&)>; using TextFunction = std::function; LayerInfo(const LayerConfig config); @@ -163,17 +165,18 @@ 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; double z_offset; ColorFunction node_color; + EdgeColorFunction edge_color; TextFunction node_text; mutable FilterFunction filter; private: - std::unique_ptr color_adapter_; + std::unique_ptr node_color_adapter_; + std::unique_ptr edge_color_adapter_; std::unique_ptr text_adapter_; }; diff --git a/hydra_visualizer/src/adapters/edge_color.cpp b/hydra_visualizer/src/adapters/edge_color.cpp new file mode 100644 index 0000000..631fcf7 --- /dev/null +++ b/hydra_visualizer/src/adapters/edge_color.cpp @@ -0,0 +1,118 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "hydra_visualizer/adapters/edge_color.h" + +#include +#include +#include +#include +#include + +#include "hydra_visualizer/color/color_parsing.h" + +namespace hydra { + +using namespace spark_dsg; +using EdgeColor = EdgeColorAdapter::EdgeColor; + +void declare_config(UniformEdgeColorAdapter::Config& config) { + using namespace config; + name("UniformEdgeColorAdapter::Config"); + field(config.color, "color"); +} + +UniformEdgeColorAdapter::UniformEdgeColorAdapter(const Config& config) + : config(config) {} + +EdgeColor UniformEdgeColorAdapter::getColor(const DynamicSceneGraph&, + const SceneGraphEdge&) const { + return {config.color, config.color}; +} + +double EdgeWeightFunctor::eval(const DynamicSceneGraph&, + const SceneGraphEdge& edge) const { + return edge.attributes().weight; +} + +ValueEdgeColorAdapter::ValueEdgeColorAdapter(const Config& config) + : config(config), + min_value_(0.0), + max_value_(1.0), + functor_(config::create(config.value_functor)), + colormap_(config.colormap) {} + +void ValueEdgeColorAdapter::setGraph(const DynamicSceneGraph& graph, LayerId layer) { + if (!functor_) { + return; + } + + bool is_first = true; + try { + for (const auto& [key, edge] : graph.getLayer(layer).edges()) { + const auto value = functor_->eval(graph, edge); + if (is_first) { + min_value_ = value; + max_value_ = value; + is_first = false; + } else { + min_value_ = std::min(value, min_value_); + max_value_ = std::max(value, max_value_); + } + } + } catch (const std::exception& e) { + LOG_FIRST_N(ERROR, 1) << "Value functor unable to evaluate: " << e.what(); + } +} + +EdgeColor ValueEdgeColorAdapter::getColor(const DynamicSceneGraph& graph, + const SceneGraphEdge& edge) const { + try { + const auto color = + colormap_.getColor(functor_->eval(graph, edge), min_value_, max_value_); + return {color, color}; + } catch (const std::exception& e) { + LOG_FIRST_N(ERROR, 1) << "Value functor unable to evaluate: " << e.what(); + return {Color(), Color()}; + } +} + +void declare_config(ValueEdgeColorAdapter::Config& config) { + using namespace config; + name("ValueEdgeColorAdapter::Config"); + field(config.colormap, "colormap"); + field(config.value_functor, "value_functor"); +} + +} // namespace hydra diff --git a/hydra_visualizer/src/adapters/graph_color.cpp b/hydra_visualizer/src/adapters/graph_color.cpp index a85bc3b..0aeed14 100644 --- a/hydra_visualizer/src/adapters/graph_color.cpp +++ b/hydra_visualizer/src/adapters/graph_color.cpp @@ -234,9 +234,7 @@ ValueColorAdapter::ValueColorAdapter(const Config& config) min_value_(0.0), max_value_(1.0), functor_(config::create(config.value_functor)), - colormap_(config.colormap) { - CHECK(functor_) << "invalid functor type: " << config.value_functor; -} + colormap_(config.colormap) {} void ValueColorAdapter::setGraph(const DynamicSceneGraph& graph, LayerId layer) { if (!functor_) { diff --git a/hydra_visualizer/src/drawing.cpp b/hydra_visualizer/src/drawing.cpp index c0bd0cf..7e9f21e 100644 --- a/hydra_visualizer/src/drawing.cpp +++ b/hydra_visualizer/src/drawing.cpp @@ -526,12 +526,9 @@ Marker makeLayerEdgeMarkers(const std_msgs::msg::Header& header, target.z += info.z_offset; marker.points.push_back(target); - const auto c_source = - 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) : 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)); + const auto [color_source, color_target] = info.edge_color(edge); + marker.colors.push_back(makeColorMsg(color_source, info.config.edges.alpha)); + marker.colors.push_back(makeColorMsg(color_target, info.config.edges.alpha)); } return marker; diff --git a/hydra_visualizer/src/layer_info.cpp b/hydra_visualizer/src/layer_info.cpp index b469fb9..be11de6 100644 --- a/hydra_visualizer/src/layer_info.cpp +++ b/hydra_visualizer/src/layer_info.cpp @@ -72,8 +72,7 @@ void declare_config(LayerConfig::Edges& config) { field(config.draw, "draw"); field(config.scale, "scale"); field(config.alpha, "alpha"); - enum_field(config.color, "color"); - field(config.use_color, "use_color"); + field(config.color, "color"); field(config.draw_interlayer, "draw_interlayer"); field(config.interlayer_use_source, "interlayer_use_source"); field(config.interlayer_scale, "interlayer_scale"); @@ -147,11 +146,27 @@ LayerInfo& LayerInfo::offset(double offset_size, bool collapse) { } LayerInfo& LayerInfo::graph(const DynamicSceneGraph& graph, LayerId layer) { - color_adapter_ = config.nodes.color.create(); - if (color_adapter_) { - color_adapter_->setGraph(graph, layer); + node_color_adapter_ = config.nodes.color.create(); + if (node_color_adapter_) { + node_color_adapter_->setGraph(graph, layer); node_color = [this, &graph](const SceneGraphNode& node) { - return color_adapter_->getColor(graph, node); + return node_color_adapter_->getColor(graph, node); + }; + } + + edge_color_adapter_ = config.edges.color.create(); + if (edge_color_adapter_) { + edge_color_adapter_->setGraph(graph, layer); + edge_color = [this, &graph](const SceneGraphEdge& edge) { + return edge_color_adapter_->getColor(graph, edge); + }; + } else { + // If not specified, use node colors. + // TODO(lschmid): This is not the prettiest, but e.g. handing the layerinfo to the + // adapters or so also didn't seem right. + edge_color = [this, &graph](const SceneGraphEdge& edge) { + return std::make_pair(node_color(graph.getNode(edge.source)), + node_color(graph.getNode(edge.target))); }; } @@ -167,8 +182,6 @@ LayerInfo& LayerInfo::graph(const DynamicSceneGraph& graph, LayerId layer) { 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;