From 52810237fb3545c1b7b04ee35fde8dfd15f3afb3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 24 Nov 2024 22:38:10 +0000 Subject: [PATCH 1/7] Let sensor export joints too --- .../hardware_interface/sensor_interface.hpp | 14 +- .../test/test_component_interfaces.cpp | 145 ++++++++++++++++-- ...est_component_interfaces_custom_export.cpp | 17 +- .../components_urdfs.hpp | 19 ++- 4 files changed, 169 insertions(+), 26 deletions(-) diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 74c1d04bd7..455f6216a2 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -123,6 +123,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); return CallbackReturn::SUCCESS; }; @@ -179,7 +180,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::vector state_interfaces; state_interfaces.reserve( - unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); + unlisted_interface_descriptions.size() + sensor_state_interfaces_.size() + + joint_state_interfaces_.size()); // add InterfaceDescriptions and create StateInterfaces from the descriptions and add to maps. for (const auto & description : unlisted_interface_descriptions) @@ -201,6 +203,14 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI state_interfaces.push_back(std::const_pointer_cast(state_interface)); } + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + return state_interfaces; } @@ -274,10 +284,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; // interface names to InterfaceDescription + std::unordered_map joint_state_interfaces_; std::unordered_map sensor_state_interfaces_; std::unordered_map unlisted_state_interfaces_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector joint_states_; std::vector sensor_states_; std::vector unlisted_states_; diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 9b90d81dfd..1f3cf54a76 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -270,7 +270,7 @@ class DummySensor : public hardware_interface::SensorInterface // We can read some voltage level std::vector state_interfaces; state_interfaces.emplace_back( - hardware_interface::StateInterface("joint1", "voltage", &voltage_level_)); + hardware_interface::StateInterface("sens1", "voltage", &voltage_level_)); return state_interfaces; } @@ -331,15 +331,72 @@ class DummySensorDefault : public hardware_interface::SensorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { - for (const auto & [name, descr] : sensor_state_interfaces_) + set_state("sens1/voltage", 0.0); + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("sens1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySensorJointDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { - set_state(name, 0.0); + return hardware_interface::CallbackReturn::ERROR; } + + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 10.0); + set_state("sens1/voltage", 0.0); read_calls_ = 0; return CallbackReturn::SUCCESS; } - std::string get_name() const override { return "DummySensorDefault"; } + std::string get_name() const override { return "DummySensorJointDefault"; } hardware_interface::return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -351,7 +408,8 @@ class DummySensorDefault : public hardware_interface::SensorInterface } // no-op, static value - set_state("joint1/voltage", voltage_level_hw_value_); + set_state("joint1/position", position_hw_value_); + set_state("sens1/voltage", voltage_level_hw_value_); return hardware_interface::return_type::OK; } @@ -370,6 +428,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface } private: + double position_hw_value_ = 0x777; double voltage_level_hw_value_ = 0x666; // Helper variables to initiate error on read @@ -903,9 +962,9 @@ TEST(TestComponentInterfaces, dummy_sensor) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("sens1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("sens1", state_interfaces[0]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Not updated because is is UNCONFIGURED @@ -943,29 +1002,85 @@ TEST(TestComponentInterfaces, dummy_sensor_default) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/voltage"); + auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + + // Not updated because is is UNCONFIGURED + auto si_sens1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_joint) +{ + hardware_interface::Sensor sensor_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_joint_voltage_sensor + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo sensor_res = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(sensor_res, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + { + auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); EXPECT_TRUE(contains); - EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ("position", state_interfaces[position]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } // Not updated because is is UNCONFIGURED - auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; + auto si_sens1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_value()); } // BEGIN (Handle export change): for backward compatibility @@ -1694,7 +1809,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values - auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; + auto si_joint1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; state = sensor_hw.configure(); EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index ab6f490b92..506b9d1690 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -242,21 +242,20 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/voltage"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); + ASSERT_TRUE(contains); + EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } { auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + test_components::vector_contains(state_interfaces, "sens1/some_unlisted_interface"); + ASSERT_TRUE(contains); + EXPECT_EQ("sens1/some_unlisted_interface", state_interfaces[position]->get_name()); EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); } } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index c887ca3251..46feee39a9 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -636,7 +636,24 @@ const auto valid_urdf_ros2_control_voltage_sensor_only = ros2_control_demo_hardware/SingleJointVoltageSensor 2 - + + + + +)"; + +// Joint+Voltage Sensor +const auto valid_urdf_ros2_control_joint_voltage_sensor = + R"( + + + ros2_control_demo_hardware/SingleJointVoltageSensor + 2 + + + + + From 66a6ffecd45945615ec86ffc841aad05cd9b23bb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 24 Nov 2024 23:01:09 +0000 Subject: [PATCH 2/7] Fix wrong warning --- hardware_interface/src/resource_manager.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e5445491c8..efdf0ea347 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -611,11 +611,13 @@ class ResourceStorage auto interfaces = hardware.export_state_interfaces(); const auto interface_names = add_state_interfaces(interfaces); - RCLCPP_WARN( - get_logger(), - "Importing state interfaces for the hardware '%s' returned no state interfaces.", - hardware.get_name().c_str()); - + if (interface_names.empty()) + { + RCLCPP_WARN( + get_logger(), + "Importing state interfaces for the hardware '%s' returned no state interfaces.", + hardware.get_name().c_str()); + } hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); From 1125e2eacdf7675b8ea3940fa8159fbb8575596d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 25 Nov 2024 21:14:21 +0100 Subject: [PATCH 3/7] Use RCLCPP_WARN_EXPRESSION Co-authored-by: Sai Kishor Kothakota --- hardware_interface/src/resource_manager.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index efdf0ea347..971700fb9e 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -611,13 +611,10 @@ class ResourceStorage auto interfaces = hardware.export_state_interfaces(); const auto interface_names = add_state_interfaces(interfaces); - if (interface_names.empty()) - { - RCLCPP_WARN( - get_logger(), - "Importing state interfaces for the hardware '%s' returned no state interfaces.", - hardware.get_name().c_str()); - } + RCLCPP_WARN_EXPRESSION( + get_logger(), interface_names.empty(), + "Importing state interfaces for the hardware '%s' returned no state interfaces.", + hardware.get_name().c_str()); hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); From dc9b67b53f19c2deac8e89f457d3e21c7cfecedc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 5 Dec 2024 20:08:09 +0000 Subject: [PATCH 4/7] Add backward ros to hardware_interface --- hardware_interface/CMakeLists.txt | 83 ++++++++++++++++--------------- hardware_interface/package.xml | 1 + 2 files changed, 43 insertions(+), 41 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 385ae96fb1..de03015b97 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -21,6 +21,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS find_package(ament_cmake REQUIRED) find_package(ament_cmake_gen_version_h REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -64,54 +65,54 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_macros test/test_macros.cpp) - target_include_directories(test_macros PRIVATE include) - ament_target_dependencies(test_macros rcpputils) + # ament_add_gmock(test_macros test/test_macros.cpp) + # target_include_directories(test_macros PRIVATE include) + # ament_target_dependencies(test_macros rcpputils) - ament_add_gmock(test_inst_hardwares test/test_inst_hardwares.cpp) - target_link_libraries(test_inst_hardwares hardware_interface) - ament_target_dependencies(test_inst_hardwares rcpputils) + # ament_add_gmock(test_inst_hardwares test/test_inst_hardwares.cpp) + # target_link_libraries(test_inst_hardwares hardware_interface) + # ament_target_dependencies(test_inst_hardwares rcpputils) - ament_add_gmock(test_joint_handle test/test_handle.cpp) - target_link_libraries(test_joint_handle hardware_interface) - ament_target_dependencies(test_joint_handle rcpputils) + # ament_add_gmock(test_joint_handle test/test_handle.cpp) + # target_link_libraries(test_joint_handle hardware_interface) + # ament_target_dependencies(test_joint_handle rcpputils) ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) target_link_libraries(test_component_interfaces hardware_interface) ament_target_dependencies(test_component_interfaces ros2_control_test_assets) - ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) - target_link_libraries(test_component_interfaces_custom_export hardware_interface) - ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) - - ament_add_gmock(test_component_parser test/test_component_parser.cpp) - target_link_libraries(test_component_parser hardware_interface) - ament_target_dependencies(test_component_parser ros2_control_test_assets) - - add_library(test_hardware_components SHARED - test/test_hardware_components/test_single_joint_actuator.cpp - test/test_hardware_components/test_force_torque_sensor.cpp - test/test_hardware_components/test_imu_sensor.cpp - test/test_hardware_components/test_two_joint_system.cpp - test/test_hardware_components/test_system_with_command_modes.cpp - ) - target_link_libraries(test_hardware_components hardware_interface) - ament_target_dependencies(test_hardware_components - pluginlib) - install(TARGETS test_hardware_components - DESTINATION lib - ) - pluginlib_export_plugin_description_file( - hardware_interface test/test_hardware_components/test_hardware_components.xml - ) - - ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) - target_include_directories(test_generic_system PRIVATE include) - target_link_libraries(test_generic_system hardware_interface) - ament_target_dependencies(test_generic_system - pluginlib - ros2_control_test_assets - ) + # ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) + # target_link_libraries(test_component_interfaces_custom_export hardware_interface) + # ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + + # ament_add_gmock(test_component_parser test/test_component_parser.cpp) + # target_link_libraries(test_component_parser hardware_interface) + # ament_target_dependencies(test_component_parser ros2_control_test_assets) + + # add_library(test_hardware_components SHARED + # test/test_hardware_components/test_single_joint_actuator.cpp + # test/test_hardware_components/test_force_torque_sensor.cpp + # test/test_hardware_components/test_imu_sensor.cpp + # test/test_hardware_components/test_two_joint_system.cpp + # test/test_hardware_components/test_system_with_command_modes.cpp + # ) + # target_link_libraries(test_hardware_components hardware_interface) + # ament_target_dependencies(test_hardware_components + # pluginlib) + # install(TARGETS test_hardware_components + # DESTINATION lib + # ) + # pluginlib_export_plugin_description_file( + # hardware_interface test/test_hardware_components/test_hardware_components.xml + # ) + + # ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) + # target_include_directories(test_generic_system PRIVATE include) + # target_link_libraries(test_generic_system hardware_interface) + # ament_target_dependencies(test_generic_system + # pluginlib + # ros2_control_test_assets + # ) endif() install( diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index dbe9ad5750..0e42840d41 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -10,6 +10,7 @@ ament_cmake ament_cmake_gen_version_h + backward_ros control_msgs lifecycle_msgs pluginlib From 8c979e1da5d8f0858663e72d0ddcccfb2da402a6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 5 Dec 2024 20:08:33 +0000 Subject: [PATCH 5/7] Fix test due to changes of #1570 --- .../test/test_component_interfaces.cpp | 51 ++++++++++--------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 9ca5b1737e..f43af8f49a 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1043,42 +1043,43 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo sensor_res = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); - auto state = sensor_hw.initialize(sensor_res, logger, nullptr); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(sensor_res, node->get_logger(), node->get_node_clock_interface()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + ASSERT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - { - auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage"); - EXPECT_TRUE(contains); - EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); - EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); - EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); - } - { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/position"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); - EXPECT_EQ("position", state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); - } + std::cout << "test 1" << std::endl; + auto [contains_sens1_vol, si_sens1_vol] = + test_components::vector_contains(state_interfaces, "sens1/voltage"); + ASSERT_TRUE(contains_sens1_vol); + EXPECT_EQ("sens1/voltage", state_interfaces[si_sens1_vol]->get_name()); + EXPECT_EQ("voltage", state_interfaces[si_sens1_vol]->get_interface_name()); + EXPECT_EQ("sens1", state_interfaces[si_sens1_vol]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + std::cout << "test 2" << std::endl; + + auto [contains_joint1_pos, si_joint1_pos] = + test_components::vector_contains(state_interfaces, "joint1/position"); + ASSERT_TRUE(contains_joint1_pos); + EXPECT_EQ("joint1/position", state_interfaces[si_joint1_pos]->get_name()); + EXPECT_EQ("position", state_interfaces[si_joint1_pos]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[si_joint1_pos]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); + + std::cout << "test 3" << std::endl; // Not updated because is is UNCONFIGURED - auto si_sens1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; - auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; sensor_hw.read(TIME, PERIOD); EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value()); From ab0311ddb232a8a84d09297a6d608cf0cdfa8477 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 5 Dec 2024 20:09:59 +0000 Subject: [PATCH 6/7] Readd deactivated tests --- hardware_interface/CMakeLists.txt | 82 +++++++++++++++---------------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index de03015b97..f9637b4f07 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -65,54 +65,54 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) - # ament_add_gmock(test_macros test/test_macros.cpp) - # target_include_directories(test_macros PRIVATE include) - # ament_target_dependencies(test_macros rcpputils) + ament_add_gmock(test_macros test/test_macros.cpp) + target_include_directories(test_macros PRIVATE include) + ament_target_dependencies(test_macros rcpputils) - # ament_add_gmock(test_inst_hardwares test/test_inst_hardwares.cpp) - # target_link_libraries(test_inst_hardwares hardware_interface) - # ament_target_dependencies(test_inst_hardwares rcpputils) + ament_add_gmock(test_inst_hardwares test/test_inst_hardwares.cpp) + target_link_libraries(test_inst_hardwares hardware_interface) + ament_target_dependencies(test_inst_hardwares rcpputils) - # ament_add_gmock(test_joint_handle test/test_handle.cpp) - # target_link_libraries(test_joint_handle hardware_interface) - # ament_target_dependencies(test_joint_handle rcpputils) + ament_add_gmock(test_joint_handle test/test_handle.cpp) + target_link_libraries(test_joint_handle hardware_interface) + ament_target_dependencies(test_joint_handle rcpputils) ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) target_link_libraries(test_component_interfaces hardware_interface) ament_target_dependencies(test_component_interfaces ros2_control_test_assets) - # ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) - # target_link_libraries(test_component_interfaces_custom_export hardware_interface) - # ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) - - # ament_add_gmock(test_component_parser test/test_component_parser.cpp) - # target_link_libraries(test_component_parser hardware_interface) - # ament_target_dependencies(test_component_parser ros2_control_test_assets) - - # add_library(test_hardware_components SHARED - # test/test_hardware_components/test_single_joint_actuator.cpp - # test/test_hardware_components/test_force_torque_sensor.cpp - # test/test_hardware_components/test_imu_sensor.cpp - # test/test_hardware_components/test_two_joint_system.cpp - # test/test_hardware_components/test_system_with_command_modes.cpp - # ) - # target_link_libraries(test_hardware_components hardware_interface) - # ament_target_dependencies(test_hardware_components - # pluginlib) - # install(TARGETS test_hardware_components - # DESTINATION lib - # ) - # pluginlib_export_plugin_description_file( - # hardware_interface test/test_hardware_components/test_hardware_components.xml - # ) - - # ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) - # target_include_directories(test_generic_system PRIVATE include) - # target_link_libraries(test_generic_system hardware_interface) - # ament_target_dependencies(test_generic_system - # pluginlib - # ros2_control_test_assets - # ) + ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) + target_link_libraries(test_component_interfaces_custom_export hardware_interface) + ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + + ament_add_gmock(test_component_parser test/test_component_parser.cpp) + target_link_libraries(test_component_parser hardware_interface) + ament_target_dependencies(test_component_parser ros2_control_test_assets) + + add_library(test_hardware_components SHARED + test/test_hardware_components/test_single_joint_actuator.cpp + test/test_hardware_components/test_force_torque_sensor.cpp + test/test_hardware_components/test_imu_sensor.cpp + test/test_hardware_components/test_two_joint_system.cpp + test/test_hardware_components/test_system_with_command_modes.cpp + ) + target_link_libraries(test_hardware_components hardware_interface) + ament_target_dependencies(test_hardware_components + pluginlib) + install(TARGETS test_hardware_components + DESTINATION lib + ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_hardware_components/test_hardware_components.xml + ) + + ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) + target_include_directories(test_generic_system PRIVATE include) + target_link_libraries(test_generic_system hardware_interface) + ament_target_dependencies(test_generic_system + pluginlib + ros2_control_test_assets + ) endif() install( From b0db6cc65bc2d68f21b55874715f75ccb37c0d28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 5 Dec 2024 21:45:05 +0100 Subject: [PATCH 7/7] Remove debug output Co-authored-by: Sai Kishor Kothakota --- hardware_interface/test/test_component_interfaces.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index f43af8f49a..20f0bfe930 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1052,7 +1052,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - std::cout << "test 1" << std::endl; auto [contains_sens1_vol, si_sens1_vol] = test_components::vector_contains(state_interfaces, "sens1/voltage"); ASSERT_TRUE(contains_sens1_vol); @@ -1060,7 +1059,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) EXPECT_EQ("voltage", state_interfaces[si_sens1_vol]->get_interface_name()); EXPECT_EQ("sens1", state_interfaces[si_sens1_vol]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); - std::cout << "test 2" << std::endl; auto [contains_joint1_pos, si_joint1_pos] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -1070,7 +1068,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) EXPECT_EQ("joint1", state_interfaces[si_joint1_pos]->get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); - std::cout << "test 3" << std::endl; // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value()));