Feature/save viz config (#69)

* start on code to save current config

* move config dumping to visualizer node and add plugins

* export plugin type when dumping config

* export entire config

* actually export layer plugin configs

* add options to region growing plugin

* add option to fill boundary polygons

* fix linting

Author: nathanhhughes

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/config/visualizer_plugins.yaml

@@ -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/basis_point_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/footprint_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/khronos_object_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/layer_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_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/mesh_point_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/places_freespace_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_;