From edfeb82ffcbb958785ee4f2d7b6eea38ee79dc78 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 23 Mar 2024 00:12:22 -0400 Subject: [PATCH] Restore functionality for mapped params with no struct name (#185) * Restore functionality for mapped params with no struct name * Fix nested maps with no struct name --- .gitignore | 1 + example/config/implementation.yaml | 4 ++++ example/src/parameters.yaml | 11 +++++++++++ example_python/config/implementation.yaml | 4 ++++ .../generate_parameter_module_example/parameters.yaml | 11 +++++++++++ .../jinja_templates/cpp/declare_runtime_parameter | 8 ++++++++ .../jinja_templates/cpp/remove_runtime_parameter | 4 ++++ .../jinja_templates/cpp/update_runtime_parameter | 8 ++++++++ .../jinja_templates/python/declare_runtime_parameter | 8 +++++++- .../jinja_templates/python/update_runtime_parameter | 10 +++++++++- 10 files changed, 67 insertions(+), 2 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..eeb8a6e --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +**/__pycache__ diff --git a/example/config/implementation.yaml b/example/config/implementation.yaml index 324356c..4cd1ba9 100644 --- a/example/config/implementation.yaml +++ b/example/config/implementation.yaml @@ -11,6 +11,10 @@ admittance_controller: fixed_string_no_default: "happy" + elbow_joint: + x: + weight: 2.0 + pid: shoulder_pan_joint: i: 0.7 diff --git a/example/src/parameters.yaml b/example/src/parameters.yaml index 7520d0d..31e7e61 100644 --- a/example/src/parameters.yaml +++ b/example/src/parameters.yaml @@ -24,6 +24,17 @@ admittance_controller: description: "specifies which joints will be used by the controller", } + __map_joints: + __map_dof_names: + weight: { + type: double, + default_value: 1.0, + description: "map parameter without struct name", + validation: { + gt<>: [0.0], + } + } + nested_dynamic: __map_joints: __map_dof_names: diff --git a/example_python/config/implementation.yaml b/example_python/config/implementation.yaml index 324356c..4cd1ba9 100644 --- a/example_python/config/implementation.yaml +++ b/example_python/config/implementation.yaml @@ -11,6 +11,10 @@ admittance_controller: fixed_string_no_default: "happy" + elbow_joint: + x: + weight: 2.0 + pid: shoulder_pan_joint: i: 0.7 diff --git a/example_python/generate_parameter_module_example/parameters.yaml b/example_python/generate_parameter_module_example/parameters.yaml index 9c856f4..1440e58 100644 --- a/example_python/generate_parameter_module_example/parameters.yaml +++ b/example_python/generate_parameter_module_example/parameters.yaml @@ -32,6 +32,17 @@ admittance_controller: description: "specifies which joints will be used by the controller", } + __map_joints: + __map_dof_names: + weight: { + type: double, + default_value: 1.0, + description: "map parameter without struct name", + validation: { + gt<>: [0.0], + } + } + nested_dynamic: __map_joints: __map_dof_names: 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 f960b64..244e595 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 @@ -2,10 +2,18 @@ for (const auto & value_{{loop.index}} : updated_params.{{mapped_param}}) { {% endfor -%} {%- filter indent(width=4) -%} +{% if struct_name != "" %} auto& entry = {{param_struct_instance}}.{{struct_name}}{% for map in parameter_map%}.{{map}}[value_{{loop.index}}]{% endfor %}; +{% else %} +auto& entry = {{param_struct_instance}}{% for map in parameter_map%}.{{map}}[value_{{loop.index}}]{% endfor %}; +{% endif -%} std::string value = fmt::format("{%- for mapped_param in mapped_params -%}{% if loop.index == 1 %}{}{% else %}.{}{% endif -%} {%- endfor -%}", {%- for mapped_param in mapped_params -%}{% if loop.index == 1 %} value_{{loop.index}}{% else %}, value_{{loop.index}}{% endif -%} {%- endfor %}); +{% if struct_name != "" %} auto param_name = fmt::format("{}{}.{}.{}", prefix_, "{{struct_name}}", value, "{{parameter_field}}"); +{% else %} +auto param_name = fmt::format("{}{}.{}", prefix_, value, "{{parameter_field}}"); +{% endif -%} if (!parameters_interface_->has_parameter(param_name)) { {%- filter indent(width=4) %} rcl_interfaces::msg::ParameterDescriptor descriptor; diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/remove_runtime_parameter b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/remove_runtime_parameter index abaa2c2..9617e45 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/remove_runtime_parameter +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/remove_runtime_parameter @@ -2,7 +2,11 @@ std::set {{mapped_param}}_set(updated_params.{{mapped_param}}.begin(), updated_params.{{mapped_param}}.end()); for (const auto &it: updated_params.{{parameter_map}}) { if ({{mapped_param}}_set.find(it.first) == {{mapped_param}}_set.end()) { + {% if struct_name != "" %} auto param_name = fmt::format("{}{}.{}.{}", prefix_, "{{struct_name}}", it.first, "{{parameter_field}}"); + {% else %} + auto param_name = fmt::format("{}{}.{}", prefix_, it.first, "{{parameter_field}}"); + {% endif -%} parameters_interface_->undeclare_parameter(param_name); } } diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_runtime_parameter b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_runtime_parameter index 3fb84fb..b4cd4e3 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_runtime_parameter +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/update_runtime_parameter @@ -4,13 +4,21 @@ for (const auto & value_{{loop.index}} : updated_params.{{mapped_param}}) { {%- filter indent(width=4) -%} std::string value = fmt::format("{%- for mapped_param in mapped_params -%}{% if loop.index == 1 %}{}{% else %}.{}{% endif -%} {%- endfor -%}", {%- for mapped_param in mapped_params -%}{% if loop.index == 1 %} value_{{loop.index}}{% else %}, value_{{loop.index}}{% endif -%} {%- endfor %}); +{% if struct_name != "" %} auto param_name = fmt::format("{}{}.{}.{}", prefix_, "{{struct_name}}", value, "{{parameter_field}}"); +{% else %} +auto param_name = fmt::format("{}{}.{}", prefix_, value, "{{parameter_field}}"); +{% endif -%} if (param.get_name() == param_name) { {%- filter indent(width=4) %} {% if parameter_validations|length -%} {{parameter_validations-}} {% endif -%} +{% if struct_name != "" %} updated_params.{{struct_name}}{% for map in parameter_map%}.{{map}}[value_{{loop.index}}]{% endfor %}.{{parameter_field}} = param.{{parameter_as_function}}; +{% else %} +updated_params{% for map in parameter_map%}.{{map}}[value_{{loop.index}}]{% endfor %}.{{parameter_field}} = param.{{parameter_as_function}}; +{% endif -%} RCLCPP_DEBUG_STREAM(logger_, param.get_name() << ": " << param.get_type_name() << " = " << param.value_to_string()); {% endfilter -%} } diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/declare_runtime_parameter b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/declare_runtime_parameter index 117fa1b..96c48f7 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/declare_runtime_parameter +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/declare_runtime_parameter @@ -4,9 +4,15 @@ for value_{{loop.index}} in updated_params.{{mapped_param}}: {%- endfilter -%} {% endfor -%} {%- filter indent(width=4*(mapped_params|length)) %} +{% if struct_name != "" %} {{param_struct_instance}}.{{struct_name}}{% for map in parameter_map%}.add_entry(value_{{loop.index}}){% endfor %} entry = {{param_struct_instance}}.{{struct_name}}{% for map in parameter_map%}.get_entry(value_{{loop.index}}){% endfor %} -param_name = f"{self.prefix_}{{struct_name}}{% for map in parameter_map%}.{value_{{loop.index}}}{% endfor %}.{{parameter_field}}" +param_name = f"{self.prefix_}{{struct_name}}.{% for map in parameter_map%}{value_{{loop.index}}}.{% endfor %}{{parameter_field}}" +{% else %} +{{param_struct_instance}}{% for map in parameter_map%}.add_entry(value_{{loop.index}}){% endfor %} +entry = {{param_struct_instance}}{% for map in parameter_map%}.get_entry(value_{{loop.index}}){% endfor %} +param_name = f"{self.prefix_}{% for map in parameter_map%}{value_{{loop.index}}}.{% endfor %}{{parameter_field}}" +{% endif -%} if not self.node_.has_parameter(self.prefix_ + param_name): {%- filter indent(width=4) %} descriptor = ParameterDescriptor(description="{{parameter_description|valid_string_python}}", read_only = {{parameter_read_only}}) diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/update_runtime_parameter b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/update_runtime_parameter index 231a3c4..195fb78 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/update_runtime_parameter +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/python/update_runtime_parameter @@ -4,13 +4,21 @@ for value_{{loop.index}} in updated_params.{{mapped_param}}: {%- endfilter -%} {% endfor -%} {%- filter indent(width=4*(1+mapped_params|length)) %} -param_name = f"{self.prefix_}{{struct_name}}{% for map in parameter_map%}.{value_{{loop.index}}}{% endfor %}.{{parameter_field}}" +{% if struct_name != "" %} +param_name = f"{self.prefix_}{{struct_name}}{% for map in parameter_map%}{value_{{loop.index}}}.{% endfor %}{{parameter_field}}" +{% else %} +param_name = f"{self.prefix_}{% for map in parameter_map%}{value_{{loop.index}}}.{% endfor %}{{parameter_field}}" +{% endif -%} if param.name == param_name: {%- filter indent(width=4) %} {% if parameter_validations|length -%} {{parameter_validations-}} {% endif -%} +{% if struct_name != "" %} updated_params.{{struct_name}}{% for map in parameter_map%}.get_entry(value_{{loop.index}}){% endfor %}.{{parameter_field}} = param.{{parameter_as_function}} +{% else %} +updated_params{% for map in parameter_map%}.get_entry(value_{{loop.index}}){% endfor %}.{{parameter_field}} = param.{{parameter_as_function}} +{% endif -%} self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) {% endfilter -%} {% endfilter -%}