diff --git a/hardware_interface/include/hardware_interface/tools.hpp b/hardware_interface/include/hardware_interface/tools.hpp new file mode 100644 index 00000000000..d8bba10e6b4 --- /dev/null +++ b/hardware_interface/include/hardware_interface/tools.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TOOLS_HPP_ +#define HARDWARE_INTERFACE__TOOLS_HPP_ + +#include +#include + +double parse_double(const std::string & text) +{ + double result_value; + const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value); + if (parse_result.ec == std::errc()) + { + return result_value; + } + + return 0.0; +} + +#endif // HARDWARE_INTERFACE__TOOLS_HPP_ diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e9b38de65dc..fbb8547ab14 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 b140c63adb0..ef0f01e022a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/tools.hpp" namespace { @@ -659,39 +660,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 = parse_double(joint.parameters.at("multiplier")); + } + param_it = joint.parameters.find("offset"); + if (param_it != joint.parameters.end()) + { + mimic_joint.offset = parse_double(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 f4aee6c8a6d..20f1d173103 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 @@ -26,22 +25,12 @@ #include #include "hardware_interface/component_parser.hpp" +#include "hardware_interface/tools.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" namespace mock_components { -double parse_double(const std::string & text) -{ - double result_value; - const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value); - if (parse_result.ec == std::errc()) - { - return result_value; - } - - return 0.0; -} CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) { @@ -147,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 = parse_double(joint.parameters.at("multiplier")); - } - mimic_joints_.push_back(mimic_joint); - } - } - // search for non-standard joint interfaces for (const auto & joint : info_.joints) { @@ -604,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 2996e748fcb..e3b712f6905 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 - - + @@ -1208,11 +1204,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"); @@ -1262,7 +1256,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, "HardwareSystem2dofWithMimicJoint"); @@ -1923,10 +1917,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); auto start_interfaces = rm.command_interface_keys(); std::vector stop_interfaces; return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces); @@ -1940,7 +1935,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 bbdf8b10daf..bf7b6ec2b58 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"(