From 9c314998d602786c994029e7123e652f0297b2a9 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sun, 26 Apr 2026 15:46:55 +0000 Subject: [PATCH 1/8] start on code to save current config --- .../hydra_visualizer/scene_graph_renderer.h | 5 ++++ hydra_visualizer/src/scene_graph_renderer.cpp | 24 +++++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h index f56424e..76a422e 100644 --- a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h +++ b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h @@ -36,6 +36,7 @@ #include #include #include +#include #include @@ -107,6 +108,9 @@ class SceneGraphRenderer { virtual void clearChangeFlag(); protected: + void saveConfigs(const std_srvs::srv::Empty::Request::SharedPtr& req, + std_srvs::srv::Empty::Response::SharedPtr res); + virtual void setConfigs(const spark_dsg::DynamicSceneGraph& graph) const; virtual void drawInterlayerEdges(const std_msgs::msg::Header& header, @@ -133,6 +137,7 @@ class SceneGraphRenderer { config::DynamicConfig graph_config_; rclcpp::Publisher::SharedPtr pub_; std::map> layer_plugins_; + rclcpp::Service::SharedPtr save_service_; mutable MarkerTracker tracker_; mutable std::atomic has_change_; diff --git a/hydra_visualizer/src/scene_graph_renderer.cpp b/hydra_visualizer/src/scene_graph_renderer.cpp index f4456b7..154fd95 100644 --- a/hydra_visualizer/src/scene_graph_renderer.cpp +++ b/hydra_visualizer/src/scene_graph_renderer.cpp @@ -35,12 +35,15 @@ #include "hydra_visualizer/scene_graph_renderer.h" #include +#include #include #include #include #include #include +#include + #include #include @@ -52,6 +55,7 @@ namespace hydra { using namespace spark_dsg; using namespace visualizer; +using std_srvs::srv::Empty; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -157,6 +161,8 @@ SceneGraphRenderer::SceneGraphRenderer(const Config& config, ianvs::NodeHandle n nh_(nh), graph_config_("scene_graph", config.graph, [this]() { has_change_ = true; }), pub_(nh.create_publisher("graph", rclcpp::QoS(1).transient_local())), + save_service_( + nh_.create_service("reload", &SceneGraphRenderer::saveConfigs, this)), has_change_(false) { for (const auto& plugin_config : config.layer_plugins) { const auto key = plugin_config.layer; @@ -204,6 +210,24 @@ void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, } } +void SceneGraphRenderer::saveConfigs(const Empty::Request::SharedPtr&, + Empty::Response::SharedPtr) { + YAML::Node root; + root["renderer"] = config::toYaml(graph_config_.get()); + + YAML::Node layers; + for (const auto& [key, wrapper] : layers_) { + layers[LayerKeySelector{key}.str()] = config::toYaml(wrapper->get()); + } + + root["renderer"]["layers"] = layers; + + const auto outfile_name = std::string(std::tmpnam(nullptr)) + "_config.yaml"; + std::ofstream fout(outfile_name); + fout << root; + LOG(WARNING) << "Saved config to '" << outfile_name << "'"; +} + InterlayerEdgeConfig SceneGraphRenderer::getInterlayerEdgeConfig(LayerKey parent, LayerKey child) const { const auto name = keyToLayerName(parent) + "_to_" + keyToLayerName(child); From 31fe21bfccc9e344f94fd37502d00f5e3fda041d Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sun, 26 Apr 2026 16:49:06 +0000 Subject: [PATCH 2/8] move config dumping to visualizer node and add plugins --- .../config/visualizer_config.yaml | 19 ++++++ .../config/visualizer_plugins.yaml | 24 -------- .../plugins/basis_point_plugin.h | 2 + .../plugins/footprint_plugin.h | 2 + .../plugins/khronos_object_plugin.h | 2 + .../hydra_visualizer/plugins/layer_plugin.h | 2 + .../hydra_visualizer/plugins/mesh_plugin.h | 2 + .../plugins/mesh_point_plugin.h | 2 + .../plugins/places_freespace_plugin.h | 2 + .../hydra_visualizer/plugins/pose_plugin.h | 2 + .../plugins/region_growing_boundary_plugin.h | 2 + .../plugins/traversability_plugin.h | 2 + .../plugins/visualizer_plugin.h | 3 + .../hydra_visualizer/scene_graph_renderer.h | 7 +-- .../hydra_visualizer/visualizer_node.h | 4 ++ .../launch/static_visualizer.launch.yaml | 2 - .../launch/streaming_visualizer.launch.yaml | 2 - .../src/plugins/basis_point_plugin.cpp | 5 ++ .../src/plugins/footprint_plugin.cpp | 3 + .../src/plugins/khronos_object_plugin.cpp | 5 ++ hydra_visualizer/src/plugins/mesh_plugin.cpp | 3 + .../src/plugins/mesh_point_plugin.cpp | 3 + .../src/plugins/places_freespace_plugin.cpp | 9 ++- hydra_visualizer/src/plugins/pose_plugin.cpp | 3 + .../region_growing_boundary_plugin.cpp | 8 ++- .../src/plugins/traversability_plugin.cpp | 5 ++ hydra_visualizer/src/scene_graph_renderer.cpp | 61 +++++++++++++------ hydra_visualizer/src/visualizer_node.cpp | 25 ++++++++ 28 files changed, 155 insertions(+), 56 deletions(-) delete mode 100644 hydra_visualizer/config/visualizer_plugins.yaml diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index 376fa3e..65ce11d 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/hydra_visualizer/config/visualizer_config.yaml @@ -1,4 +1,23 @@ --- +plugins: + mesh: + type: MeshPlugin + coloring: + type: SemanticMeshColoring + places_basis_points: + type: BasisPointPlugin + show_voxblox_connections: false + draw_basis_points: true + basis_point_scale: 0.1 + places_node_scale: 0.1 + khronos_objects: + type: KhronosObjectPlugin + dynamic_color_mode: CONSTANT # ID, SEMANTIC, CONSTANT + places_traversability: + type: TraversabilityPlugin + agent_poses: + type: PosePlugin + num_to_skip: 0 renderer: layer_z_step: 5.5 # unit separation between layers collapse_layers: false # whether or not to apply offsets to each of the layers diff --git a/hydra_visualizer/config/visualizer_plugins.yaml b/hydra_visualizer/config/visualizer_plugins.yaml deleted file mode 100644 index 5980d44..0000000 --- a/hydra_visualizer/config/visualizer_plugins.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -plugins: - mesh: - type: MeshPlugin - coloring: - type: SemanticMeshColoring - places_basis_points: - type: BasisPointPlugin - show_voxblox_connections: false - draw_basis_points: true - basis_point_scale: 0.1 - places_node_scale: 0.1 - khronos_objects: - type: KhronosObjectPlugin - dynamic_color_mode: CONSTANT # ID, SEMANTIC, CONSTANT - places_traversability: - type: TraversabilityPlugin - agent_poses: - type: PosePlugin - num_to_skip: 0 -# dynamic reconfigure settings -places_basis_points: - graph: - visualize: true diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h index 29c0fff..f5c5b0d 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h @@ -70,6 +70,8 @@ class BasisPointPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: void fillMarkers(const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h index d50964e..6097b57 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/footprint_plugin.h @@ -66,6 +66,8 @@ class FootprintPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: rclcpp::Publisher::SharedPtr pub_; MarkerTracker tracker_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h index f959fa9..80b5aa9 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h @@ -135,6 +135,8 @@ class KhronosObjectPlugin : public VisualizerPlugin { const spark_dsg::DynamicSceneGraph& graph) override; void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: // Helper functions. void drawDynamicObjects(const Config& config, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h index 1898fc0..bd96d9e 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/layer_plugin.h @@ -59,6 +59,8 @@ struct LayerPlugin { virtual void clearChangeFlag() { has_change_ = false; } + virtual YAML::Node dumpConfig() const { return YAML::Node(); } + protected: bool has_change_ = false; }; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h index a68f060..6f2b435 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h @@ -63,6 +63,8 @@ class MeshPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: config::DynamicConfig config_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h index ad98ea1..f21d8e4 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_point_plugin.h @@ -67,6 +67,8 @@ class MeshPointPlugin : public LayerPlugin { visualization_msgs::msg::MarkerArray& msg, MarkerTracker& tracker) override; + YAML::Node dumpConfig() const override; + private: std::string ns_; config::DynamicConfig config_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h index 0ecaac3..dc0bc3f 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h @@ -64,6 +64,8 @@ class PlacesFreespacePlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: void fillMarkers(const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h index 4439de2..367ef64 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/pose_plugin.h @@ -60,6 +60,8 @@ class PosePlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: size_t num_received_; rclcpp::Publisher::SharedPtr pub_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h index aed858b..c2c6f98 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h @@ -63,6 +63,8 @@ class RegionGrowingBoundaryPlugin : public LayerPlugin { visualization_msgs::msg::MarkerArray& msg, MarkerTracker& tracker) override; + YAML::Node dumpConfig() const override; + protected: const std::string ns_; config::DynamicConfig config_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h index 40201d7..ac020ea 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h @@ -75,6 +75,8 @@ class TraversabilityPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; + YAML::Node dumpConfig() const override; + protected: void fillMarkers(const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph, diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h index a47fcef..bfedfdc 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once #include +#include #include @@ -57,6 +58,8 @@ class VisualizerPlugin { virtual void clearChangeFlag() { has_change_ = false; } + virtual YAML::Node dumpConfig() const { return YAML::Node(); } + const std::string name; protected: diff --git a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h index 76a422e..72fbfc6 100644 --- a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h +++ b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h @@ -36,7 +36,6 @@ #include #include #include -#include #include @@ -107,10 +106,9 @@ class SceneGraphRenderer { virtual void clearChangeFlag(); - protected: - void saveConfigs(const std_srvs::srv::Empty::Request::SharedPtr& req, - std_srvs::srv::Empty::Response::SharedPtr res); + YAML::Node dumpConfig() const; + protected: virtual void setConfigs(const spark_dsg::DynamicSceneGraph& graph) const; virtual void drawInterlayerEdges(const std_msgs::msg::Header& header, @@ -137,7 +135,6 @@ class SceneGraphRenderer { config::DynamicConfig graph_config_; rclcpp::Publisher::SharedPtr pub_; std::map> layer_plugins_; - rclcpp::Service::SharedPtr save_service_; mutable MarkerTracker tracker_; mutable std::atomic has_change_; diff --git a/hydra_visualizer/include/hydra_visualizer/visualizer_node.h b/hydra_visualizer/include/hydra_visualizer/visualizer_node.h index bce207b..e03f44e 100644 --- a/hydra_visualizer/include/hydra_visualizer/visualizer_node.h +++ b/hydra_visualizer/include/hydra_visualizer/visualizer_node.h @@ -39,6 +39,7 @@ #include #include +#include #include "hydra_visualizer/io/graph_wrapper.h" #include "hydra_visualizer/plugins/visualizer_plugin.h" @@ -76,6 +77,8 @@ class DsgVisualizer { private: void spinOnce(bool force = false); + void saveConfigs(const std::filesystem::path& output); + ianvs::NodeHandle nh_; rclcpp::TimerBase::SharedPtr loop_timer_; @@ -85,6 +88,7 @@ class DsgVisualizer { const config::RosDynamicConfigServer server_; rclcpp::Service::SharedPtr redraw_service_; rclcpp::Service::SharedPtr reset_service_; + rclcpp::Subscription::SharedPtr save_sub_; }; void declare_config(DsgVisualizer::Config& config); diff --git a/hydra_visualizer/launch/static_visualizer.launch.yaml b/hydra_visualizer/launch/static_visualizer.launch.yaml index e15d0a3..5acb17b 100644 --- a/hydra_visualizer/launch/static_visualizer.launch.yaml +++ b/hydra_visualizer/launch/static_visualizer.launch.yaml @@ -6,7 +6,6 @@ launch: - arg: {name: visualizer_frame, default: map, description: frame_id for visualizations} - arg: {name: visualizer_ns, default: hydra_dsg_visualizer, description: visualizer namespace} - arg: {name: visualizer_config_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml} - - arg: {name: visualizer_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_plugins.yaml} - arg: {name: external_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml} # visualizer node and control for launching - node: @@ -17,7 +16,6 @@ launch: on_exit: shutdown args: > --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: 0, glog_verbosity: $(var verbosity)} --config-utilities-yaml {graph: {type: GraphFromFile, frame_id: $(var visualizer_frame), filepath: $(var scene_graph)}} diff --git a/hydra_visualizer/launch/streaming_visualizer.launch.yaml b/hydra_visualizer/launch/streaming_visualizer.launch.yaml index 8d55c5c..c588c74 100644 --- a/hydra_visualizer/launch/streaming_visualizer.launch.yaml +++ b/hydra_visualizer/launch/streaming_visualizer.launch.yaml @@ -6,7 +6,6 @@ launch: # visualizer configuration - arg: {name: visualizer_ns, default: hydra_visualizer} - arg: {name: visualizer_config_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml} - - arg: {name: visualizer_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_plugins.yaml} - arg: {name: external_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml} # communication - arg: {name: use_zmq, default: 'false', description: use zmq to receive scene graphs} @@ -23,7 +22,6 @@ launch: on_exit: shutdown args: > --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: 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)}} diff --git a/hydra_visualizer/src/plugins/basis_point_plugin.cpp b/hydra_visualizer/src/plugins/basis_point_plugin.cpp index 1ec5cbe..614594c 100644 --- a/hydra_visualizer/src/plugins/basis_point_plugin.cpp +++ b/hydra_visualizer/src/plugins/basis_point_plugin.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -244,4 +245,8 @@ void BasisPointPlugin::drawBasisPoints(const Config& config, tracker_.add(marker, msg); } +YAML::Node BasisPointPlugin::dumpConfig() const { + return config::toYaml(config_.get()); +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/footprint_plugin.cpp b/hydra_visualizer/src/plugins/footprint_plugin.cpp index 84284e5..d773dc6 100644 --- a/hydra_visualizer/src/plugins/footprint_plugin.cpp +++ b/hydra_visualizer/src/plugins/footprint_plugin.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -174,4 +175,6 @@ void FootprintPlugin::reset(const std_msgs::msg::Header& header) { } } +YAML::Node FootprintPlugin::dumpConfig() const { return config::toYaml(config); } + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp index bdddb06..0402b2e 100644 --- a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp +++ b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp @@ -75,6 +75,7 @@ #include #include +#include #include #include #include @@ -313,4 +314,8 @@ Color KhronosObjectPlugin::getDynamicColor(const Config& config, } } +YAML::Node KhronosObjectPlugin::dumpConfig() const { + return config::toYaml(config_.get()); +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index 7d4ff98..971a035 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_plugin.cpp @@ -35,6 +35,7 @@ #include "hydra_visualizer/plugins/mesh_plugin.h" #include +#include #include #include @@ -97,4 +98,6 @@ std::string MeshPlugin::getMsgNamespace() const { return "robot0/dsg_mesh"; } +YAML::Node MeshPlugin::dumpConfig() const { return config::toYaml(config_.get()); } + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/mesh_point_plugin.cpp b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp index 974ad2c..24238a6 100644 --- a/hydra_visualizer/src/plugins/mesh_point_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp @@ -1,6 +1,7 @@ #include "hydra_visualizer/plugins/mesh_point_plugin.h" #include +#include #include "hydra_visualizer/color/colormap_utilities.h" @@ -131,4 +132,6 @@ void MeshPointPlugin::draw(const std_msgs::msg::Header& header, tracker.add(marker, msg); } +YAML::Node MeshPointPlugin::dumpConfig() const { return config::toYaml(config_.get()); } + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/places_freespace_plugin.cpp b/hydra_visualizer/src/plugins/places_freespace_plugin.cpp index 626a614..18bdba1 100644 --- a/hydra_visualizer/src/plugins/places_freespace_plugin.cpp +++ b/hydra_visualizer/src/plugins/places_freespace_plugin.cpp @@ -34,7 +34,8 @@ * -------------------------------------------------------------------------- */ #include "hydra_visualizer/plugins/places_freespace_plugin.h" -#include +#include +#include #include #include #include @@ -62,8 +63,6 @@ using spark_dsg::DsgLayers; using spark_dsg::DynamicSceneGraph; using spark_dsg::PlaceNodeAttributes; using spark_dsg::SceneGraphLayer; -using spark_dsg::SceneGraphNode; -using spark_dsg::SemanticNodeAttributes; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -157,4 +156,8 @@ void PlacesFreespacePlugin::drawSpheres(const Config& config, } } +YAML::Node PlacesFreespacePlugin::dumpConfig() const { + return config::toYaml(config_.get()); +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/pose_plugin.cpp b/hydra_visualizer/src/plugins/pose_plugin.cpp index 11d85b0..009f3cf 100644 --- a/hydra_visualizer/src/plugins/pose_plugin.cpp +++ b/hydra_visualizer/src/plugins/pose_plugin.cpp @@ -35,6 +35,7 @@ #include "hydra_visualizer/plugins/pose_plugin.h" #include +#include #include #include #include @@ -124,4 +125,6 @@ void PosePlugin::reset(const std_msgs::msg::Header& header) { pub_->publish(msg); } +YAML::Node PosePlugin::dumpConfig() const { return config::toYaml(config); } + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp index 8dab32e..f2287c3 100644 --- a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp +++ b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp @@ -35,6 +35,7 @@ #include "hydra_visualizer/plugins/region_growing_boundary_plugin.h" #include +#include #include #include @@ -72,7 +73,8 @@ void declare_config(RegionGrowingBoundaryPlugin::Config& config) { RegionGrowingBoundaryPlugin::RegionGrowingBoundaryPlugin(const Config& config, const std::string& ns) : ns_(ns), - config_(ns + "_mesh_point_plugin", config, [this]() { has_change_ = true; }) {} + config_(ns + "_region_growing_plugin", config, [this]() { has_change_ = true; }) { +} void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, const visualizer::LayerInfo& info, @@ -121,4 +123,8 @@ void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, tracker.add(marker, msg); } +YAML::Node RegionGrowingBoundaryPlugin::dumpConfig() const { + return config::toYaml(config_.get()); +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/traversability_plugin.cpp b/hydra_visualizer/src/plugins/traversability_plugin.cpp index 150f187..bf21ad4 100644 --- a/hydra_visualizer/src/plugins/traversability_plugin.cpp +++ b/hydra_visualizer/src/plugins/traversability_plugin.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -236,4 +237,8 @@ void TraversabilityPlugin::addBoundaryPoint(const Config& config, } } +YAML::Node TraversabilityPlugin::dumpConfig() const { + return config::toYaml(config_.get()); +} + } // namespace hydra diff --git a/hydra_visualizer/src/scene_graph_renderer.cpp b/hydra_visualizer/src/scene_graph_renderer.cpp index 154fd95..10e681a 100644 --- a/hydra_visualizer/src/scene_graph_renderer.cpp +++ b/hydra_visualizer/src/scene_graph_renderer.cpp @@ -42,9 +42,6 @@ #include #include -#include - -#include #include #include "hydra_visualizer/color/colormap_utilities.h" @@ -55,7 +52,6 @@ namespace hydra { using namespace spark_dsg; using namespace visualizer; -using std_srvs::srv::Empty; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -161,8 +157,6 @@ SceneGraphRenderer::SceneGraphRenderer(const Config& config, ianvs::NodeHandle n nh_(nh), graph_config_("scene_graph", config.graph, [this]() { has_change_ = true; }), pub_(nh.create_publisher("graph", rclcpp::QoS(1).transient_local())), - save_service_( - nh_.create_service("reload", &SceneGraphRenderer::saveConfigs, this)), has_change_(false) { for (const auto& plugin_config : config.layer_plugins) { const auto key = plugin_config.layer; @@ -181,9 +175,32 @@ void SceneGraphRenderer::reset(const std_msgs::msg::Header& header) { } } -bool SceneGraphRenderer::hasChange() const { return has_change_; } +bool SceneGraphRenderer::hasChange() const { + if (has_change_) { + return true; + } + + for (const auto& [key, plugins] : layer_plugins_) { + for (const auto& plugin : plugins) { + if (plugin && plugin->hasChange()) { + return true; + } + } + } -void SceneGraphRenderer::clearChangeFlag() { has_change_ = false; } + return false; +} + +void SceneGraphRenderer::clearChangeFlag() { + has_change_ = false; + for (const auto& [key, plugins] : layer_plugins_) { + for (const auto& plugin : plugins) { + if (plugin) { + plugin->clearChangeFlag(); + } + } + } +} void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, const DynamicSceneGraph& graph) const { @@ -210,22 +227,28 @@ void SceneGraphRenderer::draw(const std_msgs::msg::Header& header, } } -void SceneGraphRenderer::saveConfigs(const Empty::Request::SharedPtr&, - Empty::Response::SharedPtr) { - YAML::Node root; - root["renderer"] = config::toYaml(graph_config_.get()); - - YAML::Node layers; +YAML::Node SceneGraphRenderer::dumpConfig() const { + YAML::Node layers(YAML::NodeType::Map); for (const auto& [key, wrapper] : layers_) { layers[LayerKeySelector{key}.str()] = config::toYaml(wrapper->get()); } - root["renderer"]["layers"] = layers; + YAML::Node layer_plugins(YAML::NodeType::Sequence); + for (const auto& [key, plugins] : layer_plugins_) { + YAML::Node plugin_configs; + for (const auto& plugin : plugins) { + if (!plugin) { + continue; + } + + plugin_configs.push_back(plugin->dumpConfig()); + } + } - const auto outfile_name = std::string(std::tmpnam(nullptr)) + "_config.yaml"; - std::ofstream fout(outfile_name); - fout << root; - LOG(WARNING) << "Saved config to '" << outfile_name << "'"; + YAML::Node root = config::toYaml(graph_config_.get()); + root["layers"] = layers; + root["layer_plugins"] = layer_plugins; + return root; } InterlayerEdgeConfig SceneGraphRenderer::getInterlayerEdgeConfig(LayerKey parent, diff --git a/hydra_visualizer/src/visualizer_node.cpp b/hydra_visualizer/src/visualizer_node.cpp index bed30aa..15a54ed 100644 --- a/hydra_visualizer/src/visualizer_node.cpp +++ b/hydra_visualizer/src/visualizer_node.cpp @@ -39,8 +39,12 @@ #include #include +#include + namespace hydra { +using std_msgs::msg::String; + void declare_config(DsgVisualizer::Config& config) { using namespace config; name("HydraVisualizerConfig"); @@ -67,6 +71,8 @@ DsgVisualizer::DsgVisualizer(const Config& config, ianvs::NodeHandle nh) "reset", [this](const std_srvs::srv::Empty::Request::SharedPtr&, std_srvs::srv::Empty::Response::SharedPtr) { reset(); }); + save_sub_ = nh_.create_subscription( + "save_config", 1, [this](const String& msg) { saveConfigs(msg.data); }); } void DsgVisualizer::start() { @@ -141,4 +147,23 @@ void DsgVisualizer::spinOnce(bool force) { } } +void DsgVisualizer::saveConfigs(const std::filesystem::path& output) { + YAML::Node plugins(YAML::NodeType::Map); + for (const auto& plugin : plugins_) { + if (!plugin) { + continue; + } + + plugins[plugin->name] = plugin->dumpConfig(); + } + + YAML::Node root; + root["renderer"] = renderer_->dumpConfig(); + root["plugins"] = plugins; + + std::ofstream fout(output); + fout << root; + LOG(INFO) << "Saved config to " << output; +} + } // namespace hydra From c859ed6f78bb20d8d03e66ff779dbcb83552ffda Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sun, 26 Apr 2026 17:02:25 +0000 Subject: [PATCH 3/8] export plugin type when dumping config --- hydra_visualizer/src/plugins/basis_point_plugin.cpp | 4 +++- hydra_visualizer/src/plugins/footprint_plugin.cpp | 6 +++++- hydra_visualizer/src/plugins/khronos_object_plugin.cpp | 4 +++- hydra_visualizer/src/plugins/mesh_plugin.cpp | 6 +++++- hydra_visualizer/src/plugins/mesh_point_plugin.cpp | 6 +++++- hydra_visualizer/src/plugins/places_freespace_plugin.cpp | 4 +++- hydra_visualizer/src/plugins/pose_plugin.cpp | 8 ++++++-- .../src/plugins/region_growing_boundary_plugin.cpp | 4 +++- hydra_visualizer/src/plugins/traversability_plugin.cpp | 6 ++++-- 9 files changed, 37 insertions(+), 11 deletions(-) diff --git a/hydra_visualizer/src/plugins/basis_point_plugin.cpp b/hydra_visualizer/src/plugins/basis_point_plugin.cpp index 614594c..ff0f1e8 100644 --- a/hydra_visualizer/src/plugins/basis_point_plugin.cpp +++ b/hydra_visualizer/src/plugins/basis_point_plugin.cpp @@ -246,7 +246,9 @@ void BasisPointPlugin::drawBasisPoints(const Config& config, } YAML::Node BasisPointPlugin::dumpConfig() const { - return config::toYaml(config_.get()); + auto root = config::toYaml(config_.get()); + root["type"] = "BasisPointPlugin"; + return root; } } // namespace hydra diff --git a/hydra_visualizer/src/plugins/footprint_plugin.cpp b/hydra_visualizer/src/plugins/footprint_plugin.cpp index d773dc6..bab2bed 100644 --- a/hydra_visualizer/src/plugins/footprint_plugin.cpp +++ b/hydra_visualizer/src/plugins/footprint_plugin.cpp @@ -175,6 +175,10 @@ void FootprintPlugin::reset(const std_msgs::msg::Header& header) { } } -YAML::Node FootprintPlugin::dumpConfig() const { return config::toYaml(config); } +YAML::Node FootprintPlugin::dumpConfig() const { + auto root = config::toYaml(config); + root["type"] = "FootprintPlugin"; + return root; +} } // namespace hydra diff --git a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp index 0402b2e..57d9deb 100644 --- a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp +++ b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp @@ -315,7 +315,9 @@ Color KhronosObjectPlugin::getDynamicColor(const Config& config, } YAML::Node KhronosObjectPlugin::dumpConfig() const { - return config::toYaml(config_.get()); + auto root = config::toYaml(config_.get()); + root["type"] = "KhronosObjectPlugin"; + return root; } } // namespace hydra diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index 971a035..322b097 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_plugin.cpp @@ -98,6 +98,10 @@ std::string MeshPlugin::getMsgNamespace() const { return "robot0/dsg_mesh"; } -YAML::Node MeshPlugin::dumpConfig() const { return config::toYaml(config_.get()); } +YAML::Node MeshPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "MeshPlugin"; + return root; +} } // namespace hydra diff --git a/hydra_visualizer/src/plugins/mesh_point_plugin.cpp b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp index 24238a6..4f8b2ad 100644 --- a/hydra_visualizer/src/plugins/mesh_point_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_point_plugin.cpp @@ -132,6 +132,10 @@ void MeshPointPlugin::draw(const std_msgs::msg::Header& header, tracker.add(marker, msg); } -YAML::Node MeshPointPlugin::dumpConfig() const { return config::toYaml(config_.get()); } +YAML::Node MeshPointPlugin::dumpConfig() const { + auto root = config::toYaml(config_.get()); + root["type"] = "MeshPointPlugin"; + return root; +} } // namespace hydra diff --git a/hydra_visualizer/src/plugins/places_freespace_plugin.cpp b/hydra_visualizer/src/plugins/places_freespace_plugin.cpp index 18bdba1..95b3502 100644 --- a/hydra_visualizer/src/plugins/places_freespace_plugin.cpp +++ b/hydra_visualizer/src/plugins/places_freespace_plugin.cpp @@ -157,7 +157,9 @@ void PlacesFreespacePlugin::drawSpheres(const Config& config, } YAML::Node PlacesFreespacePlugin::dumpConfig() const { - return config::toYaml(config_.get()); + auto root = config::toYaml(config_.get()); + root["type"] = "PlacesFreespacePlugin"; + return root; } } // namespace hydra diff --git a/hydra_visualizer/src/plugins/pose_plugin.cpp b/hydra_visualizer/src/plugins/pose_plugin.cpp index 009f3cf..83f41c0 100644 --- a/hydra_visualizer/src/plugins/pose_plugin.cpp +++ b/hydra_visualizer/src/plugins/pose_plugin.cpp @@ -35,8 +35,8 @@ #include "hydra_visualizer/plugins/pose_plugin.h" #include -#include #include +#include #include #include #include @@ -125,6 +125,10 @@ void PosePlugin::reset(const std_msgs::msg::Header& header) { pub_->publish(msg); } -YAML::Node PosePlugin::dumpConfig() const { return config::toYaml(config); } +YAML::Node PosePlugin::dumpConfig() const { + auto root = config::toYaml(config); + root["type"] = "PosePlugin"; + return root; +} } // namespace hydra diff --git a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp index f2287c3..1b3feb8 100644 --- a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp +++ b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp @@ -124,7 +124,9 @@ void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, } YAML::Node RegionGrowingBoundaryPlugin::dumpConfig() const { - return config::toYaml(config_.get()); + auto root = config::toYaml(config_.get()); + root["type"] = "RegionGrowingBoundaryPlugin"; + return root; } } // namespace hydra diff --git a/hydra_visualizer/src/plugins/traversability_plugin.cpp b/hydra_visualizer/src/plugins/traversability_plugin.cpp index bf21ad4..4ba4e61 100644 --- a/hydra_visualizer/src/plugins/traversability_plugin.cpp +++ b/hydra_visualizer/src/plugins/traversability_plugin.cpp @@ -35,8 +35,8 @@ #include "hydra_visualizer/plugins/traversability_plugin.h" #include -#include #include +#include #include #include #include @@ -238,7 +238,9 @@ void TraversabilityPlugin::addBoundaryPoint(const Config& config, } YAML::Node TraversabilityPlugin::dumpConfig() const { - return config::toYaml(config_.get()); + auto root = config::toYaml(config_.get()); + root["type"] = "TraversabilityPlugin"; + return root; } } // namespace hydra From e7c95ac0e7906a530dad212b6ec52278a3d817f5 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sun, 26 Apr 2026 17:28:48 +0000 Subject: [PATCH 4/8] export entire config --- .../hydra_visualizer/scene_graph_renderer.h | 8 +++++++- .../launch/static_visualizer.launch.yaml | 8 ++++---- hydra_visualizer/src/scene_graph_renderer.cpp | 15 +++++++++++++-- 3 files changed, 24 insertions(+), 7 deletions(-) diff --git a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h index 72fbfc6..0887cc4 100644 --- a/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h +++ b/hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h @@ -140,7 +140,13 @@ class SceneGraphRenderer { mutable std::atomic has_change_; mutable std::map layer_infos_; mutable std::map> layers_; - mutable std::map> interlayer_edges_; + + struct EdgeConfigInfo { + spark_dsg::LayerKey parent; + spark_dsg::LayerKey child; + std::unique_ptr config; + }; + mutable std::map interlayer_edges_; }; void declare_config(SceneGraphRenderer::LayerPluginsConfig& config); diff --git a/hydra_visualizer/launch/static_visualizer.launch.yaml b/hydra_visualizer/launch/static_visualizer.launch.yaml index 5acb17b..e4d8600 100644 --- a/hydra_visualizer/launch/static_visualizer.launch.yaml +++ b/hydra_visualizer/launch/static_visualizer.launch.yaml @@ -5,8 +5,8 @@ launch: - arg: {name: scene_graph, default: '', description: filepath for scene graph to show} - arg: {name: visualizer_frame, default: map, description: frame_id for visualizations} - arg: {name: visualizer_ns, default: hydra_dsg_visualizer, description: visualizer namespace} - - arg: {name: visualizer_config_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml} - - arg: {name: external_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml} + - arg: {name: visualizer_config, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml} + - arg: {name: external_plugins, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml} # visualizer node and control for launching - node: pkg: hydra_visualizer @@ -15,8 +15,8 @@ launch: launch-prefix: $(if $(var debug) 'gdbserver localhost:3000') on_exit: shutdown args: > - --config-utilities-file $(var visualizer_config_path) - --config-utilities-file $(var external_plugins_path) + --config-utilities-file $(var visualizer_config) + --config-utilities-file $(var external_plugins) --config-utilities-yaml {glog_level: 0, glog_verbosity: $(var verbosity)} --config-utilities-yaml {graph: {type: GraphFromFile, frame_id: $(var visualizer_frame), filepath: $(var scene_graph)}} # rviz node and control for launching diff --git a/hydra_visualizer/src/scene_graph_renderer.cpp b/hydra_visualizer/src/scene_graph_renderer.cpp index 10e681a..d04222d 100644 --- a/hydra_visualizer/src/scene_graph_renderer.cpp +++ b/hydra_visualizer/src/scene_graph_renderer.cpp @@ -245,9 +245,18 @@ YAML::Node SceneGraphRenderer::dumpConfig() const { } } + YAML::Node interlayer_edge_configs(YAML::NodeType::Sequence); + for (const auto& [_, info] : interlayer_edges_) { + YAML::Node this_config = config::toYaml(info.config->get()); + this_config["from"] = LayerKeySelector{info.parent}.str(); + this_config["to"] = LayerKeySelector{info.child}.str(); + interlayer_edge_configs.push_back(this_config); + } + YAML::Node root = config::toYaml(graph_config_.get()); root["layers"] = layers; root["layer_plugins"] = layer_plugins; + root["interlayer_edges"] = interlayer_edge_configs; return root; } @@ -275,10 +284,12 @@ InterlayerEdgeConfig SceneGraphRenderer::getInterlayerEdgeConfig(LayerKey parent 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; + iter = interlayer_edges_ + .emplace(name, EdgeConfigInfo{parent, child, std::move(wrapper)}) + .first; } - return iter->second->get(); + return iter->second.config->get(); } void SceneGraphRenderer::drawInterlayerEdges(const std_msgs::msg::Header& header, From 73897ddc15369af96ac0c63d4851873b5b6f9e3c Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sat, 13 Jun 2026 15:42:53 +0000 Subject: [PATCH 5/8] actually export layer plugin configs --- hydra_visualizer/rviz/static_visualizer.rviz | 41 ++++++++++++++----- hydra_visualizer/src/scene_graph_renderer.cpp | 7 +++- 2 files changed, 36 insertions(+), 12 deletions(-) diff --git a/hydra_visualizer/rviz/static_visualizer.rviz b/hydra_visualizer/rviz/static_visualizer.rviz index d9c68da..5943b24 100644 --- a/hydra_visualizer/rviz/static_visualizer.rviz +++ b/hydra_visualizer/rviz/static_visualizer.rviz @@ -9,7 +9,7 @@ Panels: - /Mesh1/Topic1 - /Scene Graph1/Topic1 Splitter Ratio: 0.5 - Tree Height: 555 + Tree Height: 1065 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -72,7 +72,26 @@ Visualization Manager: Enabled: true Name: Scene Graph Namespaces: - {} + interlayer_edges_3[2]_2: true + interlayer_edges_3[2]_4: true + interlayer_edges_4_3: true + layer2_bounding_boxes: true + layer2_nodes: true + layer2_text: true + layer2p97_edges: true + layer2p97_nodes: true + layer2p97_text: true + layer3_edges: true + layer3_nodes: true + layer3p1_edges: true + layer3p1_nodes: true + layer3p1_polygon_boundaries: true + layer3p2_edges: true + layer3p2_nodes: true + layer3p2_region_growing_boundaries: true + layer4_edges: true + layer4_nodes: true + layer4_text: true Topic: Depth: 5 Durability Policy: Transient Local @@ -126,16 +145,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 31.058483123779297 + Distance: 108.03848266601562 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -6.118514060974121 - Y: -0.15275907516479492 - Z: 1.9664970636367798 + X: 4.692053318023682 + Y: 5.916086196899414 + Z: -5.913562774658203 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -149,10 +168,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 846 + Height: 1356 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000004b2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004b2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006ac0000003efc0100000002fb0000000800540069006d00650100000000000006ac0000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000550000004b200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -161,6 +180,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1200 - X: 60 - Y: 60 + Width: 1708 + X: 1724 + Y: 76 diff --git a/hydra_visualizer/src/scene_graph_renderer.cpp b/hydra_visualizer/src/scene_graph_renderer.cpp index d04222d..8bc4073 100644 --- a/hydra_visualizer/src/scene_graph_renderer.cpp +++ b/hydra_visualizer/src/scene_graph_renderer.cpp @@ -235,7 +235,7 @@ YAML::Node SceneGraphRenderer::dumpConfig() const { YAML::Node layer_plugins(YAML::NodeType::Sequence); for (const auto& [key, plugins] : layer_plugins_) { - YAML::Node plugin_configs; + YAML::Node plugin_configs(YAML::NodeType::Sequence); for (const auto& plugin : plugins) { if (!plugin) { continue; @@ -243,6 +243,11 @@ YAML::Node SceneGraphRenderer::dumpConfig() const { plugin_configs.push_back(plugin->dumpConfig()); } + + YAML::Node layer_info; + layer_info["layer"] = LayerKeySelector{key}.str(); + layer_info["plugins"] = plugin_configs; + layer_plugins.push_back(layer_info); } YAML::Node interlayer_edge_configs(YAML::NodeType::Sequence); From 69634ae89cefdc8061a07414dc8e6f31108bc705 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sat, 13 Jun 2026 15:54:19 +0000 Subject: [PATCH 6/8] add options to region growing plugin --- .../plugins/region_growing_boundary_plugin.h | 4 +++- .../region_growing_boundary_plugin.cpp | 23 ++++++++++++++----- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h index c2c6f98..08b8e4f 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h @@ -43,7 +43,9 @@ namespace hydra { class RegionGrowingBoundaryPlugin : public LayerPlugin { public: struct Config { - //! line width of the boundary markers + //! Use node color instead of boundary type + bool use_node_color = false; + //! Line width of the boundary markers float line_width = 0.07f; //! Colors representing each traversability state. std::vector colors{spark_dsg::Color::gray(), // Unknown diff --git a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp index 1b3feb8..519e6d4 100644 --- a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp +++ b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp @@ -53,8 +53,9 @@ static const auto registration_ = std::string>("RegionGrowingBoundaryPlugin"); std_msgs::msg::ColorRGBA makeBoundaryColor(const std::vector& colors, - spark_dsg::TraversabilityState state) { - return visualizer::makeColorMsg(colors.at(static_cast(state))); + spark_dsg::TraversabilityState state, + float alpha) { + return visualizer::makeColorMsg(colors.at(static_cast(state)), alpha); } } // namespace @@ -64,6 +65,7 @@ using visualization_msgs::msg::Marker; void declare_config(RegionGrowingBoundaryPlugin::Config& config) { using namespace config; name("RegionGrowingBoundaryPlugin::Config"); + field(config.use_node_color, "use_node_color"); field(config.colors, "colors"); field(config.line_width, "line_width"); checkCondition(config.colors.size() == 4, "colors.size() must be 4"); @@ -113,10 +115,19 @@ void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, auto end = attrs->getBoundaryPoint(end_idx); end.z() += info.z_offset; tf2::convert(end, marker.points.emplace_back()); - marker.colors.emplace_back( - makeBoundaryColor(config.colors, attrs->states[start_idx])); - marker.colors.emplace_back( - makeBoundaryColor(config.colors, attrs->states[end_idx])); + if (config.use_node_color) { + const auto color = + visualizer::makeColorMsg(info.node_color(*node), info.config.nodes.alpha); + marker.colors.emplace_back(color); + marker.colors.emplace_back(color); + } else { + const auto start_color = makeBoundaryColor( + config.colors, attrs->states[start_idx], info.config.nodes.alpha); + const auto end_color = makeBoundaryColor( + config.colors, attrs->states[end_idx], info.config.nodes.alpha); + marker.colors.emplace_back(start_color); + marker.colors.emplace_back(end_color); + } } } From 32f6495d5ca380d9735f6b9bc7d2811e13405bfa Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sat, 13 Jun 2026 16:36:15 +0000 Subject: [PATCH 7/8] add option to fill boundary polygons --- .../plugins/region_growing_boundary_plugin.h | 4 +++ .../src/plugins/footprint_plugin.cpp | 1 - .../region_growing_boundary_plugin.cpp | 35 +++++++++++++++++-- 3 files changed, 36 insertions(+), 4 deletions(-) diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h index 08b8e4f..97ba5ec 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h @@ -47,6 +47,10 @@ class RegionGrowingBoundaryPlugin : public LayerPlugin { bool use_node_color = false; //! Line width of the boundary markers float line_width = 0.07f; + //! Alpha of polygon fill + float fill_alpha = 0.5f; + //! Fill boundaries + bool fill_boundaries = false; //! Colors representing each traversability state. std::vector colors{spark_dsg::Color::gray(), // Unknown spark_dsg::Color::blue(), // Traversable diff --git a/hydra_visualizer/src/plugins/footprint_plugin.cpp b/hydra_visualizer/src/plugins/footprint_plugin.cpp index bab2bed..6268ca7 100644 --- a/hydra_visualizer/src/plugins/footprint_plugin.cpp +++ b/hydra_visualizer/src/plugins/footprint_plugin.cpp @@ -57,7 +57,6 @@ static const auto registration_ = std::string>("FootprintPlugin"); } -using spark_dsg::DsgLayers; using spark_dsg::DynamicSceneGraph; using spark_dsg::LayerId; using spark_dsg::PlaceNodeAttributes; diff --git a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp index 519e6d4..a992a10 100644 --- a/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp +++ b/hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp @@ -42,6 +42,7 @@ #include #include "hydra_visualizer/color/color_parsing.h" +#include "hydra_visualizer/utils/polygon_utilities.h" namespace hydra { namespace { @@ -68,8 +69,11 @@ void declare_config(RegionGrowingBoundaryPlugin::Config& config) { field(config.use_node_color, "use_node_color"); field(config.colors, "colors"); field(config.line_width, "line_width"); + field(config.fill_alpha, "fill_alpha"); + field(config.fill_boundaries, "fill_boundaries"); checkCondition(config.colors.size() == 4, "colors.size() must be 4"); check(config.line_width, GT, 0.0f, "line_width"); + checkInRange(config.fill_alpha, 0.0f, 1.0f, "fill_alpha", false); } RegionGrowingBoundaryPlugin::RegionGrowingBoundaryPlugin(const Config& config, @@ -96,28 +100,44 @@ void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, marker.scale.x = config.line_width; marker.scale.y = config.line_width; marker.scale.z = config.line_width; + + Marker fill_marker; + fill_marker.header = header; + fill_marker.ns = ns_ + "_region_growing_polygons"; + fill_marker.id = 0; + fill_marker.type = Marker::TRIANGLE_LIST; + fill_marker.action = Marker::ADD; + fill_marker.pose.orientation.w = 1.0; + fill_marker.color.a = config.fill_alpha; + fill_marker.scale.x = 1.0; + fill_marker.scale.y = 1.0; + fill_marker.scale.z = 1.0; + for (const auto& [node_id, node] : layer.nodes()) { if (info.filter && !info.filter(*node)) { continue; } auto attrs = node->tryAttributes(); - if (!attrs) { + if (!attrs || attrs->radii.empty()) { continue; } + Eigen::MatrixXd points(3, attrs->radii.size()); + const auto color = + visualizer::makeColorMsg(info.node_color(*node), info.config.nodes.alpha); for (size_t i = 1; i <= attrs->radii.size(); ++i) { const auto start_idx = i - 1; const auto end_idx = i % attrs->radii.size(); auto start = attrs->getBoundaryPoint(start_idx); start.z() += info.z_offset; + points.col(start_idx) = start; + tf2::convert(start, marker.points.emplace_back()); auto end = attrs->getBoundaryPoint(end_idx); end.z() += info.z_offset; tf2::convert(end, marker.points.emplace_back()); if (config.use_node_color) { - const auto color = - visualizer::makeColorMsg(info.node_color(*node), info.config.nodes.alpha); marker.colors.emplace_back(color); marker.colors.emplace_back(color); } else { @@ -129,9 +149,18 @@ void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header, marker.colors.emplace_back(end_color); } } + + if (config.fill_boundaries) { + auto mesh_color = color; + mesh_color.a = config.fill_alpha; + makeFilledPolygon(points, mesh_color, fill_marker); + } } tracker.add(marker, msg); + if (config.fill_boundaries) { + tracker.add(fill_marker, msg); + } } YAML::Node RegionGrowingBoundaryPlugin::dumpConfig() const { From b3f9f640cfdbba140c275e076582a602e38a47e7 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sat, 13 Jun 2026 16:38:28 +0000 Subject: [PATCH 8/8] fix linting --- hydra_visualizer/include/hydra_visualizer/visualizer_node.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hydra_visualizer/include/hydra_visualizer/visualizer_node.h b/hydra_visualizer/include/hydra_visualizer/visualizer_node.h index e03f44e..e95a584 100644 --- a/hydra_visualizer/include/hydra_visualizer/visualizer_node.h +++ b/hydra_visualizer/include/hydra_visualizer/visualizer_node.h @@ -38,8 +38,8 @@ #include #include -#include #include +#include #include "hydra_visualizer/io/graph_wrapper.h" #include "hydra_visualizer/plugins/visualizer_plugin.h"