[Draft]Add structured parameter support

rclcpp/CMakeLists.txt

@@ -23,6 +23,8 @@ find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
 find_package(statistics_msgs REQUIRED)
 find_package(tracetools REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 # TODO(wjwwood): remove this when gtest can build on its own, when using target_compile_features()
 # Default to C++17
@@ -218,6 +220,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
   ${statistics_msgs_TARGETS}
   tracetools::tracetools
   ${CMAKE_THREAD_LIBS_INIT}
+  yaml-cpp::yaml-cpp
 )
 
 target_link_libraries(${PROJECT_NAME} PRIVATE
@@ -260,6 +263,8 @@ ament_export_dependencies(
   rosidl_typesupport_cpp
   statistics_msgs
   tracetools
+  yaml_cpp_vendor
+  yaml-cpp
 )
 
 if(BUILD_TESTING)

rclcpp/include/rclcpp/parameter.hpp

@@ -156,6 +156,15 @@ class Parameter
   const std::string &
   as_string() const;
 
+  /// Get value of parameter as a YAML object.
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
+  RCLCPP_PUBLIC
+  const YAML::Node
+  as_yaml() const;
+
+
   /// Get value of parameter as byte array (vector<uint8_t>).
   /**
    * \throws rclcpp::ParameterTypeException if the type doesn't match

rclcpp/include/rclcpp/parameter_value.hpp

@@ -26,6 +26,7 @@
 #include "rcl_interfaces/msg/parameter_value.hpp"
 #include "rclcpp/exceptions/exceptions.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "yaml-cpp/yaml.h"
 
 namespace rclcpp
 {
@@ -37,6 +38,7 @@ enum ParameterType : uint8_t
   PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
   PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
   PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
+  PARAMETER_YAML = rcl_interfaces::msg::ParameterType::PARAMETER_YAML,
   PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
   PARAMETER_BOOL_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
   PARAMETER_INTEGER_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
@@ -99,6 +101,9 @@ class ParameterValue
   /// Construct a parameter value with type PARAMETER_STRING.
   RCLCPP_PUBLIC
   explicit ParameterValue(const char * string_value);
+  /// Construct a parameter value with type PARAMETER_YAML
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const YAML::Node & yaml_value);
   /// Construct a parameter value with type PARAMETER_BYTE_ARRAY.
   RCLCPP_PUBLIC
   explicit ParameterValue(const std::vector<uint8_t> & byte_array_value);
@@ -121,6 +126,16 @@ class ParameterValue
   RCLCPP_PUBLIC
   explicit ParameterValue(const std::vector<std::string> & string_array_value);
 
+
+  /// Generate a parameter value with type PARAMETER_YAML
+  RCLCPP_PUBLIC
+  static ParameterValue YamlParameter(const std::string & yaml_string);
+  RCLCPP_PUBLIC
+  /// Generate a parameter value with type PARAMETER_YAML
+  static ParameterValue YamlParameter(const char * yaml_string);
+  /// Generate a parameter value with type PARAMETER_YAML
+  RCLCPP_PUBLIC
+  static ParameterValue YamlParameter(const YAML::Node & node);
   /// Return an enum indicating the type of the set value.
   RCLCPP_PUBLIC
   ParameterType
@@ -187,6 +202,17 @@ class ParameterValue
     return value_.string_value;
   }
 
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_YAML, const YAML::Node>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_YAML) {
+      throw ParameterTypeException(ParameterType::PARAMETER_YAML, get_type());
+    }
+    return YAML::Load(value_.yaml_value);
+  }
+
   template<ParameterType type>
   constexpr
   typename std::enable_if<
@@ -282,6 +308,14 @@ class ParameterValue
     return get<ParameterType::PARAMETER_STRING>();
   }
 
+  template<typename type>
+  constexpr
+  typename std::enable_if<std::is_same<type, YAML::Node>::value, const YAML::Node>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_YAML>();
+  }
+
   template<typename type>
   constexpr
   typename std::enable_if<

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>30.1.0</version>
+  <version>30.0.0</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="[email protected]">Ivan Paunovic</maintainer>
@@ -46,6 +46,7 @@
   <depend>rosidl_dynamic_typesupport</depend>
   <depend>statistics_msgs</depend>
   <depend>tracetools</depend>
+  <depend>yaml_cpp_vendor</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_google_benchmark</test_depend>

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -28,6 +28,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include "yaml-cpp/yaml.h"
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/create_publisher.hpp"
@@ -442,6 +443,131 @@ void __call_post_set_parameters_callbacks(
   }
 }
 
+
+RCLCPP_LOCAL
+/// Splits a parameter name into its component namespaces
+std::vector<std::string> __split_parameter_name(const std::string & name)
+{
+  constexpr char delimiter = '.';
+  std::vector<std::string> ret;
+  std::string out;
+  size_t position_found = -1;
+  std::string evaluation_string = name;
+  int counter = 0;
+  // Set an upper limit for the loop
+  // Anyone having more than 100 layers of namespacing should really evaluate their code - Rahul-K-A
+  while(counter++ < 100)
+  {
+    position_found = evaluation_string.find(delimiter);
+    if (std::string::npos == position_found)
+    {
+      break;
+    }
+    ret.push_back( evaluation_string.substr(0, position_found ));
+    evaluation_string = evaluation_string.substr(position_found + 1);
+  }
+  ret.push_back(evaluation_string);
+  return ret;
+}
+
+
+RCLCPP_LOCAL
+/// Pulled from Question 78243576 from Stack Overflow 
+void __traverse_node_and_change_value(YAML::Node & root, const std::vector<std::string>& path, rclcpp::ParameterValue value)
+{
+  YAML::Node traverse = root;
+  bool broken = false;
+  if (path.empty())
+  {
+    return;
+  }
+  // Skip first index because thats the name of the parameter itself and not a key
+  for (size_t i = 1; i < path.size() - 1; i++)
+  {
+    if (traverse[path[i]])
+    {
+      traverse.reset(traverse[path[i]]);
+    }  
+    else
+    {
+      broken = true;
+      break;
+    }
+  }
+
+  if (broken)
+  {
+    return;
+  }
+  // Set the value at the last level of nesting. Fugly switch case because we can't infer type - Rahul-K-A
+  switch (value.get_type()) {
+    case rclcpp::ParameterType::PARAMETER_BOOL:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_BOOL>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_INTEGER:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_INTEGER>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_DOUBLE:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_DOUBLE>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_STRING:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_STRING>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_BYTE_ARRAY:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_BYTE_ARRAY>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_BOOL_ARRAY:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_BOOL_ARRAY>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY>();
+      break;
+    case rclcpp::ParameterType::PARAMETER_STRING_ARRAY:
+      traverse[path.back()] = value.get<rclcpp::ParameterType::PARAMETER_STRING_ARRAY>();
+      break;
+    default:
+      throw rclcpp::exceptions::UnimplementedError();
+  }
+}
+
+RCLCPP_LOCAL
+void __handle_changes_inside_yaml_parameters(const std::vector<rclcpp::Parameter>& parameters, std::map<std::string, 
+  rclcpp::node_interfaces::ParameterInfo> & parameter_infos)
+{
+  for (const rclcpp::Parameter& parameter: parameters)
+  {
+    std::vector<std::string> split_name = __split_parameter_name(parameter.get_name());
+
+    if (split_name.size() == 1 && split_name[0] == parameter.get_name())
+    {
+      continue;
+    }
+
+    // Find this yaml_param name inside parameter_infos
+    if (parameter_infos.find(split_name[0]) == parameter_infos.end()) 
+    {
+      continue;
+    }
+
+    if (parameter_infos[split_name[0]].descriptor.type != rclcpp::ParameterType::PARAMETER_YAML)
+    {
+      continue;
+    }
+
+    // Load the value as a yaml
+    YAML::Node node = parameter_infos[split_name[0]].value.get<YAML::Node>();
+
+    // Set the value inside inside the yaml
+    __traverse_node_and_change_value(node, split_name, parameter.get_parameter_value());
+    // Add the modified yaml param to the "parameters" vector so that it gets set
+    parameter_infos[split_name[0]].value = rclcpp::ParameterValue::YamlParameter(node);
+  }
+}
+
+
 RCLCPP_LOCAL
 rcl_interfaces::msg::SetParametersResult
 __set_parameters_atomically_common(
@@ -471,6 +597,9 @@ __set_parameters_atomically_common(
       parameter_infos[name].descriptor.type = parameters[i].get_type();
       parameter_infos[name].value = parameters[i].get_parameter_value();
     }
+    // Handle cases where individual members of a yaml parameter have been changed
+    // We should probably not do this after the existing parameter namespacing feature is removed - Rahul-K-A
+    __handle_changes_inside_yaml_parameters(parameters, parameter_infos);
     // Call the user post set parameter callback
     __call_post_set_parameters_callbacks(parameters, post_set_callback_container);
   }

rclcpp/src/rclcpp/parameter.cpp

@@ -111,6 +111,12 @@ Parameter::as_string() const
   return get_value<ParameterType::PARAMETER_STRING>();
 }
 
+const YAML::Node
+Parameter::as_yaml() const
+{
+  return get_value<ParameterType::PARAMETER_YAML>();
+}
+
 const std::vector<uint8_t> &
 Parameter::as_byte_array() const
 {

rclcpp/src/rclcpp/parameter_map.cpp

@@ -72,7 +72,6 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod
 
     std::vector<Parameter> & params_node = parameters[node_name];
     params_node.reserve(c_params_node->num_params);
-
     for (size_t p = 0; p < c_params_node->num_params; ++p) {
       const char * const c_param_name = c_params_node->parameter_names[p];
       if (NULL == c_param_name) {
@@ -81,15 +80,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod
         throw InvalidParametersException(message);
       }
       const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]);
-      ParameterValue value;
-      try {
-        value = parameter_value_from(c_param_value);
-      } catch (const InvalidParameterValueException & e) {
-        throw InvalidParameterValueException(
-          std::string("parameter_value_from failed for parameter '") +
-          c_param_name + "': " + e.what());
-      }
-      params_node.emplace_back(c_param_name, value);
+      params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
     }
   }
 
@@ -110,6 +101,8 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value)
     return ParameterValue(*(c_param_value->double_value));
   } else if (c_param_value->string_value) {
     return ParameterValue(std::string(c_param_value->string_value));
+  } else if (c_param_value->yaml_value) {
+    return ParameterValue::YamlParameter(std::string(c_param_value->yaml_value));
   } else if (c_param_value->byte_array_value) {
     const rcl_byte_array_t * const byte_array = c_param_value->byte_array_value;
     std::vector<uint8_t> bytes;