Feature/save viz config

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

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,

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<visualization_msgs::msg::MarkerArray>::SharedPtr pub_;
   MarkerTracker tracker_;

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,

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;
 };

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> config_;
 

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> config_;

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,

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<geometry_msgs::msg::PoseArray>::SharedPtr pub_;

hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h

@@ -43,8 +43,14 @@ 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;
+    //! Alpha of polygon fill
+    float fill_alpha = 0.5f;
+    //! Fill boundaries
+    bool fill_boundaries = false;
     //! Colors representing each traversability state.
     std::vector<spark_dsg::Color> colors{spark_dsg::Color::gray(),    // Unknown
                                          spark_dsg::Color::blue(),    // Traversable
@@ -63,6 +69,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> config_;

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,

hydra_visualizer/include/hydra_visualizer/plugins/visualizer_plugin.h

@@ -34,6 +34,7 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 #include <spark_dsg/dynamic_scene_graph.h>
+#include <yaml-cpp/node/node.h>
 
 #include <std_msgs/msg/header.hpp>
 
@@ -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:

hydra_visualizer/include/hydra_visualizer/scene_graph_renderer.h

@@ -106,6 +106,8 @@ class SceneGraphRenderer {
 
   virtual void clearChangeFlag();
 
+  YAML::Node dumpConfig() const;
+
  protected:
   virtual void setConfigs(const spark_dsg::DynamicSceneGraph& graph) const;
 
@@ -138,7 +140,13 @@ class SceneGraphRenderer {
   mutable std::atomic<bool> has_change_;
   mutable std::map<spark_dsg::LayerKey, visualizer::LayerInfo> layer_infos_;
   mutable std::map<spark_dsg::LayerKey, std::unique_ptr<LayerConfigWrapper>> layers_;
-  mutable std::map<std::string, std::unique_ptr<EdgeConfigWrapper>> interlayer_edges_;
+
+  struct EdgeConfigInfo {
+    spark_dsg::LayerKey parent;
+    spark_dsg::LayerKey child;
+    std::unique_ptr<EdgeConfigWrapper> config;
+  };
+  mutable std::map<std::string, EdgeConfigInfo> interlayer_edges_;
 };
 
 void declare_config(SceneGraphRenderer::LayerPluginsConfig& config);

hydra_visualizer/include/hydra_visualizer/visualizer_node.h

@@ -38,6 +38,7 @@
 #include <config_utilities_ros/ros_dynamic_config_server.h>
 #include <ianvs/node_handle.h>
 
+#include <std_msgs/msg/string.hpp>
 #include <std_srvs/srv/empty.hpp>
 
 #include "hydra_visualizer/io/graph_wrapper.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<std_srvs::srv::Empty>::SharedPtr redraw_service_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_service_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr save_sub_;
 };
 
 void declare_config(DsgVisualizer::Config& config);

hydra_visualizer/launch/static_visualizer.launch.yaml

@@ -5,9 +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: 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}
+  - 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
@@ -16,9 +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 visualizer_plugins_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

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)}}

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

hydra_visualizer/src/plugins/basis_point_plugin.cpp

@@ -36,6 +36,7 @@
 
 #include <config_utilities/config_utilities.h>
 #include <config_utilities/factory.h>
+#include <config_utilities/parsing/yaml.h>
 #include <config_utilities/printing.h>
 #include <config_utilities/validation.h>
 #include <glog/logging.h>
@@ -244,4 +245,10 @@ void BasisPointPlugin::drawBasisPoints(const Config& config,
   tracker_.add(marker, msg);
 }
 
+YAML::Node BasisPointPlugin::dumpConfig() const {
+  auto root = config::toYaml(config_.get());
+  root["type"] = "BasisPointPlugin";
+  return root;
+}
+
 }  // namespace hydra

hydra_visualizer/src/plugins/footprint_plugin.cpp

@@ -36,6 +36,7 @@
 
 #include <config_utilities/config.h>
 #include <config_utilities/factory.h>
+#include <config_utilities/parsing/yaml.h>
 #include <config_utilities/validation.h>
 #include <glog/logging.h>
 #include <spark_dsg/node_attributes.h>
@@ -56,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;
@@ -174,4 +174,10 @@ void FootprintPlugin::reset(const std_msgs::msg::Header& header) {
   }
 }
 
+YAML::Node FootprintPlugin::dumpConfig() const {
+  auto root = config::toYaml(config);
+  root["type"] = "FootprintPlugin";
+  return root;
+}
+
 }  // namespace hydra