From 5e53146db44e4966262089a691c35d743789f2dc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 9 Dec 2024 20:42:24 +0100 Subject: [PATCH 1/2] Optimize the valid time check in the update loop (#1923) --- controller_manager/src/controller_manager.cpp | 44 +++++++++---------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 42542dec7c..d4b4076bb3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2366,6 +2366,26 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; + // Check for valid time + if ( + !get_clock()->started() && + time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + throw std::runtime_error( + "No clock received, and time argument is zero. Check your controller_manager node's " + "clock configuration (use_sim_time parameter) and if a valid clock source is " + "available. Also pass a proper time argument to the update method."); + } + else + { + // this can happen with use_sim_time=true until the /clock is received + rclcpp::Clock clock = rclcpp::Clock(); + RCLCPP_WARN_THROTTLE( + get_logger(), clock, 1000, + "No clock received, using time argument instead! Check your node's clock " + "configuration (use_sim_time parameter) and if a valid clock source is available"); + } + std::vector failed_controllers_list; for (const auto & loaded_controller : rt_controller_list) { @@ -2390,30 +2410,8 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - rclcpp::Time current_time; bool first_update_cycle = false; - if (get_clock()->started()) - { - current_time = get_clock()->now(); - } - else if ( - time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) - { - throw std::runtime_error( - "No clock received, and time argument is zero. Check your controller_manager node's " - "clock configuration (use_sim_time parameter) and if a valid clock source is " - "available. Also pass a proper time argument to the update method."); - } - else - { - // this can happen with use_sim_time=true until the /clock is received - rclcpp::Clock clock = rclcpp::Clock(); - RCLCPP_WARN_THROTTLE( - get_logger(), clock, 1000, - "No clock received, using time argument instead! Check your node's clock " - "configuration (use_sim_time parameter) and if a valid clock source is available"); - current_time = time; - } + const rclcpp::Time current_time = get_clock()->started() ? get_clock()->now() : time; if ( *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) From d40377d821d15b7d99a36e8a48c0aa75d994e21b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 9 Dec 2024 20:48:38 +0100 Subject: [PATCH 2/2] Let sensors also export state interfaces of joints (#1885) --- hardware_interface/CMakeLists.txt | 1 + .../hardware_interface/sensor_interface.hpp | 14 +- hardware_interface/package.xml | 1 + hardware_interface/src/resource_manager.cpp | 5 +- .../test/test_component_interfaces.cpp | 145 ++++++++++++++++-- ...est_component_interfaces_custom_export.cpp | 17 +- .../components_urdfs.hpp | 19 ++- 7 files changed, 172 insertions(+), 30 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 385ae96fb1..f9637b4f07 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() 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/package.xml b/hardware_interface/package.xml index ed661c5380..23b882ab02 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 diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index d338250e4f..e77917af9d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -615,11 +615,10 @@ class ResourceStorage auto interfaces = hardware.export_state_interfaces(); const auto interface_names = add_state_interfaces(interfaces); - RCLCPP_WARN( - get_logger(), + 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()); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 9ca46ee0f8..28a74aeb66 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -271,7 +271,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; } @@ -332,15 +332,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 @@ -352,7 +409,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; } @@ -371,6 +429,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 @@ -907,9 +966,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 @@ -948,29 +1007,83 @@ 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("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("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())); } // 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; 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())); // 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()); + + // 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::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_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())); + + 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())); + + // Not updated because is is UNCONFIGURED + 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(); + 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()); // 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 @@ -1708,7 +1821,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 b64ea81bc8..62e2b703cf 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -245,21 +245,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 + + + + +