diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/declare_runtime_parameter b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/declare_runtime_parameter index 9d9395d..0abd434 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/declare_runtime_parameter +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/declare_runtime_parameter @@ -47,6 +47,5 @@ auto parameter = rclcpp::ParameterValue(entry.{{parameter_field}}); parameters_interface_->declare_parameter(param_name, parameter, descriptor); {% endfilter -%} } -{{set_runtime_parameter-}} {% endfilter -%} } diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header index 9422c66..b68b61a 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header @@ -119,8 +119,25 @@ struct StackParams { } Params get_params() const{ - std::lock_guard lock(mutex_); - return params_; + auto params = get_internal_params(); + if(params.__stamp.seconds() == 0){ + Params updated_params; + // get parameters and fill struct fields + rclcpp::Parameter param; + +{%- filter indent(width=8) %} +{{declare_params_set}} +{%- endfilter %} +{% if set_dynamic_params|length %} +{% filter indent(width=8) %} +// set all dynamic parameters +{{set_dynamic_params-}} +{%- endfilter %} +{%- endif %} + updated_params.__stamp = clock_.now(); + return updated_params; + } + return params; } bool is_old(Params const& other) const { @@ -139,27 +156,19 @@ struct StackParams { return output; } + [[deprecated("Calling the method is no longer needed, will be removed in the future")]] void refresh_dynamic_parameters() { - auto updated_params = get_params(); - // TODO remove any destroyed dynamic parameters -{%- filter indent(width=6) %} -{{remove_dynamic_parameters}} -{%- endfilter %} - // declare any new dynamic parameters - rclcpp::Parameter param; -{%- filter indent(width=6) %} -{{update_declare_dynamic_parameters}} -{%- endfilter %} } rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { - auto updated_params = get_params(); - + auto updated_params = get_internal_params(); +{% if update_params_set|length %} for (const auto ¶m: parameters) { {%- filter indent(width=8) %} {{update_params_set}} {%- endfilter %} } +{%- endif %} {% if update_dynamic_parameters|length %} // update dynamic parameters for (const auto ¶m: parameters) { @@ -174,26 +183,18 @@ struct StackParams { } void declare_params(){ - auto updated_params = get_params(); + Params updated_params; // declare all parameters and give default values to non-required ones {%- filter indent(width=6) %} {{declare_params}} {%- endfilter %} - // get parameters and fill struct fields - rclcpp::Parameter param; -{%- filter indent(width=6) %} -{{declare_params_set}} -{%- endfilter %} {% if declare_set_dynamic_params|length %} {% filter indent(width=6) %} -// declare and set all dynamic parameters +// declare all dynamic parameters {{declare_set_dynamic_params}} {%- endfilter %} {%- endif %} - - updated_params.__stamp = clock_.now(); - update_interal_params(updated_params); } private: @@ -202,8 +203,15 @@ struct StackParams { params_ = updated_params; } + Params get_internal_params() const{ + std::lock_guard lock(mutex_); + return params_; + } + std::string prefix_; Params params_; + // for detecting if the parameter struct has been updated by update + rclcpp::Time latest_stamp_; rclcpp::Clock clock_; std::shared_ptr handle_; std::shared_ptr parameters_interface_; diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/set_runtime_parameter b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/set_runtime_parameter index 015be80..a30c880 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/set_runtime_parameter +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/set_runtime_parameter @@ -1,6 +1,12 @@ +for (const auto & value : {{param_struct_instance}}.{{mapped_param}}){ +{%- filter indent(width=4) %} +auto& entry = {{param_struct_instance}}.{{parameter_map}}[value]; +auto param_name = fmt::format("{}{}.{}.{}", prefix_, "{{struct_name}}", value, "{{parameter_field}}"); param = parameters_interface_->get_parameter(param_name); RCLCPP_DEBUG_STREAM(logger_, param.get_name() << ": " << param.get_type_name() << " = " << param.value_to_string()); {% if parameter_validations|length -%} {{parameter_validations-}} {% endif -%} entry.{{parameter_field}} = param.{{parameter_as_function}}; +{% endfilter -%} +} diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_parameter b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_parameter index 4a9a560..a3285cd 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_parameter +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_parameter @@ -6,4 +6,4 @@ if (param.get_name() == (prefix_ + "{{parameter_name}}")) { updated_params.{{parameter_name}} = param.{{parameter_as_function}}; RCLCPP_DEBUG_STREAM(logger_, param.get_name() << ": " << param.get_type_name() << " = " << param.value_to_string()); {% endfilter -%} -} +} \ No newline at end of file diff --git a/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py b/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py index 4ee7f75..9bf7bcb 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py +++ b/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py @@ -139,6 +139,14 @@ def get_fixed_type(yaml_type: str): return get_fixed_base_type(yaml_type) + "_fixed" +@typechecked +def get_all_keys(param_map: dict): + for key, value in param_map.items(): + yield key + if isinstance(value, dict): + yield from get_all_keys(value) + + class CodeGenVariableBase: @typechecked def __init__( @@ -434,8 +442,14 @@ def __str__(self): class SetParameterBase: @typechecked - def __init__(self, parameter_name: str, code_gen_variable: CodeGenVariableBase): + def __init__( + self, + parameter_name: str, + params_map_parents: list, + code_gen_variable: CodeGenVariableBase, + ): self.parameter_name = parameter_name + self.params_map_parents = params_map_parents self.parameter_as_function = code_gen_variable.parameter_as_function_str() self.parameter_validations = [] @@ -452,6 +466,7 @@ def __str__(self): "parameter_name": self.parameter_name, "parameter_validations": str(parameter_validations_str), "parameter_as_function": self.parameter_as_function, + "mapped_params": self.params_map_parents, } j2_template = Template(GenerateCode.templates["set_parameter"]) @@ -463,10 +478,19 @@ class SetRuntimeParameter(SetParameterBase): def __str__(self): parameter_validations_str = "".join(str(x) for x in self.parameter_validations) parameter_field = get_dynamic_parameter_field(self.parameter_name) + mapped_param = get_dynamic_mapped_parameter(self.parameter_name) + parameter_map = get_dynamic_parameter_map(self.parameter_name) + struct_name = get_dynamic_struct_name(self.parameter_name) + param_struct_instance = "updated_params" data = { + "struct_name": struct_name, "parameter_field": parameter_field, "parameter_validations": str(parameter_validations_str), "parameter_as_function": self.parameter_as_function, + "param_struct_instance": param_struct_instance, + "parameter_field": parameter_field, + "parameter_map": parameter_map, + "mapped_param": mapped_param, } j2_template = Template(GenerateCode.templates["set_runtime_parameter"]) @@ -701,6 +725,7 @@ def __init__(self, language: str): self.update_declare_dynamic_parameter = [] self.remove_dynamic_parameter = [] self.declare_parameter_sets = [] + self.declare_dynamic_parameters_sets = [] self.set_stack_params = [] if language == "cpp": self.comments = "// auto-generated DO NOT EDIT" @@ -728,6 +753,10 @@ def parse(self, yaml_file, validate_header): raise compile_error( "The yaml definition must only have one root element" ) + self.params_map_parents = list() + for key in list(get_all_keys(doc)): + if is_mapped_parameter(key): + self.params_map_parents.append((key.replace("__map_", ""))) self.namespace = list(doc.keys())[0] self.user_validation_file = validate_header self.parse_dict(self.namespace, doc[self.namespace], []) @@ -764,7 +793,9 @@ def parse_params(self, name, value, nested_name_list): is_runtime_parameter = is_mapped_parameter(self.struct_tree.struct_name) if is_runtime_parameter: - declare_parameter_set = SetRuntimeParameter(param_name, code_gen_variable) + declare_parameter_set = SetRuntimeParameter( + param_name, self.params_map_parents, code_gen_variable + ) declare_parameter = DeclareRuntimeParameter( code_gen_variable, description, read_only, validations ) @@ -774,7 +805,9 @@ def parse_params(self, name, value, nested_name_list): declare_parameter = DeclareParameter( code_gen_variable, description, read_only, validations ) - declare_parameter_set = SetParameter(param_name, code_gen_variable) + declare_parameter_set = SetParameter( + param_name, self.params_map_parents, code_gen_variable + ) update_parameter = UpdateParameter(param_name, code_gen_variable) # set parameter @@ -803,13 +836,16 @@ def parse_params(self, name, value, nested_name_list): self.set_stack_params.append(SetStackParams(code_gen_variable.param_name)) if is_runtime_parameter: self.declare_dynamic_parameters.append(declare_parameter) - self.update_dynamic_parameters.append(update_parameter) + self.declare_dynamic_parameters_sets.append(declare_parameter_set) + if str(update_parameter): + self.update_dynamic_parameters.append(update_parameter) self.update_declare_dynamic_parameter.append(declare_parameter) dynamic_update_parameter = RemoveRuntimeParameter(declare_parameter) self.remove_dynamic_parameter.append(dynamic_update_parameter) else: self.declare_parameters.append(declare_parameter) - self.update_parameters.append(update_parameter) + if str(update_parameter): + self.update_parameters.append(update_parameter) self.declare_parameter_sets.append(declare_parameter_set) def parse_dict(self, name, root_map, nested_name): @@ -860,6 +896,9 @@ def __str__(self): "declare_set_dynamic_params": "\n".join( [str(x) for x in self.declare_dynamic_parameters] ), + "set_dynamic_params": "\n".join( + [str(x) for x in self.declare_dynamic_parameters_sets] + ), "update_declare_dynamic_parameters": "\n".join( [str(x) for x in self.update_declare_dynamic_parameter] ),