Feature/dynamic configs

config_utilities/CMakeLists.txt

@@ -27,11 +27,13 @@ add_library(
   ${PROJECT_NAME}
   src/asl_formatter.cpp
   src/conversions.cpp
+  src/dynamic_config.cpp
   src/external_registry.cpp
   src/factory.cpp
   src/formatter.cpp
-  src/logger.cpp
+  src/field_input_info.cpp
   src/log_to_stdout.cpp
+  src/logger.cpp
   src/meta_data.cpp
   src/namespacing.cpp
   src/path.cpp
@@ -40,7 +42,9 @@ add_library(
   src/validation.cpp
   src/visitor.cpp
   src/yaml_parser.cpp
-  src/yaml_utils.cpp)
+  src/yaml_utils.cpp
+  $<$<BOOL:${ENABLE_roscpp}>:src/ros.cpp>
+  )
 target_link_libraries(
   ${PROJECT_NAME}
   PUBLIC yaml-cpp

config_utilities/demos/CMakeLists.txt

@@ -13,7 +13,10 @@ if(ENABLE_Eigen3 AND ENABLE_roscpp)
   add_executable(demo_ros demo_ros.cpp)
   target_link_libraries(demo_ros ${PROJECT_NAME})
 
+  add_executable(demo_dynamic_config_server demo_dynamic_config_server.cpp)
+  target_link_libraries(demo_dynamic_config_server ${PROJECT_NAME})
+
   include(GNUInstallDirs)
-  install(TARGETS demo_ros
+  install(TARGETS demo_ros demo_dynamic_config_server
           RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME})
 endif()

config_utilities/demos/demo_config.cpp

@@ -173,7 +173,7 @@ int main(int argc, char** argv) {
   const std::string my_root_path = std::string(argv[1]) + "/";
 
   // GLobal settings can be set at runtime to change the behavior and presentation of configs.
-  config::Settings().inline_subconfig_field_names = true;
+  config::Settings().printing.inline_subconfigs = true;
 
   // ===================================== Checking whether a struct is a config =====================================
   std::cout << "\n\n----- Checking whether a struct is a config -----\n\n" << std::endl;

config_utilities/demos/demo_dynamic_config.launch

@@ -0,0 +1,7 @@
+<launch>
+    <node pkg="config_utilities" type="demo_dynamic_config_server" name="demo_dynamic_config_server" output="screen" required="true">
+    </node>
+
+    <node pkg="config_utilities" type="demo_dynamic_config_client.py" name="demo_dynamic_config_client" output="screen" required="true">
+    </node>
+</launch>

config_utilities/demos/demo_dynamic_config_client.py

@@ -0,0 +1,166 @@
+#!/usr/bin/env python3
+import rospy
+from std_msgs.msg import String
+import yaml
+from time import sleep
+from dynamic_config_gui import DynamicConfigGUI
+
+APP_NAME = "[Config Utilities Dynamic Config Client] "
+
+
+class DynamicConfigRosClient:
+    """
+    A ROS client for the dynamic config GUI. This class connects to a dynamic config server
+    and allows the user to interact with the server's configuration through YAML.
+    """
+
+    def __init__(self):
+        # Setup the GUI.
+        self.gui = DynamicConfigGUI()
+        self.gui.value_changed_cb = self.send_config
+        self.gui.key_selected_cb = self.key_selected_cb
+        self.gui.server_selected_cb = self.server_selected_cb
+        self.gui.server_selection.refresh_cb = self.refresh_servers
+
+        # Variables.
+        self.listening_ns = ""
+        self.last_values_received = ""
+        self.last_info_received = ""
+
+        # ROS.
+        self.config_pub = None
+        self.config_sub = None
+        self.config_info_sub = None
+        self.reg_sub = None
+        self.dereg_sub = None
+
+        self.initialize()
+
+    def initialize(self):
+        servers = self.get_available_servers()
+        if len(servers) == 0:
+            print(
+                f"{APP_NAME}Waiting for ROS Dynamic Config Servers to register..."
+            )
+            while len(servers) == 0:
+                sleep(0.1)
+                servers = self.get_available_servers()
+
+        # This will also selected a server and trigger the connection.
+        self.gui.set_servers(servers)
+        print(f"{APP_NAME}Connected to server '{self.listening_ns}'.")
+
+    def subscriber_cb(self, msg):
+        try:
+            values = yaml.load(msg.data, Loader=yaml.FullLoader)
+        except yaml.YAMLError as e:
+            print(f"{APP_NAME}Error parsing incoming message YAML: {e}")
+            return
+        self.last_values_received = values
+        self.gui.set_config(values)
+
+    def info_sub_cb(self, msg):
+        try:
+            values = yaml.load(msg.data, Loader=yaml.FullLoader)
+        except yaml.YAMLError as e:
+            print(f"{APP_NAME}Error parsing incoming message YAML: {e}")
+            return
+        self.last_info_received = values
+        self.gui.set_config_info(values)
+
+    def reg_cb(self, _):
+        # Instead of incremental tracking just update the configs.
+        self.gui.set_keys(self.get_available_keys())
+
+    def key_selected_cb(self, key):
+        if key is not None:
+            self.connect_topic(key)
+
+    def server_selected_cb(self, server):
+        servers = self.get_available_servers()
+        if server not in servers:
+            print(f"{APP_NAME}Server '{server}' not available.")
+            self.gui.set_servers(servers)
+            return
+        self.connect_server(server)
+
+    def send_config(self, new_values):
+        if self.config_pub is None:
+            return
+        msg = String()
+        msg.data = yaml.dump(new_values, default_flow_style=False)
+        self.config_pub.publish(msg)
+
+    def refresh_servers(self):
+        previous_server = self.gui.current_server
+        previous_key = self.gui.current_key
+        servers = self.get_available_servers()
+        self.gui.set_servers(servers)
+        if (previous_server != self.gui.current_server
+                or self.gui.current_key != previous_key):
+            self.last_values_received = ""
+            self.last_info_received = ""
+            return
+        if self.last_info_received != "":
+            self.gui.set_config_info(self.last_info_received)
+        elif self.last_values_received != "":
+            self.gui.set_config(self.last_values_received)
+
+    def connect_topic(self, key):
+        if self.config_pub is not None:
+            self.config_pub.unregister()
+            self.config_sub.unregister()
+            self.config_info_sub.unregister()
+        self.config_pub = rospy.Publisher(f"{self.listening_ns}/{key}/set",
+                                          String,
+                                          queue_size=10)
+        self.config_sub = rospy.Subscriber(f"{self.listening_ns}/{key}/get",
+                                           String, self.subscriber_cb)
+        self.config_info_sub = rospy.Subscriber(
+            f"{self.listening_ns}/{key}/info", String, self.info_sub_cb)
+
+    def connect_server(self, server):
+        self.listening_ns = server
+        self.reg_sub = rospy.Subscriber(f"{self.listening_ns}/registered",
+                                        String, self.reg_cb)
+        self.dereg_sub = rospy.Subscriber(f"{self.listening_ns}/deregistered",
+                                          String, self.reg_cb)
+        self.gui.set_keys(self.get_available_keys())
+
+    def get_available_servers(self):
+        topics = rospy.get_published_topics()
+        topics = [
+            topic[0] for topic in topics if topic[1] == "std_msgs/String"
+        ]
+        # We use the queue that all servers advertise these topics.
+        reg = [t[:-11] for t in topics if t.endswith("/registered")]
+        dereg = [t[:-13] for t in topics if t.endswith("/deregistered")]
+        return [t for t in reg if t in dereg]
+
+    def get_available_keys(self):
+        topics = rospy.get_published_topics()
+        topics = [t[0] for t in topics if t[1] == "std_msgs/String"]
+        topics = [
+            t[len(self.listening_ns) + 1:] for t in topics
+            if t.startswith(self.listening_ns)
+        ]
+        return [t[:-4] for t in topics if t.endswith("/get")]
+
+    def spin(self):
+        self.gui.mainloop()
+
+
+def on_shutdown(client: DynamicConfigRosClient):
+    # Force ctk shutdown.
+    client.gui.quit()
+
+
+def main():
+    rospy.init_node("dynamic_config_client")
+    client = DynamicConfigRosClient()
+    rospy.on_shutdown(lambda: on_shutdown(client))
+    client.spin()
+
+
+if __name__ == "__main__":
+    main()

config_utilities/demos/demo_dynamic_config_server.cpp

@@ -0,0 +1,151 @@
+/** -----------------------------------------------------------------------------
+ * Copyright (c) 2023 Massachusetts Institute of Technology.
+ * All Rights Reserved.
+ *
+ * AUTHORS:     Lukas Schmid <[email protected]>, Nathan Hughes <[email protected]>
+ * AFFILIATION: MIT-SPARK Lab, Massachusetts Institute of Technology
+ * YEAR:        2023
+ * LICENSE:     BSD 3-Clause
+ *
+ * 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.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * 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.
+ * -------------------------------------------------------------------------- */
+
+/**
+ * This demo shows how to use config_utilities with ROS.
+ */
+
+#include <iostream>
+#include <string>
+
+#include <ros/ros.h>
+
+#include "config_utilities/config.h"                 // Enables declare_config().
+#include "config_utilities/dynamic_config.h"         // Enables DynamicConfig and DynamicConfigServer.
+#include "config_utilities/logging/log_to_stdout.h"  // Log config_utilities messages.
+#include "config_utilities/parsing/ros.h"            // Enable fromRos() and the RosDynamicConfigServer.
+#include "config_utilities/printing.h"               // Enable toString()
+#include "config_utilities/types/eigen_matrix.h"     // Enable parsing and printing of Eigen::Matrix types.
+#include "config_utilities/types/enum.h"             // Enable parsing and printing of enum types.
+
+namespace demo {
+
+// A sub-struct for later use.
+struct SubConfig {
+  float f = 1.1f;
+  std::string s = "test";
+};
+
+// A struct that represents what we want to be a config.
+// Requirements for a config struct: is default constructable.
+struct MyConfig {
+  int i = 100;
+  double distance = 42;
+  bool b = true;
+  std::vector<int> vec = {1, 2, 3};
+  std::map<std::string, int> map = {{"a", 1}, {"b", 2}, {"c", 3}};
+  Eigen::Matrix<double, 3, 3> mat = Eigen::Matrix<double, 3, 3>::Identity();
+  enum class MyEnum { kA, kB, kC } my_enum = MyEnum::kA;
+  SubConfig sub_config;
+};
+
+// Defining 'void declare_config(T& config)' function labels a struct as config.
+// It **MUST** be declared beforehand if being used in another declare_config
+void declare_config(SubConfig&);
+
+// All config properties are specified within declare_config.
+void declare_config(MyConfig& config) {
+  config::name("MyConfig");
+  config::field(config.i, "i");
+  config::field(config.distance, "distance", "m");
+  config::field(config.b, "b");
+  config::field(config.vec, "vec");
+  config::field(config.map, "map");
+  config::field(config.mat, "mat");
+  config::enum_field(config.my_enum, "my_enum", {"A", "B", "C"});
+  config::field(config.sub_config, "sub_config");
+
+  config::check(config.i, config::CheckMode::GT, 0, "i");
+  config::checkInRange(config.distance, 0.0, 100.0, "distance");
+}
+
+// Declaration of the subconfig.
+void declare_config(SubConfig& config) {
+  using namespace config;
+  name("SubConfig");
+  field(config.f, "f");
+  field(config.s, "s");
+  checkIsOneOf(config.f, {0.0f, 1.1f, 2.2f, 3.3f}, "f");
+}
+
+// Declare an object with a dynamic config.
+template <typename ConfigT>
+class DynamicConfigObject {
+ public:
+  explicit DynamicConfigObject(const std::string& name, const ConfigT& initial_config)
+      : name_(name), config_(name, initial_config) {
+    // The above initialization registers the dynamic config with its global identifier name, which we resolve using the
+    // nodehandle to get a unique name for every node/object. The config is initialized with the current ROS parameters.
+    // The callback is called whenever the config is updated.
+    config_.setCallback(std::bind(&DynamicConfigObject::callback, this));
+  }
+
+ private:
+  const std::string name_;
+  config::DynamicConfig<ConfigT> config_;
+
+  void callback() const {
+    // Do something with the new config.
+    std::cout << "Received new config for " << name_ << ":\n" << config::toString(config_.get()) << std::endl;
+  }
+};
+
+}  // namespace demo
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "dynamic_config_server");
+  ros::NodeHandle nh("~");
+
+  // Advertize setting and getting dynamic configs via ros topics.
+  config::RosDynamicConfigServer server(nh);
+
+  // Create dynamic config objects. These will automatically register their config with the server.
+  demo::DynamicConfigObject obj("dynamic_object_config", demo::MyConfig());
+
+  // Initialize another config with different name and params.
+  nh.setParam("i", 42);
+  nh.setParam("distance", 42.0);
+  nh.setParam("b", false);
+  nh.setParam("vec", std::vector<int>());
+  nh.setParam("map", std::map<std::string, int>({{"ASD", 42}}));
+  demo::DynamicConfigObject obj2("another_config", config::fromRos<demo::MyConfig>(nh));
+
+  // Initialize a subconfig.
+  demo::DynamicConfigObject sub_obj("sub_config", demo::SubConfig());
+
+  // Spin to keep the node alive.
+  ros::spin();
+  return 0;
+}

config_utilities/demos/demo_factory.cpp

@@ -232,7 +232,7 @@ int main(int argc, char** argv) {
   std::cout << "\n\n----- Creating objects from file -----\n\n" << std::endl;
 
   // Optionally specify the name of the type-identifying param. Default is 'type'.
-  config::Settings().factory_type_param_name = "type";
+  config::Settings().factory.type_param_name = "type";
 
   // Create an object of type and with config as specified in a file.
   object = config::createFromYamlFile<demo::Base>(my_root_path + "factory.yaml", 123);

config_utilities/demos/demo_inheritance.cpp

@@ -144,7 +144,7 @@ int main(int argc, char** argv) {
 
   const std::string my_root_path = std::string(argv[1]) + "/";
 
-  config::Settings().inline_subconfig_field_names = true;
+  config::Settings().printing.inline_subconfigs = true;
 
   // ===================================== Checking whether a struct is a config =====================================