diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 4803faff05..288e2089e8 100644 --- a/rclcpp/CMakeLists.txt +++ b/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) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 85f088d025..3798227421 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/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). /** * \throws rclcpp::ParameterTypeException if the type doesn't match diff --git a/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp index 549429aa85..f3444aa8af 100644 --- a/rclcpp/include/rclcpp/parameter_value.hpp +++ b/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 & byte_array_value); @@ -121,6 +126,16 @@ class ParameterValue RCLCPP_PUBLIC explicit ParameterValue(const std::vector & 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 + constexpr + typename std::enable_if::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 constexpr typename std::enable_if< @@ -282,6 +308,14 @@ class ParameterValue return get(); } + template + constexpr + typename std::enable_if::value, const YAML::Node>::type + get() const + { + return get(); + } + template constexpr typename std::enable_if< diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 385ecd57b4..0e4a854ce4 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 30.1.0 + 30.0.0 The ROS client library in C++. Ivan Paunovic @@ -46,6 +46,7 @@ rosidl_dynamic_typesupport statistics_msgs tracetools + yaml_cpp_vendor ament_cmake_gmock ament_cmake_google_benchmark diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 4a51f0adda..a2864c82f9 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -28,6 +28,7 @@ #include #include #include +#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 __split_parameter_name(const std::string & name) +{ + constexpr char delimiter = '.'; + std::vector 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& 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(); + break; + case rclcpp::ParameterType::PARAMETER_INTEGER: + traverse[path.back()] = value.get(); + break; + case rclcpp::ParameterType::PARAMETER_DOUBLE: + traverse[path.back()] = value.get(); + break; + case rclcpp::ParameterType::PARAMETER_STRING: + traverse[path.back()] = value.get(); + break; + case rclcpp::ParameterType::PARAMETER_BYTE_ARRAY: + traverse[path.back()] = value.get(); + break; + case rclcpp::ParameterType::PARAMETER_BOOL_ARRAY: + traverse[path.back()] = value.get(); + break; + case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY: + traverse[path.back()] = value.get(); + break; + case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY: + traverse[path.back()] = value.get(); + break; + case rclcpp::ParameterType::PARAMETER_STRING_ARRAY: + traverse[path.back()] = value.get(); + break; + default: + throw rclcpp::exceptions::UnimplementedError(); + } +} + +RCLCPP_LOCAL +void __handle_changes_inside_yaml_parameters(const std::vector& parameters, std::map & parameter_infos) +{ + for (const rclcpp::Parameter& parameter: parameters) + { + std::vector 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(); + + // 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); } diff --git a/rclcpp/src/rclcpp/parameter.cpp b/rclcpp/src/rclcpp/parameter.cpp index 673e06ab9b..0e170cf10f 100644 --- a/rclcpp/src/rclcpp/parameter.cpp +++ b/rclcpp/src/rclcpp/parameter.cpp @@ -111,6 +111,12 @@ Parameter::as_string() const return get_value(); } +const YAML::Node +Parameter::as_yaml() const +{ + return get_value(); +} + const std::vector & Parameter::as_byte_array() const { diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index bf702c8aa6..d71864c4ea 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/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 & 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 bytes; diff --git a/rclcpp/src/rclcpp/parameter_value.cpp b/rclcpp/src/rclcpp/parameter_value.cpp index ee46e77673..d720cdad83 100644 --- a/rclcpp/src/rclcpp/parameter_value.cpp +++ b/rclcpp/src/rclcpp/parameter_value.cpp @@ -34,6 +34,8 @@ rclcpp::to_string(const ParameterType type) return "double"; case ParameterType::PARAMETER_STRING: return "string"; + case ParameterType::PARAMETER_YAML: + return "yaml"; case ParameterType::PARAMETER_BYTE_ARRAY: return "byte_array"; case ParameterType::PARAMETER_BOOL_ARRAY: @@ -93,6 +95,8 @@ rclcpp::to_string(const ParameterValue & value) return std::to_string(value.get()); case ParameterType::PARAMETER_STRING: return value.get(); + case ParameterType::PARAMETER_YAML: + return value.get(); case ParameterType::PARAMETER_BYTE_ARRAY: return array_to_string(value.get>(), std::ios::hex); case ParameterType::PARAMETER_BOOL_ARRAY: @@ -121,6 +125,7 @@ ParameterValue::ParameterValue(const rcl_interfaces::msg::ParameterValue & value case PARAMETER_INTEGER: case PARAMETER_DOUBLE: case PARAMETER_STRING: + case PARAMETER_YAML: case PARAMETER_BYTE_ARRAY: case PARAMETER_BOOL_ARRAY: case PARAMETER_INTEGER_ARRAY: @@ -169,6 +174,12 @@ ParameterValue::ParameterValue(const std::string & string_value) value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; } +ParameterValue::ParameterValue(const YAML::Node & yaml_value) +{ + value_.yaml_value = YAML::Dump(yaml_value); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_YAML; +} + ParameterValue::ParameterValue(const char * string_value) : ParameterValue(std::string(string_value)) {} @@ -215,6 +226,29 @@ ParameterValue::ParameterValue(const std::vector & string_array_val value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; } + +// Create a parameter value with type PARAMETER_YAML +ParameterValue ParameterValue::YamlParameter(const std::string & yaml_string) +{ + ParameterValue param_value; + param_value.value_.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_YAML); + param_value.value_.set__yaml_value(yaml_string); + return param_value; +} + +// Create a parameter value with type PARAMETER_YAML +ParameterValue ParameterValue::YamlParameter(const char * yaml_string) +{ + return ParameterValue::YamlParameter(std::string(yaml_string)); +} + + +// Create a parameter value with type PARAMETER_YAML +ParameterValue ParameterValue::YamlParameter(const YAML::Node& node) +{ + return ParameterValue::YamlParameter(YAML::Dump(node)); +} + ParameterType ParameterValue::get_type() const {