diff --git a/hydra_ros/include/hydra_ros/visualizer/feature_color_adaptors.h b/hydra_ros/include/hydra_ros/visualizer/feature_color_adaptors.h index 4a346b4..947bf90 100644 --- a/hydra_ros/include/hydra_ros/visualizer/feature_color_adaptors.h +++ b/hydra_ros/include/hydra_ros/visualizer/feature_color_adaptors.h @@ -1,6 +1,6 @@ #pragma once #include -#include +#include #include #include @@ -10,7 +10,7 @@ namespace hydra { -class FeatureScoreColor : public GraphColorAdapter { +class FeatureScoreColor : public NodeColorAdapter { public: struct Config { std::string ns = "~"; @@ -23,7 +23,7 @@ class FeatureScoreColor : public GraphColorAdapter { explicit FeatureScoreColor(const Config& config); ~FeatureScoreColor(); void setGraph(const spark_dsg::DynamicSceneGraph& graph, - spark_dsg::LayerId layer) override; + spark_dsg::LayerKey layer) override; spark_dsg::Color getColor(const spark_dsg::DynamicSceneGraph& graph, const spark_dsg::SceneGraphNode& node) const override; @@ -46,15 +46,11 @@ class FeatureScoreColor : public GraphColorAdapter { Eigen::VectorXf feature_; std::unordered_map values_; std::unique_ptr metric_; - - inline static const auto registration_ = - config::RegistrationWithConfig( - "FeatureScoreColor"); }; void declare_config(FeatureScoreColor::Config& config); -class NearestFeatureColor : public GraphColorAdapter { +class NearestFeatureColor : public NodeColorAdapter { public: struct Config { config::VirtualConfig metric{CosineDistance::Config()}; @@ -70,15 +66,11 @@ class NearestFeatureColor : public GraphColorAdapter { std::unique_ptr metric_; std::unique_ptr features_; const visualizer::DiscreteColormap colormap_; - - inline static const auto registration_ = - config::RegistrationWithConfig( - "NearestFeatureColor"); }; void declare_config(NearestFeatureColor::Config& config); -class NearestFeatureLabel : public visualizer::GraphTextAdapter { +class NearestFeatureLabel : public visualizer::NodeTextAdapter { public: struct Config { config::VirtualConfig metric{CosineDistance::Config()}; @@ -94,10 +86,6 @@ class NearestFeatureLabel : public visualizer::GraphTextAdapter { private: std::unique_ptr metric_; std::unique_ptr features_; - - inline static const auto registration_ = - config::RegistrationWithConfig( - "NearestFeatureLabel"); }; } // namespace hydra diff --git a/hydra_ros/src/visualizer/feature_color_adaptors.cpp b/hydra_ros/src/visualizer/feature_color_adaptors.cpp index 5730efe..8ce3c0d 100644 --- a/hydra_ros/src/visualizer/feature_color_adaptors.cpp +++ b/hydra_ros/src/visualizer/feature_color_adaptors.cpp @@ -12,6 +12,24 @@ #include namespace hydra { +namespace { + +static const auto score_registration = + config::RegistrationWithConfig("FeatureScoreColor"); + +static const auto nearest_registration = + config::RegistrationWithConfig("NearestFeatureColor"); + +static const auto text_registration = + config::RegistrationWithConfig("NearestFeatureLabel"); + +} // namespace using namespace spark_dsg; using semantic_inference_msgs::msg::FeatureVectorStamped; @@ -103,13 +121,13 @@ FeatureScoreColor::FeatureScoreColor(const Config& config) FeatureScoreColor::~FeatureScoreColor() = default; -void FeatureScoreColor::setGraph(const DynamicSceneGraph& graph, LayerId layer_id) { +void FeatureScoreColor::setGraph(const DynamicSceneGraph& graph, LayerKey layer_key) { values_.clear(); if (!has_feature_ || !metric_) { return; } - const auto& layer = graph.getLayer(layer_id); + const auto& layer = graph.getLayer(layer_key.layer, layer_key.partition); range_.min = 1.0f; range_.max = 0.0f; for (const auto& [node_id, node] : layer.nodes()) { diff --git a/hydra_visualizer/CMakeLists.txt b/hydra_visualizer/CMakeLists.txt index b11c936..c200f82 100644 --- a/hydra_visualizer/CMakeLists.txt +++ b/hydra_visualizer/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.22.1) project(hydra_visualizer) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) @@ -31,8 +31,8 @@ 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/node_color.cpp src/adapters/text.cpp src/color/colormap_utilities.cpp src/drawing.cpp @@ -50,6 +50,7 @@ add_library( src/plugins/pose_plugin.cpp src/scene_graph_renderer.cpp src/utils/ear_clipping.cpp + src/utils/layer_key_selector.cpp src/utils/marker_group_pub.cpp src/utils/marker_tracker.cpp src/utils/polygon_utilities.cpp @@ -89,7 +90,11 @@ target_link_libraries( if(BUILD_TESTING) find_package(ament_cmake_gtest) - ament_add_gtest(test_${PROJECT_NAME} tests/main.cpp tests/test_ear_clipping.cpp) + ament_add_gtest(test_${PROJECT_NAME} + tests/main.cpp + tests/test_ear_clipping.cpp + tests/test_layer_key_selector.cpp + ) target_link_libraries( test_${PROJECT_NAME} ${PROJECT_NAME} diff --git a/hydra_visualizer/app/hydra_visualizer_node.cpp b/hydra_visualizer/app/hydra_visualizer_node.cpp index 10ccae3..4f77ba9 100644 --- a/hydra_visualizer/app/hydra_visualizer_node.cpp +++ b/hydra_visualizer/app/hydra_visualizer_node.cpp @@ -40,8 +40,6 @@ #include #include -#include - #include #include "hydra_visualizer/visualizer_node.h" diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index bedeff7..eb98b84 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/hydra_visualizer/config/visualizer_config.yaml @@ -9,50 +9,37 @@ renderer: 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.5} 3: z_offset_scale: 3.0 visualize: true nodes: {scale: 0.2, color: {type: ParentColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.9, use_sphere: true} - edges: - scale: 0.01 - alpha: 0.5 - color: {type: UniformEdgeColorAdapter} - interlayer_use_source: false - interlayer_scale: 0.08 - interlayer_alpha: 0.4 - interlayer_insertion_skip: 0 + edges: {scale: 0.01, alpha: 0.5, color: {type: UniformEdgeColorAdapter}} 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} - edges: - scale: 0.1 - alpha: 0.2 - color: {type: UniformEdgeColorAdapter} - interlayer_use_source: true - interlayer_scale: 0.08 - interlayer_alpha: 0.4 - interlayer_insertion_skip: 0 - partitions: - 2: + edges: {scale: 0.1, alpha: 0.2, color: {type: UniformEdgeColorAdapter}} + 2p*: 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} - 3: + 3p1: + 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} + edges: {scale: 0.01, alpha: 0.5, color: {type: UniformEdgeColorAdapter}} + 3p2: 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} - text: {draw: true, collapse: true, adapter: {type: IdTextAdapter}, height: 0.5, scale: 0.45} - edges: - scale: 0.1 - alpha: 0.8 - color: {type: TraversabilityEdgeColorAdapter} - interlayer_use_source: false - interlayer_scale: 0.08 - interlayer_alpha: 0.4 - interlayer_insertion_skip: 0 + text: {draw: false, collapse: true, adapter: {type: IdTextAdapter}, height: 0.5, scale: 0.45} + edges: {scale: 0.1, alpha: 0.8, color: {type: TraversabilityEdgeColorAdapter}} + interlayer_edges: + - {from: 3*, to: 2p*, draw: false} + - {from: 3*, to: 2, use_child_color: true, scale: 0.08, alpha: 0.5} + - {from: 4, to: 3*, scale: 0.08, alpha: 0.4} diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h index ea5cd32..f8d0426 100644 --- a/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h +++ b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h @@ -70,7 +70,7 @@ struct EdgeColorAdapter { * any edge colors when drawing the scene graph */ virtual void setGraph(const spark_dsg::DynamicSceneGraph& /* graph */, - spark_dsg::LayerId /* layer */) {} + spark_dsg::LayerKey /* layer */) {} }; struct UniformEdgeColorAdapter : EdgeColorAdapter { @@ -112,7 +112,7 @@ struct ValueEdgeColorAdapter : EdgeColorAdapter { explicit ValueEdgeColorAdapter(const Config& config); void setGraph(const spark_dsg::DynamicSceneGraph& graph, - spark_dsg::LayerId layer) override; + spark_dsg::LayerKey layer) override; EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph, const spark_dsg::SceneGraphEdge& edge) const override; @@ -137,7 +137,7 @@ struct TraversabilityEdgeColorAdapter : EdgeColorAdapter { explicit TraversabilityEdgeColorAdapter(const Config& config); void setGraph(const spark_dsg::DynamicSceneGraph& graph, - spark_dsg::LayerId layer) override; + spark_dsg::LayerKey layer) override; EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph, const spark_dsg::SceneGraphEdge& edge) const override; diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/graph_color.h b/hydra_visualizer/include/hydra_visualizer/adapters/node_color.h similarity index 89% rename from hydra_visualizer/include/hydra_visualizer/adapters/graph_color.h rename to hydra_visualizer/include/hydra_visualizer/adapters/node_color.h index 57e19be..40b837a 100644 --- a/hydra_visualizer/include/hydra_visualizer/adapters/graph_color.h +++ b/hydra_visualizer/include/hydra_visualizer/adapters/node_color.h @@ -45,12 +45,12 @@ namespace hydra { #define REGISTER_COLOR_ADAPTER(adapter) \ inline static const auto registration_ = \ - config::RegistrationWithConfig(#adapter) + config::RegistrationWithConfig(#adapter) -struct GraphColorAdapter { - using Ptr = std::shared_ptr; +struct NodeColorAdapter { + using Ptr = std::shared_ptr; - virtual ~GraphColorAdapter() = default; + virtual ~NodeColorAdapter() = default; /** * @brief Get color for a single node @@ -64,30 +64,31 @@ struct GraphColorAdapter { /** * @brief Set any pre-draw information * @param graph Graph to get information for + * @param layer Layer to get information for * * Allows color adapters to gather statistics about the scene graph before generating * any node colors when drawing the scene graph */ virtual void setGraph(const spark_dsg::DynamicSceneGraph& /* graph */, - spark_dsg::LayerId /* layer */) {} + spark_dsg::LayerKey /* layer */) {} }; -struct NodeColorAdapter : GraphColorAdapter { +struct AttributeColorAdapter : NodeColorAdapter { struct Config { spark_dsg::Color default_color; } const config; - explicit NodeColorAdapter(const Config& config); + explicit AttributeColorAdapter(const Config& config); spark_dsg::Color getColor(const spark_dsg::DynamicSceneGraph& graph, const spark_dsg::SceneGraphNode& node) const override; private: - REGISTER_COLOR_ADAPTER(NodeColorAdapter); + REGISTER_COLOR_ADAPTER(AttributeColorAdapter); }; -void declare_config(NodeColorAdapter::Config& config); +void declare_config(AttributeColorAdapter::Config& config); -struct UniformColorAdapter : GraphColorAdapter { +struct UniformColorAdapter : NodeColorAdapter { struct Config { spark_dsg::Color color; } const config; @@ -102,7 +103,7 @@ struct UniformColorAdapter : GraphColorAdapter { void declare_config(UniformColorAdapter::Config& config); -struct LabelColorAdapter : GraphColorAdapter { +struct LabelColorAdapter : NodeColorAdapter { struct Config { visualizer::CategoricalColormap::Config colormap; } const config; @@ -119,7 +120,7 @@ struct LabelColorAdapter : GraphColorAdapter { void declare_config(LabelColorAdapter::Config& config); -struct IdColorAdapter : GraphColorAdapter { +struct IdColorAdapter : NodeColorAdapter { struct Config { visualizer::DiscreteColormap::Config colormap; } const config; @@ -136,10 +137,10 @@ struct IdColorAdapter : GraphColorAdapter { void declare_config(IdColorAdapter::Config& config); -struct ParentColorAdapter : GraphColorAdapter { +struct ParentColorAdapter : NodeColorAdapter { struct Config { spark_dsg::Color default_color; - config::VirtualConfig parent_adapter{ + config::VirtualConfig parent_adapter{ IdColorAdapter::Config{visualizer::DiscretePalette::COLORBREWER}}; } const config; @@ -148,7 +149,7 @@ struct ParentColorAdapter : GraphColorAdapter { const spark_dsg::SceneGraphNode& node) const override; private: - const GraphColorAdapter::Ptr parent_adapter_; + const NodeColorAdapter::Ptr parent_adapter_; REGISTER_COLOR_ADAPTER(ParentColorAdapter); }; @@ -175,7 +176,7 @@ struct HasActiveMeshFunctor : StatusFunctor { config::Registration("has_active_mesh"); }; -struct StatusColorAdapter : GraphColorAdapter { +struct StatusColorAdapter : NodeColorAdapter { struct Config { spark_dsg::Color true_color{0, 255, 0}; spark_dsg::Color false_color; @@ -193,7 +194,7 @@ struct StatusColorAdapter : GraphColorAdapter { void declare_config(StatusColorAdapter::Config& config); -struct FrontierColorAdapter : GraphColorAdapter { +struct FrontierColorAdapter : NodeColorAdapter { struct Config { spark_dsg::Color real; spark_dsg::Color predicted{0, 0, 255}; @@ -212,7 +213,7 @@ struct FrontierColorAdapter : GraphColorAdapter { void declare_config(FrontierColorAdapter::Config& config); -struct PartitionColorAdapter : GraphColorAdapter { +struct PartitionColorAdapter : NodeColorAdapter { struct Config { visualizer::DiscreteColormap::Config colormap; } const config; @@ -251,7 +252,7 @@ struct LastUpdatedFunctor : ValueFunctor { config::Registration("last_updated"); }; -struct ValueColorAdapter : GraphColorAdapter { +struct ValueColorAdapter : NodeColorAdapter { struct Config { visualizer::RangeColormap::Config colormap; std::string value_functor{"place_distance"}; @@ -259,7 +260,7 @@ struct ValueColorAdapter : GraphColorAdapter { explicit ValueColorAdapter(const Config& config); void setGraph(const spark_dsg::DynamicSceneGraph& graph, - spark_dsg::LayerId layer) override; + spark_dsg::LayerKey layer) override; spark_dsg::Color getColor(const spark_dsg::DynamicSceneGraph& graph, const spark_dsg::SceneGraphNode& node) const override; @@ -273,7 +274,7 @@ struct ValueColorAdapter : GraphColorAdapter { void declare_config(ValueColorAdapter::Config& config); -struct LabelDistributionAdapter : GraphColorAdapter { +struct LabelDistributionAdapter : NodeColorAdapter { struct Config { visualizer::CategoricalColormap::Config colormap; } const config; diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/text.h b/hydra_visualizer/include/hydra_visualizer/adapters/text.h index 4fb10a3..4b13c75 100644 --- a/hydra_visualizer/include/hydra_visualizer/adapters/text.h +++ b/hydra_visualizer/include/hydra_visualizer/adapters/text.h @@ -38,14 +38,14 @@ namespace hydra::visualizer { -struct GraphTextAdapter { - using Ptr = std::shared_ptr; - virtual ~GraphTextAdapter() = default; +struct NodeTextAdapter { + using Ptr = std::shared_ptr; + virtual ~NodeTextAdapter() = default; virtual std::string getText(const spark_dsg::DynamicSceneGraph& graph, const spark_dsg::SceneGraphNode& node) const = 0; }; -struct IdTextAdapter : GraphTextAdapter { +struct IdTextAdapter : NodeTextAdapter { struct Config {}; explicit IdTextAdapter(const Config&) {} virtual ~IdTextAdapter() = default; @@ -55,7 +55,7 @@ struct IdTextAdapter : GraphTextAdapter { void declare_config(IdTextAdapter::Config& config); -struct LabelTextAdapter : GraphTextAdapter { +struct LabelTextAdapter : NodeTextAdapter { struct Config {}; explicit LabelTextAdapter(const Config&) {} virtual ~LabelTextAdapter() = default; @@ -67,7 +67,7 @@ struct LabelTextAdapter : GraphTextAdapter { void declare_config(LabelTextAdapter::Config& config); -struct LabelIdTextAdapter : GraphTextAdapter { +struct LabelIdTextAdapter : NodeTextAdapter { struct Config {}; explicit LabelIdTextAdapter(const Config&) {} virtual ~LabelIdTextAdapter() = default; @@ -79,7 +79,7 @@ struct LabelIdTextAdapter : GraphTextAdapter { void declare_config(LabelIdTextAdapter::Config& config); -struct NameTextAdapter : GraphTextAdapter { +struct NameTextAdapter : NodeTextAdapter { struct Config {}; explicit NameTextAdapter(const Config&) {} virtual ~NameTextAdapter() = default; @@ -89,7 +89,7 @@ struct NameTextAdapter : GraphTextAdapter { void declare_config(NameTextAdapter::Config& config); -struct NameIdTextAdapter : GraphTextAdapter { +struct NameIdTextAdapter : NodeTextAdapter { struct Config {}; explicit NameIdTextAdapter(const Config&) {} virtual ~NameIdTextAdapter() = default; @@ -99,7 +99,7 @@ struct NameIdTextAdapter : GraphTextAdapter { void declare_config(NameIdTextAdapter::Config& config); -struct AttributesTextAdaptor : GraphTextAdapter { +struct AttributesTextAdaptor : NodeTextAdapter { struct Config {}; explicit AttributesTextAdaptor(const Config&) {} virtual ~AttributesTextAdaptor() = default; diff --git a/hydra_visualizer/include/hydra_visualizer/drawing.h b/hydra_visualizer/include/hydra_visualizer/drawing.h index 97ec76a..74e0b87 100644 --- a/hydra_visualizer/include/hydra_visualizer/drawing.h +++ b/hydra_visualizer/include/hydra_visualizer/drawing.h @@ -96,11 +96,7 @@ MarkerMsg makeLayerEdgeMarkers(const std_msgs::msg::Header& header, const SceneGraphLayer& layer, const std::string& ns); -MarkerMsg makeMeshEdgesMarker(const std_msgs::msg::Header& header, - const LayerInfo& info, - const SceneGraphLayer& layer, - const spark_dsg::Mesh& mesh, - const std::string& ns); +// TODO(nathan) add ability to draw mesh points for given 2D place or object MarkerMsg makeLayerTextMarker(const std_msgs::msg::Header& header, const LayerInfo& info, diff --git a/hydra_visualizer/include/hydra_visualizer/layer_info.h b/hydra_visualizer/include/hydra_visualizer/layer_info.h index ddf70a3..eafe65c 100644 --- a/hydra_visualizer/include/hydra_visualizer/layer_info.h +++ b/hydra_visualizer/include/hydra_visualizer/layer_info.h @@ -38,7 +38,7 @@ #include #include "hydra_visualizer/adapters/edge_color.h" -#include "hydra_visualizer/adapters/graph_color.h" +#include "hydra_visualizer/adapters/node_color.h" #include "hydra_visualizer/adapters/text.h" namespace hydra::visualizer { @@ -47,11 +47,9 @@ struct LayerConfig { //! @brief show layer bool visualize = false; //! @brief number of steps of offset to apply - double z_offset_scale = 0.0; // [-5.0, 10.0] + double z_offset_scale = 0.0; //! @brief Draw current frontiers as ellipses bool draw_frontier_ellipse = false; - //! @brief whether or not to draw mesh edges - bool draw_mesh_edges = false; //! @brief Node settings struct Nodes { @@ -60,7 +58,7 @@ struct LayerConfig { //! @brief size of the centroid marker double scale = 0.1; //! @brief Color adapter - config::VirtualConfig color{NodeColorAdapter::Config()}; + config::VirtualConfig color{AttributeColorAdapter::Config()}; //! @brief alpha of the centroid marker double alpha = 1.0; //! @brief use sphere markers (instead of cubes) @@ -69,27 +67,16 @@ struct LayerConfig { //! @brief Edge configuration struct Edges { - //! @brief draw intralayer edges + //! @brief Draw edges bool draw = true; - //! @brief intralayer edge size - 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. Unspecified uses node colors. - config::VirtualConfig color{ - UniformEdgeColorAdapter::Config()}; - //! @brief draw interlayer edges - bool draw_interlayer = true; - //! @brief use edge source layer for config - bool interlayer_use_source = true; - //! @brief interlayer edge size - double interlayer_scale = 0.03; // [0.001, 1.0] - //! @brief interlayer edge alpha - double interlayer_alpha = 1.0; // [0.0, 1.0] - //! @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] + //! @brief Edge size + double scale = 0.03; + //! @brief Edge alpha + double alpha = 1.0; + //! @brief Color to use for edge (unspecified uses node colors). + config::VirtualConfig color{UniformEdgeColorAdapter::Config()}; + //! @brief Number of edges to skip when drawing edges + size_t insertion_skip = 0; } edges; //! @brief Text settings @@ -101,15 +88,15 @@ struct LayerConfig { //! @brief draw text without z offset bool collapse = false; //! @brief text adapter type - config::VirtualConfig adapter{IdTextAdapter::Config()}; + config::VirtualConfig adapter{IdTextAdapter::Config()}; //! @brief height of text above node - double height = 1.0; //[ 0.0, 5.0] + double height = 1.0; //! @brief scale of text above node - double scale = 0.5; //[ 0.05, 5.0] + double scale = 0.5; //! @brief add random noise to text z offset bool add_jitter = false; //! @brief amount of jitter to add - double jitter_scale = 0.2; //[ 0.05, 5.0] + double jitter_scale = 0.2; //! @brief Color to use for text NamedColors color = NamedColors::BLACK; } text; @@ -120,13 +107,13 @@ struct LayerConfig { //! @brief draw bounding box at ground level bool collapse = false; //! @brief scale of bounding box wireframe - double scale = 0.1; // [0.001, 1.0] + double scale = 0.1; //! @brief scale of edges drawn to bbox corners - double edge_scale = 0.01; // [ 0.001, 1.0] + double edge_scale = 0.01; //! @brief alpha of bounding boxes - double alpha = 0.5; // [0.0, 1.0] + double alpha = 0.5; //! @brief point at which to break the edge into many edges - double edge_break_ratio = 0.5; //[0.0, 1.0] + double edge_break_ratio = 0.5; } bounding_boxes; //! @brief configuration for polygon boundaries @@ -136,35 +123,42 @@ struct LayerConfig { //! @brief draw polygons at mesh level bool collapse = false; //! @brief scale of boundary wireframe - double wireframe_scale = 0.1; //[ 0.001, 1.0] + double wireframe_scale = 0.1; //! @brief draw polygons using node semantic color bool use_node_color = true; //! @brief alpha of boundary - double alpha = 0.5; //[0.0, 1.0] + double alpha = 0.5; //! @brief display minimum bounding ellipse bool draw_ellipse = false; //! @brief alpha of bounding ellipse - double ellipse_alpha = 0.5; //[ 0.0, 1.0] + double ellipse_alpha = 0.5; } boundaries; }; +void declare_config(LayerConfig::Nodes& config); +void declare_config(LayerConfig::Edges& config); +void declare_config(LayerConfig::Text& config); +void declare_config(LayerConfig::BoundingBoxes& config); +void declare_config(LayerConfig::Boundaries& config); void declare_config(LayerConfig& config); class LayerInfo { public: - using FilterFunction = std::function; - using ColorFunction = - std::function; - using EdgeColorFunction = std::function( - const spark_dsg::SceneGraphEdge&)>; - using TextFunction = std::function; - - LayerInfo(const LayerConfig config); + using Color = spark_dsg::Color; + using Node = spark_dsg::SceneGraphNode; + using Edge = spark_dsg::SceneGraphEdge; + using FilterFunction = std::function; + using ColorFunction = std::function; + using EdgeColorFunction = std::function(const Edge&)>; + using TextFunction = std::function; + + LayerInfo(const LayerConfig& config); LayerInfo& offset(double offset_size = 1.0, bool collapse = true); - LayerInfo& graph(const spark_dsg::DynamicSceneGraph& graph, spark_dsg::LayerId layer); + LayerInfo& graph(const spark_dsg::DynamicSceneGraph& graph, + spark_dsg::LayerKey layer); - bool shouldVisualize(const spark_dsg::SceneGraphNode& node) const; - spark_dsg::Color text_color() const; + bool shouldVisualize(const Node& node) const; + Color text_color() const; const LayerConfig config; @@ -175,9 +169,9 @@ class LayerInfo { mutable FilterFunction filter; private: - std::unique_ptr node_color_adapter_; + std::unique_ptr node_color_adapter_; std::unique_ptr edge_color_adapter_; - std::unique_ptr text_adapter_; + std::unique_ptr text_adapter_; }; } // namespace hydra::visualizer diff --git a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h index 9bf458c..c257ca6 100644 --- a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h +++ b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h @@ -37,12 +37,10 @@ #include #include -#include -#include - #include #include "hydra_visualizer/layer_info.h" +#include "hydra_visualizer/utils/layer_key_selector.h" #include "hydra_visualizer/utils/marker_tracker.h" namespace hydra { @@ -50,25 +48,41 @@ namespace hydra { // NOTE(nathan) separate to make config wrapper easier to use struct GraphRenderConfig { //! @brief Unit amount of distance between layers - double layer_z_step = 5.0; // [0, 50.0] + double layer_z_step = 5.0; //! @brief Whether or not to separate layers by adding z offsets bool collapse_layers = false; }; +struct InterlayerEdgeConfig : visualizer::LayerConfig::Edges { + InterlayerEdgeConfig(); + //! Use the child node to select the edge color instead of the parent + bool use_child_color = false; +}; + void declare_config(GraphRenderConfig& config); +void declare_config(InterlayerEdgeConfig& config); class SceneGraphRenderer { public: using Ptr = std::shared_ptr; using LayerConfigWrapper = config::DynamicConfig; + using EdgeConfigWrapper = config::DynamicConfig; struct Config { //! @brief Overall graph config GraphRenderConfig graph; //! @brief Configuration for each layer - std::map layers; - //! @brief Configuration for non-primary partitions by layer - std::map partitions; + std::map layers; + + struct InterlayerEdges { + LayerKeySelector from; + LayerKeySelector to; + // NOTE(nathan) this is awkward, but we don't want the key selectors to be + // visibile in the dynamic config so we need to split the structs + InterlayerEdgeConfig config; + }; + //! @brief Configuration for interlayer edges + std::vector interlayer_edges; }; explicit SceneGraphRenderer(const Config& config, ianvs::NodeHandle nh); @@ -99,18 +113,23 @@ class SceneGraphRenderer { const visualizer::LayerInfo& getLayerInfo(spark_dsg::LayerKey layer) const; + visualizer::LayerConfig getLayerConfig(spark_dsg::LayerKey key) const; + + InterlayerEdgeConfig getInterlayerEdgeConfig(spark_dsg::LayerKey l1, + spark_dsg::LayerKey l2) const; + protected: + const Config init_config_; + ianvs::NodeHandle nh_; config::DynamicConfig graph_config_; rclcpp::Publisher::SharedPtr pub_; - mutable std::atomic has_change_; - mutable std::map> layers_; - mutable std::map> partitions_; - mutable MarkerTracker tracker_; - mutable std::map layer_infos_; - mutable std::map partition_infos_; + mutable std::atomic has_change_; + mutable std::map layer_infos_; + mutable std::map> layers_; + mutable std::map> interlayer_edges_; }; void declare_config(SceneGraphRenderer::Config& config); diff --git a/hydra_visualizer/include/hydra_visualizer/utils/layer_key_selector.h b/hydra_visualizer/include/hydra_visualizer/utils/layer_key_selector.h new file mode 100644 index 0000000..842f957 --- /dev/null +++ b/hydra_visualizer/include/hydra_visualizer/utils/layer_key_selector.h @@ -0,0 +1,73 @@ +/* ----------------------------------------------------------------------------- + * 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 + +namespace hydra { + +/* + * @brief Type representing a specification for a layer and set of partitions + * + * Can be of the form + * - [LayerId] (e.g., '2'), which selects Layer 2, Partition 0 + * - [LayerId]p[PartitionId] (e.g., '2p1'), which selects Layer 2, Partition 1 + * - [LayerId]p* (e.g., '2p*'), which selects Layer 2, Partitions >= 1 + * - [LayerId]* (e.g., '2*'), which selects Layer 2, Partitions >= 0 + */ +struct LayerKeySelector { + //! Layer and partition the selector references + spark_dsg::LayerKey key; + //! @brief whether or not the selector covers all partitions >= 1 + bool wildcard = false; + //! @brief whether or not wildcard includes default partition (partition 0) + bool include_default = false; + + static std::optional parse(const std::string& selector_str); + + std::string str() const; + bool matches(spark_dsg::LayerKey to_match) const; + bool operator<(const LayerKeySelector& other) const; + bool operator==(const LayerKeySelector& other) const; + bool operator!=(const LayerKeySelector& other) const; +}; + +struct SelectorConversion { + static std::string toIntermediate(const LayerKeySelector& value, std::string& error); + static void fromIntermediate(const std::string& intermediate, + LayerKeySelector& value, + std::string& error); +}; + +} // namespace hydra diff --git a/hydra_visualizer/launch/streaming_visualizer.launch.yaml b/hydra_visualizer/launch/streaming_visualizer.launch.yaml index b4ff588..ca1ea9a 100644 --- a/hydra_visualizer/launch/streaming_visualizer.launch.yaml +++ b/hydra_visualizer/launch/streaming_visualizer.launch.yaml @@ -1,7 +1,7 @@ --- launch: # development args - - arg: {name: verbosity, default: '0'} + - arg: {name: verbosity, default: '1'} - arg: {name: debug, default: 'false'} # visualizer configuration - arg: {name: visualizer_ns, default: hydra_visualizer} @@ -25,7 +25,7 @@ launch: --config-utilities-file $(var visualizer_config_path) --config-utilities-file $(var visualizer_plugins_path) --config-utilities-file $(var external_plugins_path) - --config-utilities-yaml {glog_level: 1, glog_verbosity: $(var verbosity)} + --config-utilities-yaml {glog_level: 0, glog_verbosity: $(var verbosity)} --config-utilities-yaml {graph: {type: $(if $(var use_zmq) GraphFromZmq GraphFromRos), url: $(var zmq_url), frame_id: $(var visualizer_frame)}} # rviz node and control for launching - arg: {name: start_rviz, default: 'true', description: automatically start rviz} diff --git a/hydra_visualizer/src/adapters/edge_color.cpp b/hydra_visualizer/src/adapters/edge_color.cpp index 01e7100..3af252a 100644 --- a/hydra_visualizer/src/adapters/edge_color.cpp +++ b/hydra_visualizer/src/adapters/edge_color.cpp @@ -73,14 +73,16 @@ ValueEdgeColorAdapter::ValueEdgeColorAdapter(const Config& config) functor_(config::create(config.value_functor)), colormap_(config.colormap) {} -void ValueEdgeColorAdapter::setGraph(const DynamicSceneGraph& graph, LayerId layer) { +void ValueEdgeColorAdapter::setGraph(const DynamicSceneGraph& graph, + LayerKey layer_key) { if (!functor_) { return; } bool is_first = true; try { - for (const auto& [key, edge] : graph.getLayer(layer).edges()) { + const auto& layer = graph.getLayer(layer_key.layer, layer_key.partition); + for (const auto& [key, edge] : layer.edges()) { const auto value = functor_->eval(graph, edge); if (is_first) { min_value_ = value; @@ -119,12 +121,14 @@ TraversabilityEdgeColorAdapter::TraversabilityEdgeColorAdapter(const Config& con : config(config), min_value_(0.0), max_value_(1.0), colormap_(config.colormap) {} void TraversabilityEdgeColorAdapter::setGraph(const DynamicSceneGraph& graph, - LayerId layer) { - if (!graph.hasLayer(layer)) { + LayerKey key) { + const auto layer = graph.findLayer(key.layer, key.partition); + if (!layer) { return; } + bool is_first = true; - for (const auto& [key, edge] : graph.getLayer(layer).edges()) { + for (const auto& [key, edge] : layer->edges()) { const auto value = edge.attributes().weight; if (value < 0.0) { continue; diff --git a/hydra_visualizer/src/adapters/graph_color.cpp b/hydra_visualizer/src/adapters/node_color.cpp similarity index 94% rename from hydra_visualizer/src/adapters/graph_color.cpp rename to hydra_visualizer/src/adapters/node_color.cpp index 6773e3a..5bba03c 100644 --- a/hydra_visualizer/src/adapters/graph_color.cpp +++ b/hydra_visualizer/src/adapters/node_color.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_visualizer/adapters/graph_color.h" +#include "hydra_visualizer/adapters/node_color.h" #include #include @@ -46,16 +46,16 @@ namespace hydra { using namespace spark_dsg; -void declare_config(NodeColorAdapter::Config& config) { +void declare_config(AttributeColorAdapter::Config& config) { using namespace config; - name("NodeColorAdapter::Config"); + name("AttributeColorAdapter::Config"); field(config.default_color, "default_color"); } -NodeColorAdapter::NodeColorAdapter(const Config& config) : config(config) {} +AttributeColorAdapter::AttributeColorAdapter(const Config& config) : config(config) {} -Color NodeColorAdapter::getColor(const DynamicSceneGraph&, - const SceneGraphNode& node) const { +Color AttributeColorAdapter::getColor(const DynamicSceneGraph&, + const SceneGraphNode& node) const { try { return node.attributes().color; } catch (const std::bad_cast&) { @@ -231,14 +231,15 @@ ValueColorAdapter::ValueColorAdapter(const Config& config) functor_(config::create(config.value_functor)), colormap_(config.colormap) {} -void ValueColorAdapter::setGraph(const DynamicSceneGraph& graph, LayerId layer) { +void ValueColorAdapter::setGraph(const DynamicSceneGraph& graph, LayerKey layer_key) { if (!functor_) { return; } bool is_first = true; try { - for (const auto& [node_id, node] : graph.getLayer(layer).nodes()) { + const auto& layer = graph.getLayer(layer_key.layer, layer_key.partition); + for (const auto& [node_id, node] : layer.nodes()) { const auto value = functor_->eval(graph, *node); if (is_first) { min_value_ = value; diff --git a/hydra_visualizer/src/adapters/text.cpp b/hydra_visualizer/src/adapters/text.cpp index 5e06e71..f71a5b3 100644 --- a/hydra_visualizer/src/adapters/text.cpp +++ b/hydra_visualizer/src/adapters/text.cpp @@ -49,32 +49,32 @@ using namespace spark_dsg; namespace { static const auto id_reg = - config::RegistrationWithConfig("IdTextAdapter"); static const auto label_reg = - config::RegistrationWithConfig("LabelTextAdapter"); static const auto label_id_reg = - config::RegistrationWithConfig("LabelIdTextAdapter"); static const auto name_reg = - config::RegistrationWithConfig("NameTextAdapter"); static const auto name_id_reg = - config::RegistrationWithConfig("NameIdTextAdapter"); static const auto attributes_reg = - config::RegistrationWithConfig( "AttributesTextAdaptor"); diff --git a/hydra_visualizer/src/drawing.cpp b/hydra_visualizer/src/drawing.cpp index 6176b27..0a8e5dd 100644 --- a/hydra_visualizer/src/drawing.cpp +++ b/hydra_visualizer/src/drawing.cpp @@ -509,6 +509,7 @@ Marker makeLayerEdgeMarkers(const std_msgs::msg::Header& header, return marker; } + size_t num_seen = 0; for (const auto& [key, edge] : layer.edges()) { const auto& source_node = layer.getNode(edge.source); const auto& target_node = layer.getNode(edge.target); @@ -516,6 +517,12 @@ Marker makeLayerEdgeMarkers(const std_msgs::msg::Header& header, continue; } + bool should_skip = num_seen % (info.config.edges.insertion_skip + 1); + ++num_seen; + if (should_skip) { + continue; + } + geometry_msgs::msg::Point source; tf2::convert(source_node.attributes().position, source); source.z += info.z_offset; @@ -534,61 +541,6 @@ Marker makeLayerEdgeMarkers(const std_msgs::msg::Header& header, return marker; } -Marker makeMeshEdgesMarker(const std_msgs::msg::Header& header, - const LayerInfo& info, - const SceneGraphLayer& layer, - const Mesh& mesh, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = ns; - - marker.scale.x = info.config.edges.interlayer_scale; - fillPoseWithIdentity(marker.pose); - - for (const auto& [node_id, node] : layer.nodes()) { - const auto& attrs = node->attributes(); - const auto& mesh_edge_indices = attrs.mesh_connections; - if (mesh_edge_indices.empty()) { - continue; - } - - const auto alpha = info.config.edges.interlayer_alpha; - const auto color = - info.config.edges.interlayer_use_color ? info.node_color(*node) : Color(); - - geometry_msgs::msg::Point centroid_location; - tf2::convert(attrs.position, centroid_location); - centroid_location.z += info.z_offset; - - size_t i = 0; - for (const auto midx : mesh_edge_indices) { - ++i; - if ((i - 1) % (info.config.edges.interlayer_insertion_skip + 1) != 0) { - continue; - } - - if (midx >= mesh.numVertices()) { - continue; - } - - Eigen::Vector3d vertex_pos = mesh.pos(midx).cast(); - geometry_msgs::msg::Point vertex; - tf2::convert(vertex_pos, vertex); - - marker.points.push_back(centroid_location); - marker.points.push_back(vertex); - marker.colors.push_back(makeColorMsg(color, alpha)); - marker.colors.push_back(makeColorMsg(color, alpha)); - } - } - - return marker; -} - // NOTE(nathan) this reuses the normal node text infrastructure, which is mostly fine // because the two are mutually exclusive Marker makeLayerTextMarker(const std_msgs::msg::Header& header, diff --git a/hydra_visualizer/src/layer_info.cpp b/hydra_visualizer/src/layer_info.cpp index be11de6..5a1fac8 100644 --- a/hydra_visualizer/src/layer_info.cpp +++ b/hydra_visualizer/src/layer_info.cpp @@ -51,13 +51,12 @@ inline Color getNodeColor(const SceneGraphNode& node) { } // namespace -// TODO(nathan) validity checks - void declare_config(LayerConfig::Nodes& config) { using namespace config; name("LayerConfig::Nodes"); field(config.draw, "draw"); field(config.scale, "scale"); + config.color.setOptional(); field(config.color, "color"); field(config.alpha, "alpha"); field(config.use_sphere, "use_sphere"); @@ -72,13 +71,12 @@ void declare_config(LayerConfig::Edges& config) { field(config.draw, "draw"); field(config.scale, "scale"); field(config.alpha, "alpha"); + config.color.setOptional(); field(config.color, "color"); - field(config.draw_interlayer, "draw_interlayer"); - field(config.interlayer_use_source, "interlayer_use_source"); - field(config.interlayer_scale, "interlayer_scale"); - field(config.interlayer_alpha, "interlayer_alpha"); - field(config.interlayer_use_color, "interlayer_use_color"); - field(config.interlayer_insertion_skip, "interlayer_insertion_skip"); + field(config.insertion_skip, "insertion_skip"); + + check(config.scale, GT, 0.0, "scale"); + checkInRange(config.alpha, 0.0, 1.0, "alpha"); } void declare_config(LayerConfig::Text& config) { @@ -87,12 +85,16 @@ void declare_config(LayerConfig::Text& config) { field(config.draw, "draw"); field(config.draw_layer, "draw_layer"); field(config.collapse, "collapse"); + config.adapter.setOptional(); field(config.adapter, "adapter"); field(config.height, "height"); field(config.scale, "scale"); field(config.add_jitter, "add_jitter"); field(config.jitter_scale, "jitter_scale"); enum_field(config.color, "color"); + + check(config.scale, GT, 0.0, "scale"); + check(config.jitter_scale, GE, 0.0, "jitter_scale"); } void declare_config(LayerConfig::BoundingBoxes& config) { @@ -104,6 +106,11 @@ void declare_config(LayerConfig::BoundingBoxes& config) { field(config.edge_scale, "edge_scale"); field(config.alpha, "alpha"); field(config.edge_break_ratio, "edge_break_ratio"); + + check(config.scale, GT, 0.0, "scale"); + check(config.edge_scale, GT, 0.0, "edge_scale"); + checkInRange(config.alpha, 0.0, 1.0, "alpha"); + checkInRange(config.edge_break_ratio, 0.0, 1.0, "edge_break_ratio"); } void declare_config(LayerConfig::Boundaries& config) { @@ -116,6 +123,10 @@ void declare_config(LayerConfig::Boundaries& config) { field(config.alpha, "alpha"); field(config.draw_ellipse, "draw_ellipse"); field(config.ellipse_alpha, "ellipse_alpha"); + + check(config.wireframe_scale, GT, 0.0, "wireframe_scale"); + checkInRange(config.alpha, 0.0, 1.0, "alpha"); + checkInRange(config.ellipse_alpha, 0.0, 1.0, "ellipse_alpha"); } void declare_config(LayerConfig& config) { @@ -124,7 +135,6 @@ void declare_config(LayerConfig& config) { field(config.visualize, "visualize"); field(config.z_offset_scale, "z_offset_scale"); field(config.draw_frontier_ellipse, "draw_frontier_ellipse"); - field(config.draw_mesh_edges, "draw_mesh_edges"); // subconfigs field(config.nodes, "nodes"); @@ -134,7 +144,7 @@ void declare_config(LayerConfig& config) { field(config.boundaries, "boundaries"); } -LayerInfo::LayerInfo(const LayerConfig config) +LayerInfo::LayerInfo(const LayerConfig& config) : config(config), z_offset(0.0), node_color(&getNodeColor), @@ -145,7 +155,7 @@ LayerInfo& LayerInfo::offset(double offset_size, bool collapse) { return *this; } -LayerInfo& LayerInfo::graph(const DynamicSceneGraph& graph, LayerId layer) { +LayerInfo& LayerInfo::graph(const DynamicSceneGraph& graph, LayerKey layer) { node_color_adapter_ = config.nodes.color.create(); if (node_color_adapter_) { node_color_adapter_->setGraph(graph, layer); diff --git a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp index 5261eb7..bdddb06 100644 --- a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp +++ b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp @@ -122,7 +122,7 @@ KhronosObjectPlugin::KhronosObjectPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name) : VisualizerPlugin(name), - config_("KhronosObjectPlugin", config::checkValid(config)), + config_("khronos_object_plugin", config::checkValid(config)), dynamic_pub_( nh.create_publisher("dynamic_objects", config.queue_size)), static_pub_(nh.create_publisher("static_objects", diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index dffffe3..2465204 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_plugin.cpp @@ -65,7 +65,7 @@ MeshPlugin::MeshPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name) : VisualizerPlugin(name), - config_("MeshPlugin", config::checkValid(config)), + config_("mesh_plugin", config::checkValid(config)), mesh_pub_(nh.create_publisher( name, rclcpp::QoS(1).transient_local())) {} diff --git a/hydra_visualizer/src/scene_graph_renderer.cpp b/hydra_visualizer/src/scene_graph_renderer.cpp index cd04704..3fd6c86 100644 --- a/hydra_visualizer/src/scene_graph_renderer.cpp +++ b/hydra_visualizer/src/scene_graph_renderer.cpp @@ -35,6 +35,7 @@ #include "hydra_visualizer/scene_graph_renderer.h" #include +#include #include #include #include @@ -58,7 +59,7 @@ namespace { inline std::string keyToLayerName(LayerKey key) { std::stringstream ss; - ss << "layer_" << key.layer; + ss << "layer" << key.layer; if (key.partition) { ss << "p" << key.partition; } @@ -66,24 +67,6 @@ inline std::string keyToLayerName(LayerKey key) { return ss.str(); } -inline Marker makeNewEdgeList(const std_msgs::msg::Header& header, - const std::string& ns_prefix, - LayerKey source, - LayerKey target) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - - std::stringstream ss; - ss << ns_prefix << source << "_" << target; - marker.ns = ss.str(); - return marker; -} - -} // namespace - struct MarkerNamespaces { static std::string layerNodeNamespace(LayerKey key) { return keyToLayerName(key) + "_nodes"; @@ -118,6 +101,34 @@ struct MarkerNamespaces { } }; +struct InterlayerInfo { + using EdgeColor = EdgeColorAdapter::EdgeColor; + using ColorFunc = std::function; + InterlayerEdgeConfig config; + size_t marker_idx; + std::unique_ptr adapter; + size_t num_since_last = 0; +}; + +} // namespace + +InterlayerEdgeConfig::InterlayerEdgeConfig() { color = {}; } + +void declare_config(InterlayerEdgeConfig& config) { + using namespace config; + name("InterlayerEdgeConfig"); + base(config); + field(config.use_child_color, "use_child_color"); +} + +void declare_config(SceneGraphRenderer::Config::InterlayerEdges& config) { + using namespace config; + name("SceneGraphRenderer::Config::InterlayerEdges"); + field(config.config, ""); + field(config.from, "from"); + field(config.to, "to"); +} + void declare_config(GraphRenderConfig& config) { using namespace config; name("GraphRenderConfig"); @@ -129,30 +140,16 @@ void declare_config(SceneGraphRenderer::Config& config) { using namespace config; name("SceneGraphRenderer::Config"); field(config.graph, "graph"); - field(config.layers, "layers"); - field(config.partitions, "partitions"); + field>(config.layers, "layers"); + field(config.interlayer_edges, "interlayer_edges"); } SceneGraphRenderer::SceneGraphRenderer(const Config& config, ianvs::NodeHandle nh) - : nh_(nh), - graph_config_("renderer", config.graph, [this]() { has_change_ = true; }), + : init_config_(config), + nh_(nh), + graph_config_("scene_graph", config.graph, [this]() { has_change_ = true; }), pub_(nh.create_publisher("graph", rclcpp::QoS(1).transient_local())), - has_change_(false) { - // init wrappers from parsed initial config - for (const auto& [layer_id, layer_config] : config.layers) { - const auto ns = "renderer/config/layer" + std::to_string(layer_id); - layers_.emplace(layer_id, - std::make_unique( - ns, layer_config, [this]() { has_change_ = true; })); - } - - for (const auto& [layer_id, layer_config] : config.partitions) { - const auto ns = "renderer/config/partitions/layer" + std::to_string(layer_id); - partitions_.emplace(layer_id, - std::make_unique( - ns, layer_config, [this]() { has_change_ = true; })); - } -} + has_change_(false) {} void SceneGraphRenderer::reset(const std_msgs::msg::Header& header) { MarkerArray msg; @@ -171,13 +168,14 @@ void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, setConfigs(graph); MarkerArray msg; - for (const auto& [layer_id, layer] : graph.layers()) { - drawLayer(header, layer_infos_.at(layer_id), *layer, graph.mesh().get(), msg); + for (const auto& [_, layer] : graph.layers()) { + drawLayer(header, layer_infos_.at(layer->id), *layer, graph.mesh().get(), msg); } - for (const auto& [l_id, partitions] : graph.layer_partitions()) { - for (const auto& [partition_id, partition] : partitions) { - drawLayer(header, partition_infos_.at(l_id), *partition, graph.mesh().get(), msg); + for (const auto& [_, partitions] : graph.layer_partitions()) { + for (const auto& [_, partition] : partitions) { + const auto& partition_info = layer_infos_.at(partition->id); + drawLayer(header, partition_info, *partition, graph.mesh().get(), msg); } } @@ -191,68 +189,115 @@ void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, } } +InterlayerEdgeConfig SceneGraphRenderer::getInterlayerEdgeConfig(LayerKey parent, + LayerKey child) const { + const auto name = keyToLayerName(parent) + "_to_" + keyToLayerName(child); + auto iter = interlayer_edges_.find(name); + if (iter == interlayer_edges_.end()) { + InterlayerEdgeConfig config; + for (const auto& info : init_config_.interlayer_edges) { + bool from_parent = info.from.matches(parent) && info.to.matches(child); + bool to_parent = info.to.matches(parent) && info.from.matches(child); + if (!from_parent && !to_parent) { + continue; + } + + config = info.config; + if (info.from.wildcard || info.to.wildcard) { + continue; // allow more specific keys to take precedence + } else { + break; + } + } + + const auto ns = "scene_graph_interlayer_" + name; + auto wrapper = std::make_unique(ns, config); + wrapper->setCallback([this]() { has_change_ = true; }); + iter = interlayer_edges_.emplace(name, std::move(wrapper)).first; + } + + return iter->second->get(); +} + void SceneGraphRenderer::drawInterlayerEdges(const std_msgs::msg::Header& header, const DynamicSceneGraph& graph, MarkerArray& msg) const { const std::string ns_prefix = "interlayer_edges_"; - std::map marker_indices; - std::map num_since_last; + std::map, InterlayerInfo> edge_info; for (const auto& [key, edge] : graph.interlayer_edges()) { const auto& source = graph.getNode(edge.source); - const auto& source_info = getLayerInfo(source.layer); const auto& target = graph.getNode(edge.target); + const auto& source_info = getLayerInfo(source.layer); const auto& target_info = getLayerInfo(target.layer); if (!source_info.shouldVisualize(source) || !target_info.shouldVisualize(target)) { continue; } - if (!source_info.config.edges.draw_interlayer || - !target_info.config.edges.draw_interlayer) { - continue; + const auto target_is_parent = source.layer < target.layer; + auto keys = target_is_parent ? std::make_pair(target.layer, source.layer) + : std::make_pair(source.layer, target.layer); + auto iter = edge_info.find(keys); + if (iter == edge_info.end()) { + std::stringstream ss; + ss << ns_prefix << source.layer << "_" << target.layer; + const auto new_conf = getInterlayerEdgeConfig(keys.first, keys.second); + + InterlayerInfo new_info{new_conf, msg.markers.size(), new_conf.color.create()}; + new_info.num_since_last = new_conf.insertion_skip; + if (new_info.adapter) { + // TODO(nathan) this is awkard because it only supports intralayer edges + new_info.adapter->setGraph(graph, keys.first); + } + + iter = edge_info.emplace(keys, std::move(new_info)).first; + auto& marker = msg.markers.emplace_back(); + marker.header = header; + marker.type = Marker::LINE_LIST; + marker.action = Marker::ADD; + marker.id = 0; + marker.ns = ss.str(); + marker.scale.x = new_conf.scale; } - const auto use_source = source_info.config.edges.interlayer_use_source; - const auto& info = use_source ? source_info : target_info; - - auto iter = marker_indices.find(source.layer); - if (iter == marker_indices.end()) { - iter = marker_indices.emplace(source.layer, msg.markers.size()).first; - msg.markers.push_back( - makeNewEdgeList(header, ns_prefix, source.layer, target.layer)); - msg.markers.back().scale.x = info.config.edges.interlayer_scale; - // make sure we always draw at least one edge - num_since_last[source.layer] = info.config.edges.interlayer_insertion_skip; + auto& info = iter->second; + if (!info.config.draw) { + continue; } - if (num_since_last[source.layer] >= info.config.edges.interlayer_insertion_skip) { - num_since_last[source.layer] = 0; + if (info.num_since_last >= info.config.insertion_skip) { + info.num_since_last = 0; } else { - num_since_last[source.layer]++; + ++info.num_since_last; continue; } - auto& marker = msg.markers.at(iter->second); - geometry_msgs::msg::Point source_point; + auto& marker = msg.markers.at(info.marker_idx); + auto& source_point = marker.points.emplace_back(); tf2::convert(source.attributes().position, source_point); source_point.z += source_info.z_offset; - marker.points.push_back(source_point); - geometry_msgs::msg::Point target_point; + auto& target_point = marker.points.emplace_back(); tf2::convert(target.attributes().position, target_point); target_point.z += target_info.z_offset; - marker.points.push_back(target_point); - const auto color = makeColorMsg(info.node_color(use_source ? source : target), - info.config.edges.interlayer_alpha); - marker.colors.push_back(color); - marker.colors.push_back(color); + if (info.adapter) { + const auto [c_s, c_t] = info.adapter->getColor(graph, edge); + marker.colors.push_back(makeColorMsg(c_s, info.config.alpha)); + marker.colors.push_back(makeColorMsg(c_t, info.config.alpha)); + } else { + bool use_source = !target_is_parent ^ info.config.use_child_color; + const auto color = + use_source ? source_info.node_color(source) : target_info.node_color(target); + marker.colors.push_back(makeColorMsg(color, info.config.alpha)); + marker.colors.push_back(makeColorMsg(color, info.config.alpha)); + } } } void SceneGraphRenderer::drawLayer(const std_msgs::msg::Header& header, const LayerInfo& info, const SceneGraphLayer& layer, - const Mesh* mesh, + const Mesh* /* mesh */, MarkerArray& msg) const { if (!info.config.visualize) { return; @@ -306,55 +351,57 @@ void SceneGraphRenderer::drawLayer(const std_msgs::msg::Header& header, const auto edge_ns = MarkerNamespaces::layerEdgeNamespace(layer.id); tracker_.add(makeLayerEdgeMarkers(header, info, layer, edge_ns), msg); +} + +LayerConfig SceneGraphRenderer::getLayerConfig(spark_dsg::LayerKey key) const { + auto iter = layers_.find(key); + if (iter == layers_.end()) { + // TODO(nathan) actually allow layer names when looking up configs + LayerConfig init_config; + for (const auto& [config_key, config] : init_config_.layers) { + if (!config_key.matches(key)) { + continue; + } + + init_config = config; + if (config_key.wildcard) { + continue; // allow more specific keys to take precedence + } else { + break; + } + } - if (mesh && info.config.draw_mesh_edges) { - const std::string ns = MarkerNamespaces::meshEdgeNamespace(layer.id); - tracker_.add(makeMeshEdgesMarker(header, info, layer, *mesh, ns), msg); + const auto ns = "scene_graph_" + keyToLayerName(key); + iter = layers_.emplace(key, std::make_unique(ns, init_config)) + .first; + iter->second->setCallback([this]() { has_change_ = true; }); } + + return iter->second->get(); } void SceneGraphRenderer::setConfigs(const DynamicSceneGraph& graph) const { layer_infos_.clear(); - partition_infos_.clear(); - - const auto& graph_config = graph_config_.get(); - for (const auto& [layer_id, layer] : graph.layers()) { - auto iter = layers_.find(layer_id); - if (iter == layers_.end()) { - // TODO(nathan) think about logging - const auto ns = "renderer/config/layer" + std::to_string(layer_id); - iter = layers_.emplace(layer_id, std::make_unique(ns)).first; - iter->second->setCallback([this]() { has_change_ = true; }); - } - - // TODO(nathan) this is ugly because layer info doesn't have a copy constructor - layer_infos_.emplace(layer_id, LayerInfo(iter->second->get())) - .first->second.offset(graph_config.layer_z_step, graph_config.collapse_layers) - .graph(graph, layer_id); + const auto graph_config = graph_config_.get(); + const auto z_step = graph_config.layer_z_step; + const auto collapse = graph_config.collapse_layers; + for (const auto& [_, layer] : graph.layers()) { + const auto layer_config = getLayerConfig(layer->id); + auto& [_key, info] = *layer_infos_.emplace(layer->id, layer_config).first; + info.offset(z_step, collapse).graph(graph, layer->id); } - for (const auto& [l_id, partitions] : graph.layer_partitions()) { - auto iter = partitions_.find(l_id); - if (iter == partitions_.end()) { - // TODO(nathan) think about logging - const auto ns = "renderer/config/partitions/layer" + std::to_string(l_id); - iter = partitions_.emplace(l_id, std::make_unique(ns)).first; - iter->second->setCallback([this]() { has_change_ = true; }); + for (const auto& [_, partitions] : graph.layer_partitions()) { + for (const auto& [_, layer] : partitions) { + const auto layer_config = getLayerConfig(layer->id); + auto& [_key, info] = *layer_infos_.emplace(layer->id, layer_config).first; + info.offset(z_step, collapse).graph(graph, layer->id); } - - // TODO(nathan) this is ugly because layer info doesn't have a copy constructor - partition_infos_.emplace(l_id, LayerInfo(iter->second->get())) - .first->second.offset(graph_config.layer_z_step, graph_config.collapse_layers) - .graph(graph, l_id); } } const LayerInfo& SceneGraphRenderer::getLayerInfo(LayerKey layer) const { - if (!layer.partition) { - return layer_infos_.at(layer.layer); - } else { - return partition_infos_.at(layer.layer); - } + return layer_infos_.at(layer); } } // namespace hydra diff --git a/hydra_visualizer/src/utils/layer_key_selector.cpp b/hydra_visualizer/src/utils/layer_key_selector.cpp new file mode 100644 index 0000000..65ae45f --- /dev/null +++ b/hydra_visualizer/src/utils/layer_key_selector.cpp @@ -0,0 +1,129 @@ +/* ----------------------------------------------------------------------------- + * 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/utils/layer_key_selector.h" + +#include + +#include + +namespace hydra { + +using namespace spark_dsg; + +std::optional LayerKeySelector::parse(const std::string& str) { + std::regex re(R"((\d+)p\*$|(\d+)\*$|(\d+)p(\d+)$|(\d+)$)"); + std::smatch match; + if (!std::regex_match(str, match, re)) { + return std::nullopt; + } + + CHECK_EQ(match.size(), 6); + if (!match.str(1).empty()) { + // partitions >= 1 + return LayerKeySelector{LayerKey{std::stol(match.str(1))}, true, false}; + } else if (!match.str(2).empty()) { + // partitions >= 0 + return LayerKeySelector{LayerKey{std::stol(match.str(2))}, true, true}; + } else if (!match.str(3).empty()) { + // layer and partition + const auto part_id = static_cast(std::stoi(match.str(4))); + return LayerKeySelector{LayerKey{std::stol(match.str(3)), part_id}}; + } else { + // layer and partition 0 + return LayerKeySelector{LayerKey{std::stol(match.str(5))}}; + } +} + +std::string LayerKeySelector::str() const { + const auto layer_str = std::to_string(key.layer); + if (wildcard) { + return layer_str + (include_default ? "*" : "p*"); + } + + if (key.partition) { + return layer_str + "p" + std::to_string(key.partition); + } + + return layer_str; +} + +bool LayerKeySelector::matches(LayerKey to_match) const { + if (key.layer != to_match.layer) { + return false; + } + + if (wildcard) { + return include_default || to_match.partition; + } + + return key.partition == to_match.partition; +} + +bool LayerKeySelector::operator<(const LayerKeySelector& other) const { + if (key < other.key) { + return true; + } + + if (key == other.key) { + return wildcard < other.wildcard; + } else { + return false; + } +} + +bool LayerKeySelector::operator==(const LayerKeySelector& other) const = default; + +bool LayerKeySelector::operator!=(const LayerKeySelector& other) const = default; + +std::string SelectorConversion::toIntermediate(const LayerKeySelector& value, + std::string&) { + return value.str(); +} + +void SelectorConversion::fromIntermediate(const std::string& intermediate, + LayerKeySelector& value, + std::string& error) { + const auto parsed = LayerKeySelector::parse(intermediate); + if (!parsed) { + error = "Invalid layer selector '" + intermediate + "', must be of form " + + "integer layer (e.g., '3')" + ", layer and partition (e.g., '2p1')" + + ", or layer and wildcard (e.g., '4p*')"; + return; + } + + value = *parsed; +} + +} // namespace hydra diff --git a/hydra_visualizer/tests/test_layer_key_selector.cpp b/hydra_visualizer/tests/test_layer_key_selector.cpp new file mode 100644 index 0000000..3120ccf --- /dev/null +++ b/hydra_visualizer/tests/test_layer_key_selector.cpp @@ -0,0 +1,106 @@ +/* ----------------------------------------------------------------------------- + * 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 +#include + +namespace hydra { + +TEST(LayerKeySelector, ParsingCorrect) { + { // normal layer + LayerKeySelector expected{{3}, false, false}; + const auto result = LayerKeySelector::parse("3"); + ASSERT_TRUE(result); + EXPECT_EQ(*result, expected); + } + + { // partition + LayerKeySelector expected{{3, 2}, false, false}; + const auto result = LayerKeySelector::parse("3p2"); + ASSERT_TRUE(result); + EXPECT_EQ(*result, expected); + } + + { // partitions >= 1 + LayerKeySelector expected{{3}, true, false}; + const auto result = LayerKeySelector::parse("3p*"); + ASSERT_TRUE(result); + EXPECT_EQ(*result, expected); + } + + { // partitions >= 0 + LayerKeySelector expected{{3}, true, true}; + const auto result = LayerKeySelector::parse("3*"); + ASSERT_TRUE(result); + EXPECT_EQ(*result, expected); + } + + EXPECT_FALSE(LayerKeySelector::parse("3p")); +} + +TEST(LayerKeySelector, MatchesCorrect) { + { // normal layer + LayerKeySelector selector{{3}, false, false}; + EXPECT_FALSE(selector.matches({2})); + EXPECT_TRUE(selector.matches({3})); + EXPECT_FALSE(selector.matches({3, 1})); + EXPECT_FALSE(selector.matches({3, 1})); + } + + { // partition + LayerKeySelector selector{{3, 2}, false, false}; + EXPECT_FALSE(selector.matches({2})); + EXPECT_FALSE(selector.matches({3})); + EXPECT_FALSE(selector.matches({3, 1})); + EXPECT_TRUE(selector.matches({3, 2})); + } + + { // any partition + LayerKeySelector selector{{3}, true, false}; + EXPECT_FALSE(selector.matches({2})); + EXPECT_FALSE(selector.matches({3})); + EXPECT_TRUE(selector.matches({3, 1})); + EXPECT_TRUE(selector.matches({3, 2})); + } + + { // any partition + default + LayerKeySelector selector{{3}, true, true}; + EXPECT_FALSE(selector.matches({2})); + EXPECT_TRUE(selector.matches({3})); + EXPECT_TRUE(selector.matches({3, 1})); + EXPECT_TRUE(selector.matches({3, 2})); + } +} + +} // namespace hydra