From b45260e3fbfcc2d4f92fe447bac91e799c37da90 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:04:51 +0000 Subject: [PATCH 01/26] Add first draft of parsing mimic info from URDF --- hardware_interface/CMakeLists.txt | 1 + .../hardware_interface/hardware_info.hpp | 23 +- hardware_interface/package.xml | 1 + hardware_interface/src/component_parser.cpp | 86 +++- .../test/test_component_parser.cpp | 73 ++++ .../ros2_control_test_assets/descriptions.hpp | 373 ++++++++++++++++++ 6 files changed, 552 insertions(+), 5 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f6a4890cba..4ea7fdc2f7 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils TinyXML2 tinyxml2_vendor + urdf ) find_package(ament_cmake REQUIRED) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eb6b63cfc3..4769c3bb58 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -44,6 +44,15 @@ struct InterfaceInfo int size; }; +/// @brief This structure stores information about a joint that is mimicking another joint +struct MimicJoint +{ + std::size_t joint_index; + std::size_t mimicked_joint_index; + double multiplier = 1.0; + double offset = 0.0; +}; + /** * This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -55,6 +64,8 @@ struct ComponentInfo /// Type of the component: sensor, joint, or GPIO. std::string type; + /// If the component is a mimic joint + bool is_mimic; /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -116,22 +127,26 @@ struct HardwareInfo /// (Optional) Key-value pairs for hardware parameters. std::unordered_map hardware_parameters; /** - * Map of joints provided by the hardware where the key is the joint name. + * Vector of joints provided by the hardware. * Required for Actuator and System Hardware. */ std::vector joints; /** - * Map of sensors provided by the hardware where the key is the joint or link name. + * Vector of mimic joints. + */ + std::vector mimic_joints; + /** + * Vector of sensors provided by the hardware. * Required for Sensor and optional for System Hardware. */ std::vector sensors; /** - * Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO. + * Vector of GPIOs provided by the hardware. * Optional for any hardware components. */ std::vector gpios; /** - * Map of transmissions to calculate ration between joints and physical actuators. + * Vector of transmissions to calculate ratio between joints and physical actuators. * Optional for Actuator and System Hardware. */ std::vector transmissions; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 194a7369da..e531b9c90f 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + urdf rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 4f67d3e8b6..8c3211082b 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -21,6 +21,8 @@ #include #include +#include "urdf/model.h" + #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/lexical_casts.hpp" @@ -42,6 +44,7 @@ constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; constexpr const auto kInitialValueTag = "initial_value"; +constexpr const auto kMimicAttribute = "mimic"; constexpr const auto kDataTypeAttribute = "data_type"; constexpr const auto kSizeAttribute = "size"; constexpr const auto kNameAttribute = "name"; @@ -99,6 +102,29 @@ std::string get_attribute_value( return element_it->Attribute(attribute_name); } +/// Gets value of the attribute on an XMLelement. +/** + * If parameter is not found, returns specified default value + * + * \param[in] element_it XMLElement iterator to search for the attribute + * \param[in] attribute_name attribute name to search for and return value + * \param[in] default_value When the attribute is not found, this value is returned instead + * \return attribute value + * \throws std::runtime_error if attribute is not found + */ +std::string get_attribute_value_or( + const tinyxml2::XMLElement * element_it, const char * attribute_name, + const std::string default_value) +{ + const tinyxml2::XMLAttribute * attr; + attr = element_it->FindAttribute(attribute_name); + if (!attr) + { + return default_value; + } + return element_it->Attribute(attribute_name); +} + /// Gets value of the attribute on an XMLelement. /** * If attribute is not found throws an error. @@ -305,7 +331,7 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( * \param[in] component_it pointer to the iterator where component * info should be found * \return ComponentInfo filled with information about component - * \throws std::runtime_error if a component attribute or tag is not found + * \throws std::runtime_error if a component attribute or tag is not found or false configuration */ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) { @@ -315,8 +341,20 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it component.type = component_it->Name(); component.name = get_attribute_value(component_it, kNameAttribute, component.type); + if (!component.type.compare(kJointTag)) + { + std::string mimic_str = get_attribute_value_or(component_it, kMimicAttribute, "false"); + component.is_mimic = mimic_str.compare("true") == 0; + } + // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); + if (component.is_mimic && command_interfaces_it) + { + throw std::runtime_error( + "Component " + std::string(component.name) + + " has mimic attribute set to true: Mimic joints cannot have command interfaces."); + } while (command_interfaces_it) { component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); @@ -611,6 +649,52 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } + // parse full URDF for mimic options + urdf::Model model; + if (!model.initString(urdf)) + { + throw std::runtime_error("Failed to parse URDF file"); + } + for (auto & hw_info : hardware_info) + { + for (auto joint : hw_info.joints) + { + auto urdf_joint = model.getJoint(joint.name); + if (!urdf_joint) + { + throw std::runtime_error("Joint " + joint.name + " not found in URDF"); + } + if (joint.is_mimic) + { + if (urdf_joint->mimic) + { + auto find_joint = [&hw_info](const std::string & name) + { + auto it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&name](const auto & j) { return j.name == name; }); + if (it == hw_info.joints.end()) + { + throw std::runtime_error("Joint `" + name + "` not found in hw_info.joints"); + } + return std::distance(hw_info.joints.begin(), it); + }; + + MimicJoint mimic_joint; + mimic_joint.joint_index = find_joint(joint.name); + mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); + mimic_joint.multiplier = urdf_joint->mimic->multiplier; + mimic_joint.offset = urdf_joint->mimic->offset; + hw_info.mimic_joints.push_back(mimic_joint); + } + else + { + throw std::runtime_error("Joint `" + joint.name + "` has no mimic information in URDF"); + } + } + } + } + return hardware_info; } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index b0c7c5a16d..d1d3d1e38e 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -674,3 +674,76 @@ TEST_F(TestComponentParser, transmission_given_too_many_joints_throws_error) ros2_control_test_assets::urdf_tail; ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, mimic_true_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 1.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 0.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + +TEST_F(TestComponentParser, mimic_with_unknown_joint_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_unknown_joint) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, mimic_true_without_mimic_info_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_no_mimic) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, mimic_true_invalid_config_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, mimic_false_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_false_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(0)); +} + +TEST_F(TestComponentParser, urdf_two_base_links_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_two_base_links) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, urdf_incomplete_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head_incomplete) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index a2f6195c67..ed67d6f2a4 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -453,6 +453,379 @@ const auto diffbot_urdf = )"; +const auto gripper_urdf_head = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_no_mimic = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_unknown_joint = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_incomplete = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_urdf_head_two_base_links = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto gripper_hardware_resources_mimic_true_no_command_if = + R"( + + + + + + + + + + + + + )"; + +const auto gripper_hardware_resources_mimic_true_command_if = + R"( + + + + + + + + + + + + + + )"; + +const auto gripper_hardware_resources_mimic_false_command_if = + R"( + + + + + + + + + + + + + + )"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_unitilizable_robot_urdf = From e7c27896f5395abaedf4597fd94a8199961e83df Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:04:51 +0000 Subject: [PATCH 02/26] Fix configurations with non-joints via joint tag -> gpio --- .../mock_components/test_generic_system.cpp | 4 +- .../test/test_components/test_system.cpp | 30 ++++++--------- .../ros2_control_test_assets/descriptions.hpp | 38 ++++++++++++++++--- 3 files changed, 47 insertions(+), 25 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 0641cfda57..be304c41c7 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -142,12 +142,12 @@ class TestGenericSystem : public ::testing::Test 0.2 - + 0.5 - + )"; diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 6eed347299..7b088009f2 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -32,22 +32,19 @@ class TestSystem : public SystemInterface std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - if (info_.joints[i].name != "configuration") - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); - } + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); } - if (info_.joints.size() > 2) + if (info_.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[2].name, info_.joints[2].state_interfaces[0].name, &configuration_state_)); + info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); } return state_interfaces; @@ -58,22 +55,19 @@ class TestSystem : public SystemInterface std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - if (info_.joints[i].name != "configuration") - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); - } + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); } // Add max_acceleration command interface command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_acceleration_command_)); - if (info_.joints.size() > 2) + if (info_.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[2].name, info_.joints[2].command_interfaces[0].name, &configuration_command_)); + info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); } return command_interfaces; diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index ed67d6f2a4..be1d0e831a 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -113,6 +113,34 @@ const auto urdf_head = + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -169,10 +197,10 @@ const auto hardware_resources = - + - + )"; @@ -858,10 +886,10 @@ const auto minimal_robot_missing_command_keys_urdf = [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system"; [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_PLUGIN_NAME = "test_system"; [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = { - "joint2/velocity", "joint2/max_acceleration", "joint3/velocity", "configuration/max_tcp_jerk"}; + "joint2/velocity", "joint2/max_acceleration", "joint3/velocity"}; [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_STATE_INTERFACES = { - "joint2/position", "joint2/velocity", "joint2/acceleration", "joint3/position", - "joint3/velocity", "joint3/acceleration", "configuration/max_tcp_jerk"}; + "joint2/position", "joint2/velocity", "joint2/acceleration", + "joint3/position", "joint3/velocity", "joint3/acceleration"}; } // namespace ros2_control_test_assets From 72c242e3da5fda51fc6013e78e0e444f166d6b2e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:04:51 +0000 Subject: [PATCH 03/26] Generic_system: Use mimic info from hardware_info --- .../mock_components/generic_system.hpp | 8 -- hardware_interface/src/component_parser.cpp | 90 +++++++++++----- .../src/mock_components/generic_system.cpp | 32 +----- .../mock_components/test_generic_system.cpp | 22 ++-- .../ros2_control_test_assets/descriptions.hpp | 102 ++++++++++++++++++ 5 files changed, 177 insertions(+), 77 deletions(-) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e9b38de65d..fbb8547ab1 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -72,14 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - struct MimicJoint - { - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - }; - std::vector mimic_joints_; - /// The size of this vector is (standard_interfaces_.size() x nr_joints) std::vector> joint_commands_; std::vector> joint_states_; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 8c3211082b..34ded800ea 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -657,39 +657,77 @@ std::vector parse_control_resources_from_urdf(const std::string & } for (auto & hw_info : hardware_info) { - for (auto joint : hw_info.joints) + for (auto i = 0u; i < hw_info.joints.size(); ++i) { - auto urdf_joint = model.getJoint(joint.name); - if (!urdf_joint) + const auto & joint = hw_info.joints.at(i); + + // Search for mimic joints defined in ros2_control tag (deprecated) + if (joint.parameters.find("mimic") != joint.parameters.cend()) { - throw std::runtime_error("Joint " + joint.name + " not found in URDF"); + std::cerr << "Warning: Mimic joints defined in ros2_control tag are deprecated. " + << "Please define mimic joints in URDF." << std::endl; + const auto mimicked_joint_it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&mimicked_joint = + joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) + { return joint_info.name == mimicked_joint; }); + if (mimicked_joint_it == hw_info.joints.cend()) + { + throw std::runtime_error( + std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); + } + hardware_interface::MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.multiplier = 1.0; + mimic_joint.offset = 0.0; + mimic_joint.mimicked_joint_index = std::distance(hw_info.joints.begin(), mimicked_joint_it); + auto param_it = joint.parameters.find("multiplier"); + if (param_it != joint.parameters.end()) + { + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); + } + param_it = joint.parameters.find("offset"); + if (param_it != joint.parameters.end()) + { + mimic_joint.offset = hardware_interface::stod(joint.parameters.at("offset")); + } + hw_info.mimic_joints.push_back(mimic_joint); + hw_info.joints.at(i).is_mimic = true; } - if (joint.is_mimic) + else { - if (urdf_joint->mimic) + auto urdf_joint = model.getJoint(joint.name); + if (!urdf_joint) { - auto find_joint = [&hw_info](const std::string & name) - { - auto it = std::find_if( - hw_info.joints.begin(), hw_info.joints.end(), - [&name](const auto & j) { return j.name == name; }); - if (it == hw_info.joints.end()) - { - throw std::runtime_error("Joint `" + name + "` not found in hw_info.joints"); - } - return std::distance(hw_info.joints.begin(), it); - }; - - MimicJoint mimic_joint; - mimic_joint.joint_index = find_joint(joint.name); - mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); - mimic_joint.multiplier = urdf_joint->mimic->multiplier; - mimic_joint.offset = urdf_joint->mimic->offset; - hw_info.mimic_joints.push_back(mimic_joint); + throw std::runtime_error("Joint " + joint.name + " not found in URDF"); } - else + if (joint.is_mimic) { - throw std::runtime_error("Joint `" + joint.name + "` has no mimic information in URDF"); + if (urdf_joint->mimic) + { + auto find_joint = [&hw_info](const std::string & name) + { + auto it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&name](const auto & j) { return j.name == name; }); + if (it == hw_info.joints.end()) + { + throw std::runtime_error("Joint `" + name + "` not found in hw_info.joints"); + } + return std::distance(hw_info.joints.begin(), it); + }; + + MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); + mimic_joint.multiplier = urdf_joint->mimic->multiplier; + mimic_joint.offset = urdf_joint->mimic->offset; + hw_info.mimic_joints.push_back(mimic_joint); + } + else + { + throw std::runtime_error("Joint `" + joint.name + "` has no mimic information in URDF"); + } } } } diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 22d8aa573c..5e5165e34f 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -17,7 +17,6 @@ #include "mock_components/generic_system.hpp" #include -#include #include #include #include @@ -137,34 +136,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - // Search for mimic joints - for (auto i = 0u; i < info_.joints.size(); ++i) - { - const auto & joint = info_.joints.at(i); - if (joint.parameters.find("mimic") != joint.parameters.cend()) - { - const auto mimicked_joint_it = std::find_if( - info_.joints.begin(), info_.joints.end(), - [&mimicked_joint = - joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) - { return joint_info.name == mimicked_joint; }); - if (mimicked_joint_it == info_.joints.cend()) - { - throw std::runtime_error( - std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); - } - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = std::distance(info_.joints.begin(), mimicked_joint_it); - auto param_it = joint.parameters.find("multiplier"); - if (param_it != joint.parameters.end()) - { - mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); - } - mimic_joints_.push_back(mimic_joint); - } - } - // search for non-standard joint interfaces for (const auto & joint : info_.joints) { @@ -594,11 +565,12 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur mirror_command_to_state(joint_states_, joint_commands_, 1); } - for (const auto & mimic_joint : mimic_joints_) + for (const auto & mimic_joint : info_.mimic_joints) { for (auto i = 0u; i < joint_states_.size(); ++i) { joint_states_[i][mimic_joint.joint_index] = + mimic_joint.offset + mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; } } diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index be304c41c7..ac89dc1553 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -251,11 +251,7 @@ class TestGenericSystem : public ::testing::Test - - joint1 - -2 - - + @@ -1207,11 +1203,9 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( EXPECT_TRUE(rm.state_interface_exists("joint2/position")); EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); - ASSERT_EQ(4u, rm.command_interface_keys().size()); + ASSERT_EQ(2u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.command_interface_exists("joint2/position")); - EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -1261,7 +1255,7 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ + + auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mimic_joint(urdf, "MockHardwareSystem"); @@ -1918,10 +1912,11 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) { - auto check_prepare_command_mode_switch = [&](const std::string & urdf) + auto check_prepare_command_mode_switch = + [&]( + const std::string & urdf, const std::string & urdf_head = ros2_control_test_assets::urdf_head) { - TestableResourceManager rm( - ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); + TestableResourceManager rm(urdf_head + urdf + ros2_control_test_assets::urdf_tail); rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); rm.set_component_state("MockHardwareSystem", state); auto start_interfaces = rm.command_interface_keys(); @@ -1937,7 +1932,8 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); ASSERT_TRUE( check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_)); - ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_with_mimic_joint_, ros2_control_test_assets::urdf_head_mimic)); ASSERT_TRUE( check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_)); ASSERT_TRUE(check_prepare_command_mode_switch( diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index be1d0e831a..4c7d0c7dc9 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -149,6 +149,108 @@ const auto urdf_head = )"; +const auto urdf_head_mimic = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; const auto urdf_tail = R"( From 8e0d1d464fff8feaae4a8b11fd82ef2e7625f9ee Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:04:51 +0000 Subject: [PATCH 04/26] Fix diff_drive URDF --- ...ontrollers_chaining_with_controller_manager.cpp | 14 +++++++------- .../ros2_control_test_assets/descriptions.hpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index eadca39756..e52715604c 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -128,18 +128,18 @@ class TestControllerChainingWithControllerManager // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_0_joint/velocity"}}; controller_interface::InterfaceConfiguration pid_left_state_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_0_joint/velocity"}}; pid_left_wheel_controller->set_command_interface_configuration(pid_left_cmd_ifs_cfg); pid_left_wheel_controller->set_state_interface_configuration(pid_left_state_ifs_cfg); pid_left_wheel_controller->set_reference_interface_names({"velocity"}); // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_1_joint/velocity"}}; controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_1_joint/velocity"}}; pid_right_wheel_controller->set_command_interface_configuration(pid_right_cmd_ifs_cfg); pid_right_wheel_controller->set_state_interface_configuration(pid_right_state_ifs_cfg); pid_right_wheel_controller->set_reference_interface_names({"velocity"}); @@ -150,7 +150,7 @@ class TestControllerChainingWithControllerManager {std::string(PID_LEFT_WHEEL) + "/velocity", std::string(PID_RIGHT_WHEEL) + "/velocity"}}; controller_interface::InterfaceConfiguration diff_drive_state_ifs_cfg = { controller_interface::interface_configuration_type::INDIVIDUAL, - {"wheel_left/velocity", "wheel_right/velocity"}}; + {"wheel_0_joint/velocity", "wheel_1_joint/velocity"}}; diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); @@ -435,8 +435,8 @@ class TestControllerChainingWithControllerManager const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; - const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"}; - const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"}; + const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_0_joint/velocity"}; + const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_1_joint/velocity"}; const std::vector DIFF_DRIVE_CLAIMED_INTERFACES = { "pid_left_wheel_controller/velocity", "pid_right_wheel_controller/velocity"}; const std::vector POSITION_CONTROLLER_CLAIMED_INTERFACES = { diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 4c7d0c7dc9..12f1004ebb 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -564,7 +564,7 @@ const auto diffbot_urdf = test_actuator - + @@ -574,7 +574,7 @@ const auto diffbot_urdf = test_actuator - + From 27969950ce05ed80196639e9f530b8cfd6fc8764 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:04:51 +0000 Subject: [PATCH 05/26] Use valid URDFs for transmission tests --- transmission_interface/CMakeLists.txt | 4 + transmission_interface/package.xml | 1 + .../differential_transmission_loader_test.cpp | 25 +--- ...r_bar_linkage_transmission_loader_test.cpp | 25 +--- .../test/simple_transmission_loader_test.cpp | 127 ++---------------- 5 files changed, 29 insertions(+), 153 deletions(-) diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 60d2075956..98dcdcd192 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -31,6 +31,7 @@ pluginlib_export_plugin_description_file(transmission_interface ros2_control_plu if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_simple_transmission test/simple_transmission_test.cpp @@ -51,16 +52,19 @@ if(BUILD_TESTING) test/simple_transmission_loader_test.cpp ) target_link_libraries(test_simple_transmission_loader transmission_interface) + ament_target_dependencies(test_simple_transmission_loader ros2_control_test_assets) ament_add_gmock(test_four_bar_linkage_transmission_loader test/four_bar_linkage_transmission_loader_test.cpp ) target_link_libraries(test_four_bar_linkage_transmission_loader transmission_interface) + ament_target_dependencies(test_four_bar_linkage_transmission_loader ros2_control_test_assets) ament_add_gmock(test_differential_transmission_loader test/differential_transmission_loader_test.cpp ) target_link_libraries(test_differential_transmission_loader transmission_interface) + ament_target_dependencies(test_differential_transmission_loader ros2_control_test_assets) ament_add_gmock( test_utils diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index d77b82f5bd..fcd2c60709 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -15,6 +15,7 @@ pluginlib ament_cmake_gmock + ros2_control_test_assets ament_cmake diff --git a/transmission_interface/test/differential_transmission_loader_test.cpp b/transmission_interface/test/differential_transmission_loader_test.cpp index 70d0adf2cd..946c4703b5 100644 --- a/transmission_interface/test/differential_transmission_loader_test.cpp +++ b/transmission_interface/test/differential_transmission_loader_test.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/differential_transmission.hpp" #include "transmission_interface/differential_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" @@ -57,9 +58,7 @@ class TransmissionPluginLoader TEST(DifferentialTransmissionLoaderTest, FullSpec) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -132,9 +131,7 @@ TEST(DifferentialTransmissionLoaderTest, FullSpec) TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -203,9 +200,7 @@ TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -266,9 +261,7 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -338,9 +331,7 @@ TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -413,9 +404,7 @@ TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) TEST(DifferentialTransmissionLoaderTest, mech_red_invalid_value) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( diff --git a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp index 53b584b7b5..720cad68b4 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/four_bar_linkage_transmission.hpp" #include "transmission_interface/four_bar_linkage_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" @@ -57,9 +58,7 @@ class TransmissionPluginLoader TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -133,9 +132,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -205,9 +202,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -269,9 +264,7 @@ TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -342,9 +335,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -418,9 +409,7 @@ TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) TEST(FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value) { // Parse transmission info - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( diff --git a/transmission_interface/test/simple_transmission_loader_test.cpp b/transmission_interface/test/simple_transmission_loader_test.cpp index 4623d8c409..968f64c6e8 100644 --- a/transmission_interface/test/simple_transmission_loader_test.cpp +++ b/transmission_interface/test/simple_transmission_loader_test.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "transmission_interface/simple_transmission.hpp" #include "transmission_interface/simple_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" @@ -58,106 +59,8 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) { // Parse transmission info - std::string urdf_to_test = - R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + R"( ros2_control_demo_hardware/VelocityActuatorHardware @@ -239,9 +142,7 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -285,9 +186,7 @@ TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -327,9 +226,7 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -369,9 +266,7 @@ TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) TEST(SimpleTransmissionLoaderTest, offset_ill_defined) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( @@ -413,11 +308,9 @@ TEST(SimpleTransmissionLoaderTest, offset_ill_defined) TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) { - std::string urdf_to_test = R"( - - + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"( - + -1 1 @@ -426,7 +319,7 @@ TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) transmission_interface/SimpleTransmission - + 0 From c620831f66d919e692d0db33eceb4adfc8cfd50f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:04:51 +0000 Subject: [PATCH 06/26] Fix srv and spawner tests --- controller_manager/CMakeLists.txt | 1 + .../include/ros2_control_test_assets/descriptions.hpp | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e267856eb1..50de6ccbba 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -189,6 +189,7 @@ if(BUILD_TESTING) test_controller ros2_control_test_assets::ros2_control_test_assets ) + set_tests_properties(test_spawner_unspawner PROPERTIES TIMEOUT 120) ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 12f1004ebb..40a75a49bb 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -988,10 +988,10 @@ const auto minimal_robot_missing_command_keys_urdf = [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system"; [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_PLUGIN_NAME = "test_system"; [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = { - "joint2/velocity", "joint2/max_acceleration", "joint3/velocity"}; + "joint2/velocity", "joint2/max_acceleration", "joint3/velocity", "configuration/max_tcp_jerk"}; [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_STATE_INTERFACES = { - "joint2/position", "joint2/velocity", "joint2/acceleration", - "joint3/position", "joint3/velocity", "joint3/acceleration"}; + "joint2/position", "joint2/velocity", "joint2/acceleration", "joint3/position", + "joint3/velocity", "joint3/acceleration", "configuration/max_tcp_jerk"}; } // namespace ros2_control_test_assets From 8b0e39f910e12361e3dfc13520881cf280c5c3e9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 07/26] Update docs --- hardware_interface/doc/mock_components_userdoc.rst | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index c075fdeafc..9f72e21a27 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -19,7 +19,7 @@ For more information about hardware components check :ref:`detailed documentatio Features: - - support for mimic joints + - support for mimic joints, which is parsed from the URDF (see the `URDF wiki `__) - mirroring commands to states with and without offset - fake command interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) @@ -39,21 +39,9 @@ mock_sensor_commands (optional; boolean; default: false) position_state_following_offset (optional; double; default: 0.0) Following offset added to the commanded values when mirrored to states. - custom_interface_with_following_offset (optional; string; default: "") Mapping of offsetted commands to a custom interface. - -Per-joint Parameters -,,,,,,,,,,,,,,,,,,,, - -mimic (optional; string) - Defined name of the joint to mimic. This is often used concept with parallel grippers. Example: ``joint1``. - - -multiplier (optional; double; default: 1; used if mimic joint is defined) - Multiplier of values for mimicking joint defined in ``mimic`` parameter. Example: ``-2``. - Per-interface Parameters ,,,,,,,,,,,,,,,,,,,,,,,, From 431f6a217f1f9ea2c7041edd7671568268769475 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 08/26] Increase test coverage for deprecated config structure --- hardware_interface/src/component_parser.cpp | 9 +++-- .../test/test_component_parser.cpp | 29 ++++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 40 +++++++++++++++++++ 3 files changed, 74 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 34ded800ea..abd8061621 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -352,8 +352,8 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it if (component.is_mimic && command_interfaces_it) { throw std::runtime_error( - "Component " + std::string(component.name) + - " has mimic attribute set to true: Mimic joints cannot have command interfaces."); + "Component '" + std::string(component.name) + + "' has mimic attribute set to true: Mimic joints cannot have command interfaces."); } while (command_interfaces_it) { @@ -662,6 +662,7 @@ std::vector parse_control_resources_from_urdf(const std::string & const auto & joint = hw_info.joints.at(i); // Search for mimic joints defined in ros2_control tag (deprecated) + // TODO(christophfroehlich) delete deprecated config with ROS-J if (joint.parameters.find("mimic") != joint.parameters.cend()) { std::cerr << "Warning: Mimic joints defined in ros2_control tag are deprecated. " @@ -712,7 +713,7 @@ std::vector parse_control_resources_from_urdf(const std::string & [&name](const auto & j) { return j.name == name; }); if (it == hw_info.joints.end()) { - throw std::runtime_error("Joint `" + name + "` not found in hw_info.joints"); + throw std::runtime_error("Joint '" + name + "' not found in hw_info.joints"); } return std::distance(hw_info.joints.begin(), it); }; @@ -726,7 +727,7 @@ std::vector parse_control_resources_from_urdf(const std::string & } else { - throw std::runtime_error("Joint `" + joint.name + "` has no mimic information in URDF"); + throw std::runtime_error("Joint '" + joint.name + "' has no mimic information in URDF"); } } } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index d1d3d1e38e..29d1ab2365 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -691,6 +691,35 @@ TEST_F(TestComponentParser, mimic_true_valid_config) EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } +// TODO(christophfroehlich) delete deprecated config test +TEST_F(TestComponentParser, mimic_deprecated_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 1.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 0.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + +TEST_F(TestComponentParser, mimic_deprecated_unknown_joint_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string( + ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated_unknown_joint) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} +// end delete deprecated config test + TEST_F(TestComponentParser, mimic_with_unknown_joint_throws_error) { const auto urdf_to_test = diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 40a75a49bb..107d5d28e5 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -922,6 +922,46 @@ const auto gripper_hardware_resources_mimic_true_no_command_if = )"; +// TODO(christophfroehlich) delete deprecated config test +const auto gripper_hardware_resources_mimic_deprecated = + R"( + + + + + + + + + right_finger_joint + 1 + + + + + + )"; + +const auto gripper_hardware_resources_mimic_deprecated_unknown_joint = + R"( + + + + + + + + + middle_finger_joint + 1 + + + + + + )"; +// end delete deprecated config test + const auto gripper_hardware_resources_mimic_true_command_if = R"( From ee70607a61772a8d10f039b3422822f4a13fa780 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 09/26] Add offset to deprecated config test --- .../include/ros2_control_test_assets/descriptions.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 107d5d28e5..921b700970 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -935,6 +935,7 @@ const auto gripper_hardware_resources_mimic_deprecated = right_finger_joint 1 + 1 From 585b16752ec43d6ee4c0f6bffdfd119276b946ee Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 10/26] Fix test criteria --- hardware_interface/test/test_component_parser.cpp | 8 ++++---- .../include/ros2_control_test_assets/descriptions.hpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 29d1ab2365..cbf81ee3ab 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -685,8 +685,8 @@ TEST_F(TestComponentParser, mimic_true_valid_config) ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); ASSERT_THAT(hw_info, SizeIs(1)); ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); - EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 1.0); - EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 0.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } @@ -702,8 +702,8 @@ TEST_F(TestComponentParser, mimic_deprecated_valid_config) ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); ASSERT_THAT(hw_info, SizeIs(1)); ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); - EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 1.0); - EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 0.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 921b700970..c9dcca9d00 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -645,7 +645,7 @@ const auto gripper_urdf_head = - + @@ -934,7 +934,7 @@ const auto gripper_hardware_resources_mimic_deprecated = right_finger_joint - 1 + 2 1 From 01d7b5980b031a3cd42a1dacdf6494da35816132 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 11/26] Change mimic attribute to default true --- .../hardware_interface/hardware_info.hpp | 5 +- hardware_interface/src/component_parser.cpp | 71 ++++++++++--------- .../test/test_component_parser.cpp | 30 ++++++-- .../ros2_control_test_assets/descriptions.hpp | 16 +++++ 4 files changed, 81 insertions(+), 41 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 4769c3bb58..97926030c3 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#include #include #include #include @@ -64,8 +65,8 @@ struct ComponentInfo /// Type of the component: sensor, joint, or GPIO. std::string type; - /// If the component is a mimic joint - bool is_mimic; + /// If the component has a mimic joint tag, for opt-out + std::optional is_mimic{}; /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index abd8061621..013038452b 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -343,18 +343,20 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it if (!component.type.compare(kJointTag)) { - std::string mimic_str = get_attribute_value_or(component_it, kMimicAttribute, "false"); - component.is_mimic = mimic_str.compare("true") == 0; + try + { + component.is_mimic = + parse_bool(get_attribute_value(component_it, kMimicAttribute, kJointTag)); + } + catch (const std::runtime_error & e) + { + // mimic attribute not set + component.is_mimic = {}; + } } // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); - if (component.is_mimic && command_interfaces_it) - { - throw std::runtime_error( - "Component '" + std::string(component.name) + - "' has mimic attribute set to true: Mimic joints cannot have command interfaces."); - } while (command_interfaces_it) { component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); @@ -693,7 +695,6 @@ std::vector parse_control_resources_from_urdf(const std::string & mimic_joint.offset = hardware_interface::stod(joint.parameters.at("offset")); } hw_info.mimic_joints.push_back(mimic_joint); - hw_info.joints.at(i).is_mimic = true; } else { @@ -702,33 +703,39 @@ std::vector parse_control_resources_from_urdf(const std::string & { throw std::runtime_error("Joint " + joint.name + " not found in URDF"); } - if (joint.is_mimic) + if (!urdf_joint->mimic && joint.is_mimic.value_or(false)) + { + throw std::runtime_error( + "Joint '" + std::string(joint.name) + "' has no mimic information in the URDF."); + } + if (urdf_joint->mimic && joint.is_mimic.value_or(true)) { - if (urdf_joint->mimic) + if (joint.command_interfaces.size() > 0) { - auto find_joint = [&hw_info](const std::string & name) - { - auto it = std::find_if( - hw_info.joints.begin(), hw_info.joints.end(), - [&name](const auto & j) { return j.name == name; }); - if (it == hw_info.joints.end()) - { - throw std::runtime_error("Joint '" + name + "' not found in hw_info.joints"); - } - return std::distance(hw_info.joints.begin(), it); - }; - - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); - mimic_joint.multiplier = urdf_joint->mimic->multiplier; - mimic_joint.offset = urdf_joint->mimic->offset; - hw_info.mimic_joints.push_back(mimic_joint); + throw std::runtime_error( + "Joint '" + std::string(joint.name) + + "' has mimic attribute not set to false: Mimic joints cannot have command " + "interfaces."); } - else + auto find_joint = [&hw_info](const std::string & name) { - throw std::runtime_error("Joint '" + joint.name + "' has no mimic information in URDF"); - } + auto it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&name](const auto & j) { return j.name == name; }); + if (it == hw_info.joints.end()) + { + throw std::runtime_error( + "Mimic joint '" + name + "' not found in tag"); + } + return std::distance(hw_info.joints.begin(), it); + }; + + MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); + mimic_joint.multiplier = urdf_joint->mimic->multiplier; + mimic_joint.offset = urdf_joint->mimic->offset; + hw_info.mimic_joints.push_back(mimic_joint); } } } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index cbf81ee3ab..5c880b9571 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -675,7 +675,7 @@ TEST_F(TestComponentParser, transmission_given_too_many_joints_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_true_valid_config) +TEST_F(TestComponentParser, gripper_mimic_true_valid_config) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -691,8 +691,24 @@ TEST_F(TestComponentParser, mimic_true_valid_config) EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } +TEST_F(TestComponentParser, gripper_no_mimic_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + // TODO(christophfroehlich) delete deprecated config test -TEST_F(TestComponentParser, mimic_deprecated_valid_config) +TEST_F(TestComponentParser, gripper_mimic_deprecated_valid_config) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -708,7 +724,7 @@ TEST_F(TestComponentParser, mimic_deprecated_valid_config) EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } -TEST_F(TestComponentParser, mimic_deprecated_unknown_joint_throws_error) +TEST_F(TestComponentParser, gripper_mimic_deprecated_unknown_joint_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -720,7 +736,7 @@ TEST_F(TestComponentParser, mimic_deprecated_unknown_joint_throws_error) } // end delete deprecated config test -TEST_F(TestComponentParser, mimic_with_unknown_joint_throws_error) +TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head_unknown_joint) + @@ -729,7 +745,7 @@ TEST_F(TestComponentParser, mimic_with_unknown_joint_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_true_without_mimic_info_throws_error) +TEST_F(TestComponentParser, gripper_mimic_true_without_mimic_info_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head_no_mimic) + @@ -738,7 +754,7 @@ TEST_F(TestComponentParser, mimic_true_without_mimic_info_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_true_invalid_config_throws_error) +TEST_F(TestComponentParser, gripper_mimic_true_invalid_config_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -747,7 +763,7 @@ TEST_F(TestComponentParser, mimic_true_invalid_config_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_false_valid_config) +TEST_F(TestComponentParser, gripper_mimic_false_valid_config) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index c9dcca9d00..e2b22adba5 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -906,6 +906,22 @@ const auto gripper_urdf_head_two_base_links = )"; +const auto gripper_hardware_resources_no_command_if = + R"( + + + + + + + + + + + + + )"; + const auto gripper_hardware_resources_mimic_true_no_command_if = R"( From 0848426e8f8a1fe36ec5e6e90c5786d71efdce1a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 12/26] Update docs on URDF and robot_description --- controller_manager/doc/userdoc.rst | 10 +++++++++- .../doc/hardware_interface_types_userdoc.rst | 2 ++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 46781d461c..6af78ea8fd 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -45,6 +45,15 @@ Alternatives to the standard kernel include Though installing a realtime-kernel will definitely get the best results when it comes to low jitter, using a lowlatency kernel can improve things a lot with being really easy to install. +Subscribers +----------- + +~/robot_description [std_msgs::msg::String] + String with the URDF xml, e.g., from ``robot_state_publisher``. + Reloading of the URDF is not supported yet. + All joints defined in the ````-tag have to be present in the URDF. + + Parameters ----------- @@ -74,7 +83,6 @@ update_rate (mandatory; integer) The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controller and writes commands to hardware. - .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index cd1e475b20..ecf852cb94 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -14,6 +14,8 @@ Joints ````-tag groups the interfaces associated with the joints of physical robots and actuators. They have command and state interfaces to set the goal values for hardware and read its current state. +All joints defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `. + State interfaces of joints can be published as a ROS topic by means of the :ref:`joint_state_broadcaster ` Sensors From 5d41f1b38a98bc003e3bb476f1b00192cc0abe23 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 13/26] Revert increase of timeout --- controller_manager/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 50de6ccbba..e267856eb1 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -189,7 +189,6 @@ if(BUILD_TESTING) test_controller ros2_control_test_assets::ros2_control_test_assets ) - set_tests_properties(test_spawner_unspawner PROPERTIES TIMEOUT 120) ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp From 78235f7993a3aaccdecb55fcd7a9f5d30a7fc0e3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 14/26] Fix description of new test and fix typo --- .../test/test_components/test_actuator.cpp | 4 ++-- .../test/test_components/test_components.xml | 12 +++++----- .../test/test_components/test_sensor.cpp | 4 ++-- .../test/test_components/test_system.cpp | 4 ++-- .../test/test_resource_manager.cpp | 8 +++---- .../test/test_resource_manager.hpp | 2 +- .../ros2_control_test_assets/descriptions.hpp | 22 ++++++++----------- 7 files changed, 26 insertions(+), 30 deletions(-) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 41f27e201b..9a93db29b4 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -136,7 +136,7 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; -class TestUnitilizableActuator : public TestActuator +class TestUnitializableActuator : public TestActuator { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -147,4 +147,4 @@ class TestUnitilizableActuator : public TestActuator #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitializableActuator, hardware_interface::ActuatorInterface) diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index f029d3dd37..a4d66ef012 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -18,21 +18,21 @@ - + - Test Unitilizable Actuator + Test Unitializable Actuator - + - Test Unitilizable Sensor + Test Unitializable Sensor - + - Test Unitilizable System + Test Unitializable System diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 4811244138..149c4fa125 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -55,7 +55,7 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; -class TestUnitilizableSensor : public TestSensor +class TestUnitializableSensor : public TestSensor { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -66,4 +66,4 @@ class TestUnitilizableSensor : public TestSensor #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitializableSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 7b088009f2..f51e8c83e9 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -117,7 +117,7 @@ class TestSystem : public SystemInterface double configuration_command_ = 0.0; }; -class TestUnitilizableSystem : public TestSystem +class TestUnitializableSystem : public TestSystem { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -128,4 +128,4 @@ class TestUnitilizableSystem : public TestSystem #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitializableSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 4bb9e0c5fe..e39bdf67f4 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -101,22 +101,22 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf) ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) +TEST_F(ResourceManagerTest, test_unitializable_hardware_validation) { // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a // runtime exception is thrown TestableResourceManager rm; ASSERT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), + rm.load_urdf(ros2_control_test_assets::minimal_unitializable_robot_urdf, true), std::runtime_error); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +TEST_F(ResourceManagerTest, test_unitializable_hardware_no_validation) { // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, // the interface should not show up TestableResourceManager rm; - EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitializable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index 46a487f2ef..b20623b1f9 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -48,7 +48,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); - FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); + FRIEND_TEST(ResourceManagerTest, test_unitializable_hardware_no_validation); TestableResourceManager() : hardware_interface::ResourceManager() {} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index e2b22adba5..d1860e5689 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -306,11 +306,11 @@ const auto hardware_resources = )"; -const auto unitilizable_hardware_resources = +const auto unitializable_hardware_resources = R"( - + - test_unitilizable_actuator + test_unitializable_actuator @@ -319,9 +319,9 @@ const auto unitilizable_hardware_resources = - + - test_unitilizable_sensor + test_unitializable_sensor 2 2 @@ -329,9 +329,9 @@ const auto unitilizable_hardware_resources = - + - test_unitilizable_system + test_unitializable_system 2 2 @@ -348,10 +348,6 @@ const auto unitilizable_hardware_resources = - - - - )"; @@ -1015,8 +1011,8 @@ const auto gripper_hardware_resources_mimic_false_command_if = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); -const auto minimal_unitilizable_robot_urdf = - std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); +const auto minimal_unitializable_robot_urdf = + std::string(urdf_head) + std::string(unitializable_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + From ceb946a2108f9e1896497377a278b9d4197d17ff Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 15/26] Fix description --- hardware_interface/src/component_parser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 013038452b..34cfa7813b 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -331,7 +331,7 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( * \param[in] component_it pointer to the iterator where component * info should be found * \return ComponentInfo filled with information about component - * \throws std::runtime_error if a component attribute or tag is not found or false configuration + * \throws std::runtime_error if a component attribute or tag is not found */ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) { From aeb99e4b72be0bb217d0665b616ab17bc8a112be Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 16/26] Add comments to test code --- hardware_interface/test/test_component_parser.cpp | 11 +++++++++-- .../include/ros2_control_test_assets/descriptions.hpp | 2 +- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 5c880b9571..f67f91308d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -775,15 +775,22 @@ TEST_F(TestComponentParser, gripper_mimic_false_valid_config) ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(0)); } -TEST_F(TestComponentParser, urdf_two_base_links_throws_error) +/** + * @brief Test that the parser throws an error if the URDF contains a link with no parent. + */ +TEST_F(TestComponentParser, urdf_two_root_links_throws_error) { const auto urdf_to_test = - std::string(ros2_control_test_assets::gripper_urdf_head_two_base_links) + + std::string(ros2_control_test_assets::gripper_urdf_head_invalid_two_root_links) + std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) + std::string(ros2_control_test_assets::urdf_tail); ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } +/** + * @brief Test that the parser throws an error if a joint defined in the ros2_control tag is missing + * in the URDF + */ TEST_F(TestComponentParser, urdf_incomplete_throws_error) { const auto urdf_to_test = diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index d1860e5689..7c66aed94c 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -839,7 +839,7 @@ const auto gripper_urdf_head_incomplete = )"; -const auto gripper_urdf_head_two_base_links = +const auto gripper_urdf_head_invalid_two_root_links = R"( From 92b195b732d559d6766cb30a2883724c9fe479fe Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 17/26] Use enum instead of std::optional --- .../hardware_interface/hardware_info.hpp | 14 ++++++-- hardware_interface/src/component_parser.cpp | 36 ++++--------------- 2 files changed, 18 insertions(+), 32 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 97926030c3..97abc00438 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,7 +15,6 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ -#include #include #include #include @@ -54,6 +53,14 @@ struct MimicJoint double offset = 0.0; }; +/// @brief This enum is used to store the mimic attribute of a joint +enum class MimicAttribute +{ + NOT_SET, + TRUE, + FALSE +}; + /** * This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -65,8 +72,9 @@ struct ComponentInfo /// Type of the component: sensor, joint, or GPIO. std::string type; - /// If the component has a mimic joint tag, for opt-out - std::optional is_mimic{}; + /// Hold the value of the mimic attribute if given, NOT_SET otherwise + MimicAttribute is_mimic = MimicAttribute::NOT_SET; + /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 34cfa7813b..308d03b4dd 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -102,29 +102,6 @@ std::string get_attribute_value( return element_it->Attribute(attribute_name); } -/// Gets value of the attribute on an XMLelement. -/** - * If parameter is not found, returns specified default value - * - * \param[in] element_it XMLElement iterator to search for the attribute - * \param[in] attribute_name attribute name to search for and return value - * \param[in] default_value When the attribute is not found, this value is returned instead - * \return attribute value - * \throws std::runtime_error if attribute is not found - */ -std::string get_attribute_value_or( - const tinyxml2::XMLElement * element_it, const char * attribute_name, - const std::string default_value) -{ - const tinyxml2::XMLAttribute * attr; - attr = element_it->FindAttribute(attribute_name); - if (!attr) - { - return default_value; - } - return element_it->Attribute(attribute_name); -} - /// Gets value of the attribute on an XMLelement. /** * If attribute is not found throws an error. @@ -345,13 +322,14 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it { try { - component.is_mimic = - parse_bool(get_attribute_value(component_it, kMimicAttribute, kJointTag)); + component.is_mimic = parse_bool(get_attribute_value(component_it, kMimicAttribute, kJointTag)) + ? MimicAttribute::TRUE + : MimicAttribute::FALSE; } catch (const std::runtime_error & e) { // mimic attribute not set - component.is_mimic = {}; + component.is_mimic = MimicAttribute::NOT_SET; } } @@ -387,7 +365,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it * and the interface may be an array of a fixed size of the data type. * * \param[in] component_it pointer to the iterator where component - * info should befound + * info should be found * \throws std::runtime_error if a required component attribute or tag is not found. */ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * component_it) @@ -703,12 +681,12 @@ std::vector parse_control_resources_from_urdf(const std::string & { throw std::runtime_error("Joint " + joint.name + " not found in URDF"); } - if (!urdf_joint->mimic && joint.is_mimic.value_or(false)) + if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE) { throw std::runtime_error( "Joint '" + std::string(joint.name) + "' has no mimic information in the URDF."); } - if (urdf_joint->mimic && joint.is_mimic.value_or(true)) + if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE) { if (joint.command_interfaces.size() > 0) { From 721f9bd3acc953c8db78ee1b63e84cde8844e55f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 18/26] Make std::string comparison more readable --- hardware_interface/src/component_parser.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 308d03b4dd..d7ac3a5613 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -318,7 +318,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it component.type = component_it->Name(); component.name = get_attribute_value(component_it, kNameAttribute, component.type); - if (!component.type.compare(kJointTag)) + if (std::string(kJointTag) == component.type) { try { @@ -548,7 +548,7 @@ HardwareInfo parse_resource_from_xml( const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(); while (ros2_control_child_it) { - if (!std::string(kHardwareTag).compare(ros2_control_child_it->Name())) + if (std::string(kHardwareTag) == ros2_control_child_it->Name()) { const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag); hardware.hardware_plugin_name = @@ -559,19 +559,19 @@ HardwareInfo parse_resource_from_xml( hardware.hardware_parameters = parse_parameters_from_xml(params_it); } } - else if (!std::string(kJointTag).compare(ros2_control_child_it->Name())) + else if (std::string(kJointTag) == ros2_control_child_it->Name()) { hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it)); } - else if (!std::string(kSensorTag).compare(ros2_control_child_it->Name())) + else if (std::string(kSensorTag) == ros2_control_child_it->Name()) { hardware.sensors.push_back(parse_component_from_xml(ros2_control_child_it)); } - else if (!std::string(kGPIOTag).compare(ros2_control_child_it->Name())) + else if (std::string(kGPIOTag) == ros2_control_child_it->Name()) { hardware.gpios.push_back(parse_complex_component_from_xml(ros2_control_child_it)); } - else if (!std::string(kTransmissionTag).compare(ros2_control_child_it->Name())) + else if (std::string(kTransmissionTag) == ros2_control_child_it->Name()) { hardware.transmissions.push_back(parse_transmission_from_xml(ros2_control_child_it)); } @@ -611,7 +611,7 @@ std::vector parse_control_resources_from_urdf(const std::string & // Find robot tag const tinyxml2::XMLElement * robot_it = doc.RootElement(); - if (std::string(kRobotTag).compare(robot_it->Name())) + if (std::string(kRobotTag) != robot_it->Name()) { throw std::runtime_error("the robot tag is not root element in URDF"); } From eb568f1600ac6a9a7b84cd4f9de3404dee63f699 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 19/26] Remove unnecessary std::string() Co-authored-by: Sai Kishor Kothakota --- hardware_interface/src/component_parser.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index d7ac3a5613..0841265e81 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -655,7 +655,7 @@ std::vector parse_control_resources_from_urdf(const std::string & if (mimicked_joint_it == hw_info.joints.cend()) { throw std::runtime_error( - std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); + "Mimicked joint '" + joint.parameters.at("mimic") + "' not found"); } hardware_interface::MimicJoint mimic_joint; mimic_joint.joint_index = i; @@ -684,15 +684,15 @@ std::vector parse_control_resources_from_urdf(const std::string & if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE) { throw std::runtime_error( - "Joint '" + std::string(joint.name) + "' has no mimic information in the URDF."); + "Joint '" + joint.name + "' has no mimic information in the URDF."); } if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE) { if (joint.command_interfaces.size() > 0) { throw std::runtime_error( - "Joint '" + std::string(joint.name) + - "' has mimic attribute not set to false: Mimic joints cannot have command " + "Joint '" + joint.name + + "' has mimic attribute not set to false: Activated mimic joints cannot have command " "interfaces."); } auto find_joint = [&hw_info](const std::string & name) From 916e9e712f7c3cd2d8ee0e1584a0b254fb76a4e1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 20/26] Add description on mock_gpio_commands --- hardware_interface/doc/mock_components_userdoc.rst | 5 +++++ hardware_interface/src/mock_components/generic_system.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 9f72e21a27..782d3e01ea 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -22,6 +22,7 @@ Features: - support for mimic joints, which is parsed from the URDF (see the `URDF wiki `__) - mirroring commands to states with and without offset - fake command interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) + - fake gpio interfaces for setting sensor data from an external node (combined with a :ref:`forward controller `) Parameters @@ -36,6 +37,10 @@ mock_sensor_commands (optional; boolean; default: false) Creates fake command interfaces for faking sensor measurements with an external command. Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. +mock_gpio_commands (optional; boolean; default: false) + Creates fake command interfaces for faking GPIO states with an external command. + Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. + position_state_following_offset (optional; double; default: 0.0) Following offset added to the commanded values when mirrored to states. diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 5e5165e34f..2d8a01a34f 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -598,13 +598,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur mirror_command_to_state(sensor_states_, sensor_mock_commands_); } - // do loopback on all gpio interfaces if (use_mock_gpio_command_interfaces_) { mirror_command_to_state(gpio_states_, gpio_mock_commands_); } else { + // do loopback on all gpio interfaces mirror_command_to_state(gpio_states_, gpio_commands_); } From b61641fbcca0bd990a944439473cd02be4cae433 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 21/26] Add test for initial value of GPIO state interfaces --- hardware_interface/test/test_component_parser.cpp | 1 + .../include/ros2_control_test_assets/components_urdfs.hpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index f67f91308d..968d41ed97 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -564,6 +564,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_THAT(hardware_info.gpios[1].state_interfaces, SizeIs(1)); EXPECT_THAT(hardware_info.gpios[1].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.gpios[1].state_interfaces[0].name, "vacuum"); + EXPECT_EQ(hardware_info.gpios[1].state_interfaces[0].initial_value, "1.0"); EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum"); EXPECT_THAT(hardware_info.transmissions, IsEmpty()); 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 a42d39a241..7b2dd46e7a 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 @@ -372,7 +372,9 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio = - + + 1.0 + )"; From 9112088bfbf1fd51fd244c5e0f89f680013823a6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Feb 2024 22:08:58 +0000 Subject: [PATCH 22/26] Rename wheel_0/1 to wheel_left/right --- ...llers_chaining_with_controller_manager.cpp | 14 ++++++------- .../ros2_control_test_assets/descriptions.hpp | 20 +++++++++---------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index e52715604c..eadca39756 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -128,18 +128,18 @@ class TestControllerChainingWithControllerManager // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_0_joint/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; controller_interface::InterfaceConfiguration pid_left_state_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_0_joint/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; pid_left_wheel_controller->set_command_interface_configuration(pid_left_cmd_ifs_cfg); pid_left_wheel_controller->set_state_interface_configuration(pid_left_state_ifs_cfg); pid_left_wheel_controller->set_reference_interface_names({"velocity"}); // configure Left Wheel controller controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_1_joint/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_1_joint/velocity"}}; + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; pid_right_wheel_controller->set_command_interface_configuration(pid_right_cmd_ifs_cfg); pid_right_wheel_controller->set_state_interface_configuration(pid_right_state_ifs_cfg); pid_right_wheel_controller->set_reference_interface_names({"velocity"}); @@ -150,7 +150,7 @@ class TestControllerChainingWithControllerManager {std::string(PID_LEFT_WHEEL) + "/velocity", std::string(PID_RIGHT_WHEEL) + "/velocity"}}; controller_interface::InterfaceConfiguration diff_drive_state_ifs_cfg = { controller_interface::interface_configuration_type::INDIVIDUAL, - {"wheel_0_joint/velocity", "wheel_1_joint/velocity"}}; + {"wheel_left/velocity", "wheel_right/velocity"}}; diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); @@ -435,8 +435,8 @@ class TestControllerChainingWithControllerManager const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; - const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_0_joint/velocity"}; - const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_1_joint/velocity"}; + const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"}; + const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"}; const std::vector DIFF_DRIVE_CLAIMED_INTERFACES = { "pid_left_wheel_controller/velocity", "pid_right_wheel_controller/velocity"}; const std::vector POSITION_CONTROLLER_CLAIMED_INTERFACES = { diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 7c66aed94c..c71b5d70a1 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -498,16 +498,16 @@ const auto diffbot_urdf = - + - - - + + + 1 1 @@ -533,16 +533,16 @@ const auto diffbot_urdf = - + - - - + + + 1 1 @@ -560,7 +560,7 @@ const auto diffbot_urdf = test_actuator - + @@ -570,7 +570,7 @@ const auto diffbot_urdf = test_actuator - + From 6a95524a8e8e45cf0ca140fa393abb1189f477a6 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 25 Mar 2024 18:57:06 +0000 Subject: [PATCH 23/26] Fix uninitialize typos and use SizeIs for vector size comparisons --- .../test/test_components/test_actuator.cpp | 25 ++++++----- .../test/test_components/test_components.xml | 12 +++--- .../test/test_components/test_sensor.cpp | 4 +- .../test/test_components/test_system.cpp | 10 +++-- .../test/test_resource_manager.cpp | 42 +++++++++++-------- .../test/test_resource_manager.hpp | 2 +- .../ros2_control_test_assets/descriptions.hpp | 18 ++++---- 7 files changed, 63 insertions(+), 50 deletions(-) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 9a93db29b4..a01ccd879f 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -38,9 +38,11 @@ class TestActuator : public ActuatorInterface * // can only control one joint * if (info_.joints.size() != 1) {return CallbackReturn::ERROR;} * // can only control in position - * if (info_.joints[0].command_interfaces.size() != 1) {return CallbackReturn::ERROR;} + * if (info_.joints[0].command_interfaces.size() != 1) {return + * CallbackReturn::ERROR;} * // can only give feedback state for position and velocity - * if (info_.joints[0].state_interfaces.size() != 2) {return CallbackReturn::ERROR;} + * if (info_.joints[0].state_interfaces.size() != 2) {return + * CallbackReturn::ERROR;} */ return CallbackReturn::SUCCESS; @@ -95,7 +97,8 @@ class TestActuator : public ActuatorInterface // simulate error on read if (velocity_command_ == test_constants::READ_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_ = 0.0; return return_type::ERROR; } @@ -104,10 +107,11 @@ class TestActuator : public ActuatorInterface { return return_type::DEACTIVATE; } - // The next line is for the testing purposes. We need value to be changed to be sure that - // the feedback from hardware to controllers in the chain is working as it should. - // This makes value checks clearer and confirms there is no "state = command" line or some - // other mixture of interfaces somewhere in the test stack. + // The next line is for the testing purposes. We need value to be changed to + // be sure that the feedback from hardware to controllers in the chain is + // working as it should. This makes value checks clearer and confirms there + // is no "state = command" line or some other mixture of interfaces + // somewhere in the test stack. velocity_state_ = velocity_command_ / 2; return return_type::OK; } @@ -117,7 +121,8 @@ class TestActuator : public ActuatorInterface // simulate error on write if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_ = 0.0; return return_type::ERROR; } @@ -136,7 +141,7 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; -class TestUnitializableActuator : public TestActuator +class TestUninitializableActuator : public TestActuator { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -147,4 +152,4 @@ class TestUnitializableActuator : public TestActuator #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitializableActuator, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUninitializableActuator, hardware_interface::ActuatorInterface) diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index a4d66ef012..e24ee28317 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -18,21 +18,21 @@ - + - Test Unitializable Actuator + Test Uninitializable Actuator - + - Test Unitializable Sensor + Test Uninitializable Sensor - + - Test Unitializable System + Test Uninitializable System diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 149c4fa125..2ea36ac5c1 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -55,7 +55,7 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; -class TestUnitializableSensor : public TestSensor +class TestUninitializableSensor : public TestSensor { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -66,4 +66,4 @@ class TestUnitializableSensor : public TestSensor #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitializableSensor, hardware_interface::SensorInterface) +PLUGINLIB_EXPORT_CLASS(TestUninitializableSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index f51e8c83e9..20e606ee6d 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -78,7 +78,8 @@ class TestSystem : public SystemInterface // simulate error on read if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_[0] = 0.0; return return_type::ERROR; } @@ -95,7 +96,8 @@ class TestSystem : public SystemInterface // simulate error on write if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) { - // reset value to get out from error on the next call - simplifies CM tests + // reset value to get out from error on the next call - simplifies CM + // tests velocity_command_[0] = 0.0; return return_type::ERROR; } @@ -117,7 +119,7 @@ class TestSystem : public SystemInterface double configuration_command_ = 0.0; }; -class TestUnitializableSystem : public TestSystem +class TestUninitializableSystem : public TestSystem { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -128,4 +130,4 @@ class TestUnitializableSystem : public TestSystem #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitializableSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(TestUninitializableSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index e39bdf67f4..c8b077229e 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -44,6 +44,7 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; +using testing::SizeIs; auto configure_components = [](TestableResourceManager & rm, const std::vector & components = {}) @@ -101,22 +102,23 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf) ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } -TEST_F(ResourceManagerTest, test_unitializable_hardware_validation) +TEST_F(ResourceManagerTest, test_uninitializable_hardware_validation) { - // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a - // runtime exception is thrown + // If the the hardware can not be initialized and load_urdf tried to validate + // the interfaces a runtime exception is thrown TestableResourceManager rm; ASSERT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_unitializable_robot_urdf, true), + rm.load_urdf(ros2_control_test_assets::minimal_uninitializable_robot_urdf, true), std::runtime_error); } -TEST_F(ResourceManagerTest, test_unitializable_hardware_no_validation) +TEST_F(ResourceManagerTest, test_uninitializable_hardware_no_validation) { - // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, - // the interface should not show up + // If the the hardware can not be initialized and load_urdf didn't try to + // validate the interfaces, the interface should not show up TestableResourceManager rm; - EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitializable_robot_urdf, false)); + EXPECT_NO_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_uninitializable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -150,7 +152,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_EQ(1u, rm.system_components_size()); auto state_interface_keys = rm.state_interface_keys(); - ASSERT_EQ(11u, state_interface_keys.size()); + ASSERT_THAT(state_interface_keys, SizeIs(11)); EXPECT_TRUE(rm.state_interface_exists("joint1/position")); EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")); @@ -158,7 +160,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.state_interface_exists("joint3/position")); auto command_interface_keys = rm.command_interface_keys(); - ASSERT_EQ(6u, command_interface_keys.size()); + ASSERT_THAT(command_interface_keys, SizeIs(6)); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); @@ -351,8 +353,8 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) EXPECT_EQ(1u, rm.sensor_components_size()); EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(11u, rm.state_interface_keys().size()); - ASSERT_EQ(6u, rm.command_interface_keys().size()); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(11)); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(6)); hardware_interface::HardwareInfo external_component_hw_info; external_component_hw_info.name = "ExternalComponent"; @@ -361,9 +363,9 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) rm.import_component(std::make_unique(), external_component_hw_info); EXPECT_EQ(2u, rm.actuator_components_size()); - ASSERT_EQ(12u, rm.state_interface_keys().size()); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")); - ASSERT_EQ(7u, rm.command_interface_keys().size()); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(7)); EXPECT_TRUE(rm.command_interface_exists("external_joint/external_command_interface")); auto status_map = rm.get_components_status(); @@ -874,7 +876,8 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) std::bind(&TestableResourceManager::command_interface_is_claimed, &rm, _1), expected_result); }; - // All resources start as UNCONFIGURED - All interfaces are imported but not available + // All resources start as UNCONFIGURED - All interfaces are imported but not + // available { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, @@ -954,7 +957,8 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, false); } - // When actuator is activated all state- and command- interfaces become available + // When actuator is activated all state- and command- interfaces become + // available activate_components(rm, {TEST_ACTUATOR_HARDWARE_NAME}); { check_interfaces( @@ -1430,7 +1434,8 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest check_if_interface_available(true, true); } - // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and + // TEST_SYSTEM_HARDWARE_NAME claimed_itfs[0].set_value(fail_value); claimed_itfs[1].set_value(fail_value); { @@ -1547,7 +1552,8 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest check_if_interface_available(true, true); } - // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and + // TEST_SYSTEM_HARDWARE_NAME claimed_itfs[0].set_value(deactivate_value); claimed_itfs[1].set_value(deactivate_value); { diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index b20623b1f9..554dfe2c86 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -48,7 +48,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); - FRIEND_TEST(ResourceManagerTest, test_unitializable_hardware_no_validation); + FRIEND_TEST(ResourceManagerTest, test_uninitializable_hardware_no_validation); TestableResourceManager() : hardware_interface::ResourceManager() {} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index c71b5d70a1..308751e0d8 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -306,11 +306,11 @@ const auto hardware_resources = )"; -const auto unitializable_hardware_resources = +const auto uninitializable_hardware_resources = R"( - + - test_unitializable_actuator + test_uninitializable_actuator @@ -319,9 +319,9 @@ const auto unitializable_hardware_resources = - + - test_unitializable_sensor + test_uninitializable_sensor 2 2 @@ -329,9 +329,9 @@ const auto unitializable_hardware_resources = - + - test_unitializable_system + test_uninitializable_system 2 2 @@ -1011,8 +1011,8 @@ const auto gripper_hardware_resources_mimic_false_command_if = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); -const auto minimal_unitializable_robot_urdf = - std::string(urdf_head) + std::string(unitializable_hardware_resources) + std::string(urdf_tail); +const auto minimal_uninitializable_robot_urdf = + std::string(urdf_head) + std::string(uninitializable_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + From 614363673d1a2ae25cef5e143abe446345ff0283 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 27 Mar 2024 08:18:58 +0000 Subject: [PATCH 24/26] Add migration notes --- doc/index.rst | 11 ++++ doc/migration/Foxy.rst | 51 +++++++++++++++++++ doc/migration/Iron.rst | 49 ++++++++++++++++++ .../doc/hardware_components_userdoc.rst | 50 ------------------ 4 files changed, 111 insertions(+), 50 deletions(-) create mode 100644 doc/migration/Foxy.rst create mode 100644 doc/migration/Iron.rst diff --git a/doc/index.rst b/doc/index.rst index 91f965d601..153bb26165 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -33,3 +33,14 @@ Concepts Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> + + +================ +Migration Guides +================ + +.. toctree:: + :titlesonly: + + Foxy to Galactic + Iron to Jazzy diff --git a/doc/migration/Foxy.rst b/doc/migration/Foxy.rst new file mode 100644 index 0000000000..a1afeddc70 --- /dev/null +++ b/doc/migration/Foxy.rst @@ -0,0 +1,51 @@ +Foxy to Galactic +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +hardware_interface +************************************** +Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. +The following list shows a set of quick changes to port existing hardware components to Galactic: + +1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` + +2. If using BaseInterface as base class then you should remove it. Specifically, change: + + .. code-block:: c++ + + hardware_interface::BaseInterface + + to + + .. code-block:: c++ + + hardware_interface::[Actuator|Sensor|System]Interface + +3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` + +4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary + +5. replace first three lines in ``on_init`` to + + .. code-block:: c++ + + if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + +6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` + +7. Remove all lines with ``status_ = ...`` or ``status::...`` + +8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and + ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` + +9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` + +10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` + +11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` + +12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add + ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` diff --git a/doc/migration/Iron.rst b/doc/migration/Iron.rst new file mode 100644 index 0000000000..0ae7446903 --- /dev/null +++ b/doc/migration/Iron.rst @@ -0,0 +1,49 @@ +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +component parser +************************************** + +* All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise a ``std::runtime_error`` is thrown. This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` tags instead. +* The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of + + .. code-block:: xml + + + + + + 0.15 + + + + + + right_finger_joint + 1 + + + + + + + + define your mimic joints directly in the joint definitions: + + .. code-block:: xml + + + + + + + + + + + + + + + + diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 404e90ee03..0a980cad4e 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -27,53 +27,3 @@ If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``w Error handling follows the `node lifecycle `_. If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED`` state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered. The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager. - -Migration from Foxy to newer versions -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. -The following list shows a set of quick changes to port existing hardware components to Galactic: - -1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` - -2. If using BaseInterface as base class then you should remove it. Specifically, change: - - .. code-block:: c++ - - hardware_interface::BaseInterface - - to - - .. code-block:: c++ - - hardware_interface::[Actuator|Sensor|System]Interface - -3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` - -4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary - -5. replace first three lines in ``on_init`` to - - .. code-block:: c++ - - if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - - -6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` - -7. Remove all lines with ``status_ = ...`` or ``status::...`` - -8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and - ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` - -9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` - -10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` - -11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` - -12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add - ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` From 2fdcca4d58ec936ed3117819f142b66cc2a30936 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 27 Mar 2024 09:28:40 +0000 Subject: [PATCH 25/26] Remove duplicte section --- controller_manager/doc/userdoc.rst | 7 ------- 1 file changed, 7 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 6af78ea8fd..c8aa68d774 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -87,13 +87,6 @@ update_rate (mandatory; integer) Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. -Subscribers ------------ - -robot_description (std_msgs/msg/String) - The URDF string as robot description. - This is usually published by the ``robot_state_publisher`` node. - Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 931f1089e68c373317e02b5d965cd3f1158c2b7b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 27 Mar 2024 19:30:25 +0000 Subject: [PATCH 26/26] Remove migration notes from ros2_control page --- doc/index.rst | 11 ----------- doc/migration/Iron.rst | 3 ++- 2 files changed, 2 insertions(+), 12 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 153bb26165..91f965d601 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -33,14 +33,3 @@ Concepts Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> - - -================ -Migration Guides -================ - -.. toctree:: - :titlesonly: - - Foxy to Galactic - Iron to Jazzy diff --git a/doc/migration/Iron.rst b/doc/migration/Iron.rst index 0ae7446903..21e28fc43f 100644 --- a/doc/migration/Iron.rst +++ b/doc/migration/Iron.rst @@ -2,7 +2,8 @@ Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ component parser -************************************** +***************** +Changes from `(PR #1256) `__ * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise a ``std::runtime_error`` is thrown. This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` tags instead. * The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of