Feature/dynamic configs (#23)

* use new infrastructure to set config_utilities settings

* actually try and use dynamic configs instead of custom config wrapper

* fix config bug and dynamic config namespacing

* fix duplicated config names

* fix path resolution and add gui to launch

Author: nathanhhughes

hydra_ros/app/hydra_node.cpp

@@ -50,12 +50,6 @@ struct RunSettings {
   size_t robot_id = 0;
   bool exit_after_clock = false;
   bool force_shutdown = false;
-  size_t print_width = 100;
-  size_t print_indent = 45;
-  bool print_missing = false;
-  bool allow_plugins = true;
-  bool verbose_plugins = false;
-  bool trace_plugin_allocations = false;
   std::vector<std::string> paths;
   int config_verbosity = 1;
   bool forward_glog_to_ros = true;
@@ -71,12 +65,6 @@ void declare_config(RunSettings& config) {
   field(config.robot_id, "robot_id");
   field(config.exit_after_clock, "exit_after_clock");
   field(config.force_shutdown, "force_shutdown");
-  field(config.print_width, "print_width");
-  field(config.print_indent, "print_indent");
-  field(config.print_missing, "print_missing");
-  field(config.allow_plugins, "allow_plugins");
-  field(config.verbose_plugins, "verbose_plugins");
-  field(config.trace_plugin_allocations, "trace_plugin_allocations");
   field(config.paths, "paths");
   field(config.config_verbosity, "config_verbosity");
   field(config.forward_glog_to_ros, "forward_glog_to_ros");
@@ -123,6 +111,7 @@ struct RosSink : google::LogSink {
 
 int main(int argc, char* argv[]) {
   config::initContext(argc, argv, true);
+  config::setConfigSettingsFromContext();
   rclcpp::init(argc, argv);
 
   const auto settings = config::fromContext<hydra::RunSettings>();
@@ -144,12 +133,6 @@ int main(int argc, char* argv[]) {
   }
 
   config::Settings().setLogger("glog");
-  config::Settings().print_width = settings.print_width;
-  config::Settings().print_indent = settings.print_indent;
-  config::Settings().print_missing = settings.print_missing;
-  config::Settings().allow_external_libraries = settings.allow_plugins;
-  config::Settings().verbose_external_load = settings.verbose_plugins;
-  config::Settings().print_external_allocations = settings.trace_plugin_allocations;
   [[maybe_unused]] const auto plugins = config::loadExternalFactories(settings.paths);
 
   ianvs::NodeHandle nh(*node);

hydra_ros/config/hydra_ros_visualizer_plugins.yaml

@@ -1,5 +1,3 @@
 ---
-external_plugins:
-  allow_plugins: true
-  trace_plugin_allocations: true
-  paths: [hydra_ros]
+external_libraries: {enabled: true}
+external_library_paths: [hydra_ros]

hydra_ros/include/hydra_ros/frontend/places_visualizer.h

@@ -33,10 +33,10 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
+#include <config_utilities/dynamic_config.h>
 #include <hydra/frontend/gvd_place_extractor.h>
 #include <hydra_visualizer/color/colormap_utilities.h>
 #include <hydra_visualizer/layer_info.h>
-#include <hydra_visualizer/utils/config_wrapper.h>
 #include <hydra_visualizer/utils/marker_group_pub.h>
 #include <ianvs/node_handle.h>
 
@@ -77,8 +77,8 @@ class PlacesVisualizer : public GvdPlaceExtractor::Sink {
  protected:
   ianvs::NodeHandle nh_;
   MarkerGroupPub pubs_;
-  visualizer::ConfigWrapper<GvdVisualizerConfig> gvd_config_;
-  visualizer::ConfigWrapper<visualizer::LayerConfig> layer_config_;
+  config::DynamicConfig<GvdVisualizerConfig> gvd_config_;
+  config::DynamicConfig<visualizer::LayerConfig> layer_config_;
   const visualizer::RangeColormap colormap_;
 
  private:

hydra_ros/src/frontend/places_visualizer.cpp

@@ -72,8 +72,8 @@ PlacesVisualizer::PlacesVisualizer(const Config& config)
     : config(config),
       nh_(getHydraNodeHandle(config.ns)),
       pubs_(nh_),
-      gvd_config_(nh_, "gvd"),
-      layer_config_(nh_, "graph"),
+      gvd_config_("gvd"),
+      layer_config_("graph"),
       colormap_(config.colormap) {}
 
 std::string PlacesVisualizer::printInfo() const { return config::toString(config); }

hydra_visualizer/CMakeLists.txt

@@ -13,6 +13,7 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 
 find_package(ament_cmake REQUIRED)
 find_package(config_utilities REQUIRED)
+find_package(config_utilities_ros REQUIRED)
 find_package(glog REQUIRED)
 find_package(gflags REQUIRED)
 find_package(hydra_msgs REQUIRED)
@@ -59,6 +60,7 @@ target_include_directories(
 target_link_libraries(
   ${PROJECT_NAME}
   PUBLIC config_utilities::config_utilities
+         config_utilities_ros::config_utilities_ros
          ianvs::ianvs
          rclcpp::rclcpp
          spark_dsg::spark_dsg
@@ -76,12 +78,22 @@ ament_target_dependencies(
 )
 
 add_executable(hydra_visualizer_node app/hydra_visualizer_node.cpp)
-target_link_libraries(hydra_visualizer_node ${PROJECT_NAME} gflags glog::glog)
+target_link_libraries(
+  hydra_visualizer_node
+  ${PROJECT_NAME}
+  gflags
+  glog::glog
+)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest)
   ament_add_gtest(test_${PROJECT_NAME} tests/main.cpp tests/test_ear_clipping.cpp)
-  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gflags glog::glog)
+  target_link_libraries(
+    test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+    gflags
+    glog::glog
+  )
 endif()
 
 install(
@@ -100,6 +112,7 @@ install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
 ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   config_utilities
+  config_utilities_ros
   hydra_msgs
   ianvs
   kimera_pgmo_msgs

hydra_visualizer/app/hydra_visualizer_node.cpp

@@ -46,40 +46,26 @@
 #include "hydra_visualizer/visualizer_node.h"
 
 namespace hydra::visualizer {
-struct ExternalPluginConfig {
-  bool allow_plugins = true;
-  bool verbose_plugins = false;
-  bool trace_plugin_allocations = false;
-  std::vector<std::string> paths;
-};
 
 struct NodeSettings {
-  ExternalPluginConfig external_plugins;
   int glog_verbosity = 1;
   int glog_level = 0;
+  std::vector<std::string> external_library_paths;
 };
 
-void declare_config(ExternalPluginConfig& config) {
-  using namespace config;
-  name("ExternalPluginConfig");
-  field(config.allow_plugins, "allow_plugins");
-  field(config.verbose_plugins, "verbose_plugins");
-  field(config.trace_plugin_allocations, "trace_plugin_allocations");
-  field(config.paths, "paths");
-}
-
 void declare_config(NodeSettings& config) {
   using namespace config;
   name("NodeSettings");
-  field(config.external_plugins, "external_plugins");
   field(config.glog_verbosity, "glog_verbosity");
   field(config.glog_level, "glog_level");
+  field(config.external_library_paths, "external_library_paths");
 }
 
 }  // namespace hydra::visualizer
 
 int main(int argc, char** argv) {
   config::initContext(argc, argv, true);
+  config::setConfigSettingsFromContext();
   rclcpp::init(argc, argv);
 
   const auto node_settings = config::fromContext<hydra::visualizer::NodeSettings>();
@@ -96,14 +82,8 @@ int main(int argc, char** argv) {
 
   VLOG(1) << "Settings:\n" << config::toString(node_settings);
 
-  auto& settings = config::Settings();
-  settings.allow_external_libraries = node_settings.external_plugins.allow_plugins;
-  settings.verbose_external_load = node_settings.external_plugins.verbose_plugins;
-  settings.print_external_allocations =
-      node_settings.external_plugins.trace_plugin_allocations;
   [[maybe_unused]] const auto plugins =
-      config::loadExternalFactories(node_settings.external_plugins.paths);
-
+      config::loadExternalFactories(node_settings.external_library_paths);
 
   rclcpp::executors::MultiThreadedExecutor executor;
   {  // start visualizer scope

hydra_visualizer/config/external_plugins.yaml

@@ -1,3 +1,2 @@
 ---
-external_plugins:
-  allow_plugins: false
+external_libraries: {enabled: false}

hydra_visualizer/include/hydra_visualizer/layer_info.h

@@ -153,13 +153,13 @@ class LayerInfo {
       std::function<spark_dsg::Color(const spark_dsg::SceneGraphNode&)>;
   using TextFunction = std::function<std::string(const spark_dsg::SceneGraphNode&)>;
 
-  LayerInfo(const LayerConfig& config);
+  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);
 
   bool shouldVisualize(const spark_dsg::SceneGraphNode& node) const;
 
-  const LayerConfig& config;
+  const LayerConfig config;
 
   double z_offset;
   ColorFunction node_color;

hydra_visualizer/include/hydra_visualizer/plugins/basis_point_plugin.h

@@ -34,16 +34,16 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 
+#include <config_utilities/dynamic_config.h>
 #include <ianvs/node_handle.h>
 
 #include <rclcpp/publisher.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include "hydra_visualizer/color/colormap_utilities.h"
+#include "hydra_visualizer/layer_info.h"
 #include "hydra_visualizer/plugins/visualizer_plugin.h"
-#include "hydra_visualizer/utils/config_wrapper.h"
 #include "hydra_visualizer/utils/marker_tracker.h"
-#include "hydra_visualizer/layer_info.h"
 
 namespace hydra {
 
@@ -58,7 +58,9 @@ class BasisPointPlugin : public VisualizerPlugin {
     double basis_point_scale = 0.1;
     double basis_point_alpha = 0.8;
     visualizer::CategoricalColormap::Config colormap;
-  } const config;
+    visualizer::LayerConfig graph;
+  };
+
   BasisPointPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name);
 
   virtual ~BasisPointPlugin() = default;
@@ -73,22 +75,21 @@ class BasisPointPlugin : public VisualizerPlugin {
                    const spark_dsg::DynamicSceneGraph& graph,
                    visualization_msgs::msg::MarkerArray& msg) const;
 
-  void drawNodes(const std_msgs::msg::Header& header,
-                 const spark_dsg::DynamicSceneGraph& graph,
-                 visualization_msgs::msg::MarkerArray& msg) const;
-
-  void drawEdges(const std_msgs::msg::Header& header,
+  void drawEdges(const Config& config,
+                 const visualizer::CategoricalColormap& colormap,
+                 const std_msgs::msg::Header& header,
                  const spark_dsg::DynamicSceneGraph& graph,
                  visualization_msgs::msg::MarkerArray& msg) const;
 
-  void drawBasisPoints(const std_msgs::msg::Header& header,
+  void drawBasisPoints(const Config& config,
+                       const visualizer::CategoricalColormap& colormap,
+                       const std_msgs::msg::Header& header,
                        const spark_dsg::DynamicSceneGraph& graph,
                        visualization_msgs::msg::MarkerArray& msg) const;
 
   mutable MarkerTracker tracker_;
+  config::DynamicConfig<Config> config_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_;
-  visualizer::ConfigWrapper<visualizer::LayerConfig> layer_config_;
-  const visualizer::CategoricalColormap colormap_;
 };
 
 void declare_config(BasisPointPlugin::Config& config);

hydra_visualizer/include/hydra_visualizer/plugins/places_freespace_plugin.h

@@ -33,15 +33,14 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
+#include <config_utilities/dynamic_config.h>
 #include <ianvs/node_handle.h>
 
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include "hydra_visualizer/color/colormap_utilities.h"
+#include "hydra_visualizer/layer_info.h"
 #include "hydra_visualizer/plugins/visualizer_plugin.h"
-#include "hydra_visualizer/utils/config_wrapper.h"
 #include "hydra_visualizer/utils/marker_tracker.h"
-#include "hydra_visualizer/layer_info.h"
 
 namespace hydra {
 
@@ -51,7 +50,8 @@ class PlacesFreespacePlugin : public VisualizerPlugin {
     bool draw_edges = true;
     spark_dsg::Color sphere_color{spark_dsg::Color::red()};
     double sphere_alpha = 0.5;
-  } const config;
+    visualizer::LayerConfig graph;
+  };
 
   PlacesFreespacePlugin(const Config& config,
                         ianvs::NodeHandle nh,
@@ -69,13 +69,14 @@ class PlacesFreespacePlugin : public VisualizerPlugin {
                    const spark_dsg::DynamicSceneGraph& graph,
                    visualization_msgs::msg::MarkerArray& msg) const;
 
-  void drawSpheres(const std_msgs::msg::Header& header,
+  void drawSpheres(const Config& config,
+                   const std_msgs::msg::Header& header,
                    const spark_dsg::SceneGraphLayer& places,
                    visualization_msgs::msg::MarkerArray& msg) const;
 
   mutable MarkerTracker tracker_;
+  config::DynamicConfig<Config> config_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_;
-  visualizer::ConfigWrapper<visualizer::LayerConfig> layer_config_;
 };
 
 void declare_config(PlacesFreespacePlugin::Config& config);