From 0b04bb1beea26415da4358997fbaf027d8d3cb56 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 10 Jun 2026 14:54:10 +0000 Subject: [PATCH 1/3] Add additional_constraints in python parameter file --- .../generate_parameter_module_example/parameters.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/example_python/generate_parameter_module_example/parameters.yaml b/example_python/generate_parameter_module_example/parameters.yaml index 9a5d334..904f660 100644 --- a/example_python/generate_parameter_module_example/parameters.yaml +++ b/example_python/generate_parameter_module_example/parameters.yaml @@ -3,6 +3,7 @@ admittance_controller: type: double default_value: 0.00000000001 description: "Test scientific notation" + additional_constraints: "Any string can be here. For example, you might want to embed JSON schema" interpolation_mode: type: string default_value: "spline" @@ -61,11 +62,13 @@ admittance_controller: description: "test deep nested map params" validation: gt_eq<>: [ 0.0001 ] + pid: rate: type: double default_value: 0.005 description: "update loop period in seconds" + __map_joints: p: type: double @@ -134,6 +137,7 @@ admittance_controller: command_interfaces: type: string_array description: "specifies which command interfaces to claim" + additional_constraints: "some additional constraints" read_only: true state_interfaces: From 24823d797e18fc0d20534b5259c0441767d7f28f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 10 Jun 2026 14:55:16 +0000 Subject: [PATCH 2/3] Use a shared symlinked parameter file in all examples --- example/src/parameters.yaml | 355 +----------------- .../parameters.yaml | 227 +---------- 2 files changed, 2 insertions(+), 580 deletions(-) mode change 100644 => 120000 example/src/parameters.yaml mode change 100644 => 120000 example_cmake_python/cmake_generate_parameter_module_example/parameters.yaml diff --git a/example/src/parameters.yaml b/example/src/parameters.yaml deleted file mode 100644 index ba28043..0000000 --- a/example/src/parameters.yaml +++ /dev/null @@ -1,354 +0,0 @@ -admittance_controller: - scientific_notation_num: - type: double - default_value: 0.00000000001 - description: "Test scientific notation" - additional_constraints: "Any string can be here. For example, you might want to embed JSON schema" - interpolation_mode: - type: string - default_value: "spline" - description: "specifies which algorithm to use for interpolation." - validation: - one_of<>: [ [ "spline", "linear" ] ] - "custom_validators::no_args_validator": null - joints: - type: string_array - default_value: ["joint1", "joint2", "joint3"] - description: "specifies which joints will be used by the controller" - dof_names: - type: string_array - default_value: ["x", "y", "rz"] - 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] - - limit: - type: double_array - description: "specifies limit for x, y and z axis" - default_value: [0.0, 0.0, 0.0] - validation: - fixed_size<>: 3 - lower_element_bounds<>: -10.0 - upper_element_bounds<>: 10.0 - - nested_dynamic: - __map_joints: - __map_dof_names: - nested: - type: double - default_value: 1.0 - description: "test nested map params" - validation: - gt_eq<>: [ 0.0001 ] - __map_joints: - __map_dof_names: - nested_deep: - type: double - default_value: 1.0 - description: "test deep nested map params" - validation: - gt_eq<>: [ 0.0001 ] - - pid: - rate: - type: double - default_value: 0.005 - description: "update loop period in seconds" - - __map_joints: - p: - type: double - default_value: 1.0 - description: "proportional gain term" - validation: - gt_eq<>: [ 0.0001 ] - i: - type: double - default_value: 1.0 - description: "integral gain term" - d: - type: double - default_value: 1.0 - description: "derivative gain term" - - gains: - __map_dof_names: - k: - type: double - default_value: 2.0 - description: "general gain" - - nested_map: - entries: - type: string_array - default_value: ["entry1", "entry2"] - description: "Keys for nested mapped parameters" - __map_entries: - value: - type: double - default_value: 1.0 - description: "A value in the nested map with sibling keys" - - fixed_string: - type: string_fixed_25 - default_value: "string_value" - description: "test code generation for fixed sized string" - fixed_array: - type: double_array_fixed_10 - default_value: [1.0, 2.3, 4.0 ,5.4, 3.3] - description: "test code generation for fixed sized array" - fixed_string_no_default: - type: string_fixed_25 - description: "test code generation for fixed sized string with no default" - fixed_string_array: - type: string_array_fixed_5 - default_value: ["alpha", "beta", "gamma", "delta", "epsilon"] - description: "test code generation for fixed sized string array" - fixed_string_array_no_default: - type: string_array_fixed_5 - description: "test code generation for fixed sized string array with no default" - - command_interfaces: - type: string_array - description: "specifies which command interfaces to claim" - additional_constraints: "some additional constraints" - read_only: true - - state_interfaces: - type: string_array - description: "specifies which state interfaces to claim" - read_only: true - - chainable_command_interfaces: - type: string_array - description: "specifies which chainable interfaces to claim" - read_only: true - - kinematics: - plugin_name: - type: string - description: "specifies which kinematics plugin to load" - plugin_package: - type: string - description: "specifies the package to load the kinematics plugin from" - base: - type: string - description: "specifies the base link of the robot description used by the kinematics plugin" - tip: - type: string - description: "specifies the end effector link of the robot description used by the kinematics plugin" - alpha: - type: double - default_value: 0.0005 - description: "specifies the damping coefficient for the Jacobian pseudo inverse" - group_name: - type: string - description: "specifies the group name for planning with Moveit" - - ft_sensor: - name: - type: string - description: "name of the force torque sensor in the robot description" - frame: - id: - type: string - description: "frame of the force torque sensor" - external: - type: bool - default_value: false - description: "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip" - filter_coefficient: - type: double - default_value: 0.005 - description: "specifies the coefficient for the sensor's exponential filter" - - control: - frame: - id: - type: string - description: "control frame used for admittance control" - external: - type: bool - default_value: false - description: "specifies if the control frame is contained in the kinematics chain from the base to the tip" - - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - frame: - id: - type: string - description: "world frame, gravity points down (neg. Z) in this frame" - external: - type: bool - default_value: false - description: "specifies if the world frame is contained in the kinematics chain from the base to the tip" - - gravity_compensation: - frame: - id: - type: string - description: "frame which center of gravity (CoG) is defined in" - external: - type: bool - default_value: false - description: "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip" - CoG: # specifies the center of gravity of the end effector - pos: - type: double_array - description: "position of the center of gravity (CoG) in its frame" - validation: - fixed_size<>: 3 - force: - type: double - default_value: .NAN - description: "weight of the end effector, e.g mass * 9.81" - - admittance: - selected_axes: - type: bool_array - description: "specifies if the axes x, y, z, rx, ry, and rz are enabled" - validation: - fixed_size<>: 6 - - # Having ".0" at the end is MUST, otherwise there is a loading error - # F = M*a + D*v + S*(x - x_d) - mass: - type: double_array - description: "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation" - validation: - fixed_size<>: 6 - element_bounds<>: [ 0.0001, 1000000.0 ] - - damping_ratio: - type: double_array - description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. - The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))" - validation: - fixed_size<>: 6 - "custom_validators::validate_double_array_custom_func": [ 20.3, 5.0 ] - element_bounds<>: [ 0.1, 10.0 ] - - stiffness: - type: double_array - description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation" - validation: - element_bounds: [ 0.0001, 100000.0 ] - - acceleration_limits: - type: double_array - description: "specifies maximum acceleration limits for x, y and z axis" - default_value: [0.0, 0.0, 0.0] - validation: - fixed_size<>: 3 - lower_element_bounds<>: -10.0 - upper_element_bounds<>: 10.0 - - # general settings - enable_parameter_update_without_reactivation: - type: bool - default_value: true - description: "if enabled, read_only parameters will be dynamically updated in the control loop" - use_feedforward_commanded_input: - type: bool - default_value: false - description: "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity" - lt_eq_fifteen: - type: int - default_value: 1 - description: "should be a number less than or equal to 15" - validation: - lt_eq<>: [ 15 ] - gt_fifteen: - type: int - default_value: 16 - description: "should be a number greater than 15" - validation: - gt<>: [ 15 ] - gt_fifteen_lt_eq_twenty: - type: int - default_value: 20 - description: "should be a number greater than 15 and less than or equal to 20" - validation: - gt<>: [ 15 ] - lt_eq<>: [ 20 ] - gt_fifteen_lt_twenty: - type: int - default_value: 16 - description: "should be a number greater than 15 and less than 20" - validation: - gt<>: [ 15 ] - lt<>: [ 20 ] - lt_eq_fifteen_double: - type: double - default_value: 1.0 - description: "should be a number less than or equal to 15" - validation: - lt_eq<>: [ 15.0 ] - gt_fifteen_double: - type: double - default_value: 16.0 - description: "should be a number greater than 15" - validation: - gt<>: [ 15.0 ] - gt_fifteen_lt_eq_twenty_double: - type: double - default_value: 20.0 - description: "should be a number greater than 15 and less than or equal to 20" - validation: - gt<>: [ 15.0 ] - lt_eq<>: [ 20.0 ] - gt_fifteen_lt_twenty_double: - type: double - default_value: 16.0 - description: "should be a number greater than 15 and less than 20" - validation: - gt<>: [ 15.0 ] - lt<>: [ 20.0 ] - one_number: - type: int - default_value: 14540 - read_only: true - validation: - bounds<>: [ 1024, 65535 ] - three_numbers: - type: int_array - default_value: [3,4,5] - read_only: true - three_numbers_of_five: - type: int_array_fixed_5 - default_value: [3,3,3] - read_only: true - hover_override: - type: int - default_value: 1 - description: "Override hover action:\n0: Hover\n1: Push\n2: Pull\n-1: Do not override" - validation: - one_of<>: [ [ 0, 1, 2, -1 ] ] - angle_wraparound: - type: bool - default_value: false - description: 'For joints that wrap around (without end stop, ie. are continuous) - where the shortest rotation to the target position is the desired motion. - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured - position :math:`s` from the state interface.' - open_loop_control: - type: bool - default_value: false - description: "Use controller in open-loop control mode - \n\n - * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n - * It deactivates the feedback control, see the ``gains`` structure. - \n\n - This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). - \n\n - If this flag is set, the controller tries to read the values from the command interfaces on activation. - If they have real numeric values, those will be used instead of state interfaces. - Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started.\n" - read_only: true diff --git a/example/src/parameters.yaml b/example/src/parameters.yaml new file mode 120000 index 0000000..7cee8b0 --- /dev/null +++ b/example/src/parameters.yaml @@ -0,0 +1 @@ +../../example_python/generate_parameter_module_example/parameters.yaml \ No newline at end of file diff --git a/example_cmake_python/cmake_generate_parameter_module_example/parameters.yaml b/example_cmake_python/cmake_generate_parameter_module_example/parameters.yaml deleted file mode 100644 index 00d3b63..0000000 --- a/example_cmake_python/cmake_generate_parameter_module_example/parameters.yaml +++ /dev/null @@ -1,226 +0,0 @@ -admittance_controller: - scientific_notation_num: - type: double - default_value: 0.00000000001 - description: "Test scientific notation" - interpolation_mode: - type: string - default_value: "spline" - description: "specifies which algorithm to use for interpolation." - validation: - one_of<>: [ [ "spline", "linear" ] ] - "custom_validators::no_args_validator": null - subset_selection: - type: string_array - default_value: ["A", "B"] - description: "test subset of validator." - validation: - subset_of<>: [ [ "A", "B", "C"] ] - joints: - type: string_array - default_value: ["joint1", "joint2", "joint3"] - description: "specifies which joints will be used by the controller" - dof_names: - type: string_array - default_value: ["x", "y", "rz"] - description: "specifies which joints will be used by the controller" - - pid: - rate: - type: double - default_value: 0.005 - description: "update loop period in seconds" - - __map_joints: - p: - type: double - default_value: 1.0 - description: "proportional gain term" - validation: - gt_eq<>: [ 0.0001 ] - i: - type: double - default_value: 1.0 - description: "integral gain term" - d: - type: double - default_value: 1.0 - description: "derivative gain term" - - gains: - __map_dof_names: - k: - type: double - default_value: 2.0 - description: "general gain" - - fixed_string: - type: string_fixed_25 - default_value: "string_value" - description: "test code generation for fixed sized string" - fixed_array: - type: double_array_fixed_10 - default_value: [1.0, 2.3, 4.0 ,5.4, 3.3] - description: "test code generation for fixed sized array" - fixed_string_no_default: - type: string_fixed_25 - description: "test code generation for fixed sized string with no default" - command_interfaces: - type: string_array - description: "specifies which command interfaces to claim" - read_only: true - - state_interfaces: - type: string_array - description: "specifies which state interfaces to claim" - read_only: true - - chainable_command_interfaces: - type: string_array - description: "specifies which chainable interfaces to claim" - read_only: true - - kinematics: - plugin_name: - type: string - description: "specifies which kinematics plugin to load" - plugin_package: - type: string - description: "specifies the package to load the kinematics plugin from" - base: - type: string - description: "specifies the base link of the robot description used by the kinematics plugin" - tip: - type: string - description: "specifies the end effector link of the robot description used by the kinematics plugin" - alpha: - type: double - default_value: 0.0005 - description: "specifies the damping coefficient for the Jacobian pseudo inverse" - group_name: - type: string - description: "specifies the group name for planning with Moveit" - - ft_sensor: - name: - type: string - description: "name of the force torque sensor in the robot description" - frame: - id: - type: string - description: "frame of the force torque sensor" - external: - type: bool - default_value: false - description: "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip" - filter_coefficient: - type: double - default_value: 0.005 - description: "specifies the coefficient for the sensor's exponential filter" - - control: - frame: - id: - type: string - description: "control frame used for admittance control" - external: - type: bool - default_value: false - description: "specifies if the control frame is contained in the kinematics chain from the base to the tip" - - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - frame: - id: - type: string - description: "world frame, gravity points down (neg. Z) in this frame" - external: - type: bool - default_value: false - description: "specifies if the world frame is contained in the kinematics chain from the base to the tip" - - gravity_compensation: - frame: - id: - type: string - description: "frame which center of gravity (CoG) is defined in" - external: - type: bool - default_value: false - description: "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip" - CoG: # specifies the center of gravity of the end effector - pos: - type: double_array - description: "position of the center of gravity (CoG) in its frame" - validation: - fixed_size<>: 3 - force: - type: double - default_value: .NAN - description: "weight of the end effector, e.g mass * 9.81" - - admittance: - selected_axes: - type: bool_array - description: "specifies if the axes x, y, z, rx, ry, and rz are enabled" - validation: - fixed_size<>: 6 - - # Having ".0" at the end is MUST, otherwise there is a loading error - # F = M*a + D*v + S*(x - x_d) - mass: - type: double_array - description: "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation" - validation: - fixed_size<>: 6 - element_bounds<>: [ 0.0001, 1000000.0 ] - - damping_ratio: - type: double_array - description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. - The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))" - validation: - fixed_size<>: 6 - "custom_validators::validate_double_array_custom_func": [ 20.3, 5.0 ] - element_bounds<>: [ 0.1, 10.0 ] - - stiffness: - type: double_array - description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation" - validation: - element_bounds: [ 0.0001, 100000.0 ] - - # general settings - enable_parameter_update_without_reactivation: - type: bool - default_value: true - description: "if enabled, read_only parameters will be dynamically updated in the control loop" - use_feedforward_commanded_input: - type: bool - default_value: false - description: "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity" - lt_eq_fifteen: - type: int - default_value: 1 - description: "should be a number less than or equal to 15" - validation: - lt_eq<>: [ 15 ] - gt_fifteen: - type: int - default_value: 16 - description: "should be a number greater than 15" - validation: - gt<>: [ 15 ] - one_number: - type: int - default_value: 14540 - read_only: true - validation: - bounds<>: [ 1024, 65535 ] - three_numbers: - type: int_array - default_value: [3,4,5] - read_only: true - three_numbers_of_five: - type: int_array_fixed_5 - default_value: [3,3,3] - read_only: true diff --git a/example_cmake_python/cmake_generate_parameter_module_example/parameters.yaml b/example_cmake_python/cmake_generate_parameter_module_example/parameters.yaml new file mode 120000 index 0000000..7cee8b0 --- /dev/null +++ b/example_cmake_python/cmake_generate_parameter_module_example/parameters.yaml @@ -0,0 +1 @@ +../../example_python/generate_parameter_module_example/parameters.yaml \ No newline at end of file From 70836bfc726f8c36f8cb2f27eebb08544f6be4e7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 10 Jun 2026 14:55:16 +0000 Subject: [PATCH 3/3] Copy and translate the test_params_consistency test to c++ --- example/CMakeLists.txt | 5 + example/test/test_params_consistency.cpp | 555 +++++++++++++++++++++++ 2 files changed, 560 insertions(+) create mode 100644 example/test/test_params_consistency.cpp diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 2e6e295..21250cd 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -57,6 +57,11 @@ if(BUILD_TESTING) target_include_directories(test_example_gmock PRIVATE include) target_link_libraries(test_example_gmock admittance_controller_parameters rclcpp::rclcpp) + # test_params_consistency - GMock test verifying consistency between get_params() and ROS2 parameter server + ament_add_gmock(test_params_consistency test/test_params_consistency.cpp) + target_include_directories(test_params_consistency PRIVATE include) + target_link_libraries(test_params_consistency admittance_controller_parameters rclcpp::rclcpp) + find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(test/launch.test.py TIMEOUT 50 diff --git a/example/test/test_params_consistency.cpp b/example/test/test_params_consistency.cpp new file mode 100644 index 0000000..44822fc --- /dev/null +++ b/example/test/test_params_consistency.cpp @@ -0,0 +1,555 @@ +// Copyright 2026 Generate Parameter Library Contributors +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * 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. +// +// * Neither the name of the Generate Parameter Library contributors 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. + +/** + * Tests that verify consistency between get_params() (library internal state) + * and node.get_parameter() (ROS2 parameter server) for admittance_controller. + * + * After ParamListener::__init__() calls declare_params(), all parameters + * (static and dynamic map entries) are declared in the ROS2 node and read back + * via get_parameter(), then stored in params_ via update_internal_params(). + * This test verifies both sources remain in sync under default values and after + * updates via set_parameters(). + */ + +#include +#include +#include + +#include "generate_parameter_library_example/admittance_controller_parameters.hpp" +#include "gmock/gmock.h" +#include "rclcpp/rclcpp.hpp" + +class TestParamsConsistency : public ::testing::Test { + public: + static void SetUpTestSuite() { rclcpp::init(0, nullptr); } + + static void TearDownTestSuite() { rclcpp::shutdown(); } + + void SetUp() override { + // Create node with required parameters + node_ = std::make_shared( + "test_admittance_controller", + rclcpp::NodeOptions() + .automatically_declare_parameters_from_overrides(true) + .parameter_overrides({ + rclcpp::Parameter("fixed_string_no_default", "happy"), + rclcpp::Parameter( + "fixed_string_array_no_default", + std::vector{"alpha", "beta", "gamma", "delta", + "epsilon"}), + rclcpp::Parameter("command_interfaces", + std::vector{"position"}), + rclcpp::Parameter( + "state_interfaces", + std::vector{"position", "velocity"}), + rclcpp::Parameter( + "chainable_command_interfaces", + std::vector{"position", "velocity"}), + rclcpp::Parameter("kinematics.plugin_name", + "kdl_plugin/KDLKinematics"), + rclcpp::Parameter("kinematics.plugin_package", + "kinematics_interface_kdl"), + rclcpp::Parameter("kinematics.base", "base_link"), + rclcpp::Parameter("kinematics.tip", "ee_link"), + rclcpp::Parameter("kinematics.group_name", "manipulator"), + rclcpp::Parameter("ft_sensor.name", "tcp_fts_sensor"), + rclcpp::Parameter("ft_sensor.frame.id", "ee_link"), + rclcpp::Parameter("control.frame.id", "ee_link"), + rclcpp::Parameter("fixed_world_frame.frame.id", "base_link"), + rclcpp::Parameter("gravity_compensation.frame.id", "ee_link"), + rclcpp::Parameter("gravity_compensation.CoG.pos", + std::vector{0.1, 0.0, 0.0}), + rclcpp::Parameter( + "admittance.selected_axes", + std::vector{true, true, true, true, true, true}), + rclcpp::Parameter( + "admittance.mass", + std::vector{3.0, 3.0, 3.0, 3.0, 3.0, 3.0}), + rclcpp::Parameter( + "admittance.damping_ratio", + std::vector{2.828427, 2.828427, 2.828427, 2.828427, + 2.828427, 2.828427}), + rclcpp::Parameter( + "admittance.stiffness", + std::vector{50.0, 50.0, 50.0, 1.0, 1.0, 1.0}), + })); + + param_listener_ = std::make_shared( + node_->get_node_parameters_interface()); + params_ = param_listener_->get_params(); + } + + void TearDown() override { + node_.reset(); + param_listener_.reset(); + } + + protected: + std::shared_ptr node_; + std::shared_ptr param_listener_; + admittance_controller::Params params_; +}; + +// ========================================================================= +// Static / Flat Parameters Tests +// ========================================================================= + +TEST_F(TestParamsConsistency, ScientificNotationNum) { + EXPECT_DOUBLE_EQ(params_.scientific_notation_num, + node_->get_parameter("scientific_notation_num").as_double()); +} + +TEST_F(TestParamsConsistency, InterpolationMode) { + EXPECT_EQ(params_.interpolation_mode, + node_->get_parameter("interpolation_mode").as_string()); +} + +TEST_F(TestParamsConsistency, SubsetSelection) { + auto param_vec = node_->get_parameter("subset_selection").as_string_array(); + EXPECT_THAT(params_.subset_selection, ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, Joints) { + auto param_vec = node_->get_parameter("joints").as_string_array(); + EXPECT_THAT(params_.joints, ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, DofNames) { + auto param_vec = node_->get_parameter("dof_names").as_string_array(); + EXPECT_THAT(params_.dof_names, ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, FixedString) { + EXPECT_EQ(rsl::to_string(params_.fixed_string), + node_->get_parameter("fixed_string").as_string()); +} + +TEST_F(TestParamsConsistency, FixedArray) { + auto param_vec = node_->get_parameter("fixed_array").as_double_array(); + auto params_vec = rsl::to_vector(params_.fixed_array); + EXPECT_THAT(params_vec, ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, EnableParameterUpdateWithoutReactivation) { + EXPECT_EQ(params_.enable_parameter_update_without_reactivation, + node_->get_parameter("enable_parameter_update_without_reactivation") + .as_bool()); +} + +TEST_F(TestParamsConsistency, UseFeedforwardCommandedInput) { + EXPECT_EQ(params_.use_feedforward_commanded_input, + node_->get_parameter("use_feedforward_commanded_input").as_bool()); +} + +TEST_F(TestParamsConsistency, LtEqFifteen) { + EXPECT_EQ(params_.lt_eq_fifteen, + node_->get_parameter("lt_eq_fifteen").as_int()); +} + +TEST_F(TestParamsConsistency, GtFifteen) { + EXPECT_EQ(params_.gt_fifteen, node_->get_parameter("gt_fifteen").as_int()); +} + +TEST_F(TestParamsConsistency, OneNumber) { + EXPECT_EQ(params_.one_number, node_->get_parameter("one_number").as_int()); +} + +TEST_F(TestParamsConsistency, ThreeNumbers) { + auto param_vec = node_->get_parameter("three_numbers").as_integer_array(); + auto params_vec = params_.three_numbers; + EXPECT_THAT(params_vec, ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, ThreeNumbersOfFive) { + auto param_vec = + node_->get_parameter("three_numbers_of_five").as_integer_array(); + auto params_vec = rsl::to_vector(params_.three_numbers_of_five); + EXPECT_THAT(params_vec, ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, HoverOverride) { + EXPECT_EQ(params_.hover_override, + node_->get_parameter("hover_override").as_int()); +} + +TEST_F(TestParamsConsistency, AngleWraparound) { + EXPECT_EQ(params_.angle_wraparound, + node_->get_parameter("angle_wraparound").as_bool()); +} + +TEST_F(TestParamsConsistency, OpenLoopControl) { + EXPECT_EQ(params_.open_loop_control, + node_->get_parameter("open_loop_control").as_bool()); +} + +// ========================================================================= +// Nested Struct Parameters Tests +// ========================================================================= + +TEST_F(TestParamsConsistency, PidRate) { + EXPECT_DOUBLE_EQ(params_.pid.rate, + node_->get_parameter("pid.rate").as_double()); +} + +TEST_F(TestParamsConsistency, KinematicsAlpha) { + EXPECT_DOUBLE_EQ(params_.kinematics.alpha, + node_->get_parameter("kinematics.alpha").as_double()); +} + +TEST_F(TestParamsConsistency, FtSensorFilterCoefficient) { + EXPECT_DOUBLE_EQ( + params_.ft_sensor.filter_coefficient, + node_->get_parameter("ft_sensor.filter_coefficient").as_double()); +} + +TEST_F(TestParamsConsistency, FtSensorFrameExternal) { + EXPECT_EQ(params_.ft_sensor.frame.external, + node_->get_parameter("ft_sensor.frame.external").as_bool()); +} + +TEST_F(TestParamsConsistency, ControlFrameExternal) { + EXPECT_EQ(params_.control.frame.external, + node_->get_parameter("control.frame.external").as_bool()); +} + +TEST_F(TestParamsConsistency, FixedWorldFrameExternal) { + EXPECT_EQ(params_.fixed_world_frame.frame.external, + node_->get_parameter("fixed_world_frame.frame.external").as_bool()); +} + +TEST_F(TestParamsConsistency, GravityCompensationFrameExternal) { + EXPECT_EQ( + params_.gravity_compensation.frame.external, + node_->get_parameter("gravity_compensation.frame.external").as_bool()); +} + +TEST_F(TestParamsConsistency, GravityCompensationCogForceIsNan) { + double gravity_comp_force = params_.gravity_compensation.CoG.force; + double ros_param_force = + node_->get_parameter("gravity_compensation.CoG.force").as_double(); + EXPECT_TRUE(std::isnan(gravity_comp_force)); + EXPECT_TRUE(std::isnan(ros_param_force)); +} + +// ========================================================================= +// Dynamic Map Parameters Tests +// ========================================================================= + +TEST_F(TestParamsConsistency, MapJointsDofWeight) { + for (const auto& joint : params_.joints) { + for (const auto& dof : params_.dof_names) { + const std::string param_name = fmt::format("{}.{}.weight", joint, dof); + auto lib_value = params_.joints_map[joint].dof_names_map[dof].weight; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, ros_value) + << fmt::format("Mismatch for {}: get_params()={}, get_parameter()={}", + param_name, lib_value, ros_value); + } + } +} + +TEST_F(TestParamsConsistency, NestedDynamicMap) { + for (const auto& joint : params_.joints) { + for (const auto& dof : params_.dof_names) { + const std::string param_name = + fmt::format("nested_dynamic.{}.{}.nested", joint, dof); + auto lib_value = params_.nested_dynamic.joints_map.at(joint) + .dof_names_map.at(dof) + .nested; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, ros_value) + << fmt::format("Mismatch for {}: get_params()={}, get_parameter()={}", + param_name, lib_value, ros_value); + } + } +} + +TEST_F(TestParamsConsistency, PidMapJointsP) { + for (const auto& joint : params_.joints) { + const std::string param_name = fmt::format("pid.{}.p", joint); + auto lib_value = params_.pid.joints_map.at(joint).p; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, ros_value) + << fmt::format("Mismatch for {}: get_params()={}, get_parameter()={}", + param_name, lib_value, ros_value); + } +} + +TEST_F(TestParamsConsistency, PidMapJointsI) { + for (const auto& joint : params_.joints) { + const std::string param_name = fmt::format("pid.{}.i", joint); + auto lib_value = params_.pid.joints_map.at(joint).i; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, ros_value) + << fmt::format("Mismatch for {}: get_params()={}, get_parameter()={}", + param_name, lib_value, ros_value); + } +} + +TEST_F(TestParamsConsistency, PidMapJointsD) { + for (const auto& joint : params_.joints) { + const std::string param_name = fmt::format("pid.{}.d", joint); + auto lib_value = params_.pid.joints_map.at(joint).d; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, ros_value) + << fmt::format("Mismatch for {}: get_params()={}, get_parameter()={}", + param_name, lib_value, ros_value); + } +} + +TEST_F(TestParamsConsistency, GainMapDofK) { + for (const auto& dof : params_.dof_names) { + const std::string param_name = fmt::format("gains.{}.k", dof); + auto lib_value = params_.gains.dof_names_map.at(dof).k; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, ros_value) + << fmt::format("Mismatch for {}: get_params()={}, get_parameter()={}", + param_name, lib_value, ros_value); + } +} + +// ========================================================================= +// Consistency After set_parameters() Tests +// ========================================================================= + +TEST_F(TestParamsConsistency, UpdateInterpolationMode) { + const std::string new_value = "linear"; + node_->set_parameters({rclcpp::Parameter("interpolation_mode", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_EQ(updated_params.interpolation_mode, new_value); + EXPECT_EQ(updated_params.interpolation_mode, + node_->get_parameter("interpolation_mode").as_string()); +} + +TEST_F(TestParamsConsistency, UpdateLtEqFifteen) { + int64_t new_value = 10; + node_->set_parameters({rclcpp::Parameter("lt_eq_fifteen", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_EQ(updated_params.lt_eq_fifteen, new_value); + EXPECT_EQ(updated_params.lt_eq_fifteen, + node_->get_parameter("lt_eq_fifteen").as_int()); +} + +TEST_F(TestParamsConsistency, UpdateMapJointDofWeight) { + const auto& joint = params_.joints[0]; + const auto& dof = params_.dof_names[0]; + const std::string param_name = fmt::format("{}.{}.weight", joint, dof); + double new_value = 2.5; + node_->set_parameters({rclcpp::Parameter(param_name, new_value)}); + auto updated_params = param_listener_->get_params(); + auto lib_value = + updated_params.joints_map.at(joint).dof_names_map.at(dof).weight; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, new_value); + EXPECT_DOUBLE_EQ(lib_value, ros_value); +} + +TEST_F(TestParamsConsistency, UpdateBoolEnableParameterUpdate) { + bool new_value = false; + node_->set_parameters({rclcpp::Parameter( + "enable_parameter_update_without_reactivation", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_EQ(updated_params.enable_parameter_update_without_reactivation, + new_value); + EXPECT_EQ(updated_params.enable_parameter_update_without_reactivation, + node_->get_parameter("enable_parameter_update_without_reactivation") + .as_bool()); +} + +TEST_F(TestParamsConsistency, UpdateBoolAngleWraparound) { + bool new_value = true; + node_->set_parameters({rclcpp::Parameter("angle_wraparound", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_EQ(updated_params.angle_wraparound, new_value); + EXPECT_EQ(updated_params.angle_wraparound, + node_->get_parameter("angle_wraparound").as_bool()); +} + +TEST_F(TestParamsConsistency, UpdateBoolNestedFtSensorFrameExternal) { + bool new_value = true; + node_->set_parameters( + {rclcpp::Parameter("ft_sensor.frame.external", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_EQ(updated_params.ft_sensor.frame.external, new_value); + EXPECT_EQ(updated_params.ft_sensor.frame.external, + node_->get_parameter("ft_sensor.frame.external").as_bool()); +} + +TEST_F(TestParamsConsistency, UpdateDoubleScientificNotationNum) { + double new_value = 1.5e-4; + node_->set_parameters( + {rclcpp::Parameter("scientific_notation_num", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_DOUBLE_EQ(updated_params.scientific_notation_num, new_value); + EXPECT_DOUBLE_EQ(updated_params.scientific_notation_num, + node_->get_parameter("scientific_notation_num").as_double()); +} + +TEST_F(TestParamsConsistency, UpdateDoublePidRate) { + double new_value = 0.01; + node_->set_parameters({rclcpp::Parameter("pid.rate", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_DOUBLE_EQ(updated_params.pid.rate, new_value); + EXPECT_DOUBLE_EQ(updated_params.pid.rate, + node_->get_parameter("pid.rate").as_double()); +} + +TEST_F(TestParamsConsistency, UpdateDoubleArrayFixedArray) { + std::vector new_value{9.9, 8.8, 7.7, 6.6, 5.5}; + node_->set_parameters({rclcpp::Parameter("fixed_array", new_value)}); + auto updated_params = param_listener_->get_params(); + auto params_vec = rsl::to_vector(updated_params.fixed_array); + EXPECT_THAT(params_vec, ::testing::ElementsAreArray(new_value)); + auto param_vec = node_->get_parameter("fixed_array").as_double_array(); + EXPECT_THAT(params_vec, ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, UpdateDoubleArrayAdmittanceMass) { + std::vector new_value{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}; + node_->set_parameters({rclcpp::Parameter("admittance.mass", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_THAT(updated_params.admittance.mass, + ::testing::ElementsAreArray(new_value)); + auto param_vec = node_->get_parameter("admittance.mass").as_double_array(); + EXPECT_THAT(updated_params.admittance.mass, + ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, UpdateBoolArrayAdmittanceSelectedAxes) { + std::vector new_value{true, false, true, false, true, false}; + node_->set_parameters( + {rclcpp::Parameter("admittance.selected_axes", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_THAT(updated_params.admittance.selected_axes, + ::testing::ElementsAreArray(new_value)); + auto param_vec = + node_->get_parameter("admittance.selected_axes").as_bool_array(); + EXPECT_THAT(updated_params.admittance.selected_axes, + ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, UpdateStringArraySubsetSelection) { + std::vector new_value{"A"}; + node_->set_parameters({rclcpp::Parameter("subset_selection", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_THAT(updated_params.subset_selection, + ::testing::ElementsAreArray(new_value)); + auto param_vec = node_->get_parameter("subset_selection").as_string_array(); + EXPECT_THAT(updated_params.subset_selection, + ::testing::ElementsAreArray(param_vec)); +} + +TEST_F(TestParamsConsistency, UpdateIntGtFifteen) { + int64_t new_value = 42; + node_->set_parameters({rclcpp::Parameter("gt_fifteen", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_EQ(updated_params.gt_fifteen, new_value); + EXPECT_EQ(updated_params.gt_fifteen, + node_->get_parameter("gt_fifteen").as_int()); +} + +TEST_F(TestParamsConsistency, UpdateIntHoverOverride) { + int64_t new_value = 0; + node_->set_parameters({rclcpp::Parameter("hover_override", new_value)}); + auto updated_params = param_listener_->get_params(); + EXPECT_EQ(updated_params.hover_override, new_value); + EXPECT_EQ(updated_params.hover_override, + node_->get_parameter("hover_override").as_int()); +} + +TEST_F(TestParamsConsistency, UpdateMapPidJointP) { + const auto& joint = params_.joints[0]; + const std::string param_name = fmt::format("pid.{}.p", joint); + double new_value = 5.0; + node_->set_parameters({rclcpp::Parameter(param_name, new_value)}); + auto updated_params = param_listener_->get_params(); + auto lib_value = updated_params.pid.joints_map.at(joint).p; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, new_value); + EXPECT_DOUBLE_EQ(lib_value, ros_value); +} + +TEST_F(TestParamsConsistency, UpdateMapGainsDofK) { + const auto& dof = params_.dof_names[0]; + const std::string param_name = fmt::format("gains.{}.k", dof); + double new_value = 10.0; + node_->set_parameters({rclcpp::Parameter(param_name, new_value)}); + auto updated_params = param_listener_->get_params(); + auto lib_value = updated_params.gains.dof_names_map.at(dof).k; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, new_value); + EXPECT_DOUBLE_EQ(lib_value, ros_value); +} + +TEST_F(TestParamsConsistency, UpdateNestedDynamicMap) { + const auto& joint = params_.joints[0]; + const auto& dof = params_.dof_names[0]; + const std::string param_name = + fmt::format("nested_dynamic.{}.{}.nested", joint, dof); + double new_value = 3.14; + node_->set_parameters({rclcpp::Parameter(param_name, new_value)}); + auto updated_params = param_listener_->get_params(); + auto lib_value = updated_params.nested_dynamic.joints_map.at(joint) + .dof_names_map.at(dof) + .nested; + auto ros_value = node_->get_parameter(param_name).as_double(); + EXPECT_DOUBLE_EQ(lib_value, new_value); + EXPECT_DOUBLE_EQ(lib_value, ros_value); +} + +TEST_F(TestParamsConsistency, ParamsDoNotShareState) { + auto params1 = param_listener_->get_params(); + params1.pid.rate = 1.0; + auto params2 = param_listener_->get_params(); + params2.pid.rate = 2.0; + EXPECT_NE(params1.pid.rate, params2.pid.rate); + node_->set_parameters({rclcpp::Parameter("pid.rate", 3.0)}); + params2 = param_listener_->get_params(); + EXPECT_NE(params1.pid.rate, params2.pid.rate); +} + +TEST_F(TestParamsConsistency, MapsDoNotShareState) { + node_->set_parameters({ + rclcpp::Parameter("nested_map_struct.A.nested_struct.nested_struct_field", + "valueA"), + rclcpp::Parameter("nested_map_struct.B.nested_struct.nested_struct_field", + "valueB"), + }); + auto params = param_listener_->get_params(); + EXPECT_NE(params.nested_map_struct.nested_map_struct_entries_map.at("A") + .nested_struct.nested_struct_field, + params.nested_map_struct.nested_map_struct_entries_map.at("B") + .nested_struct.nested_struct_field); +} + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}