diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 6d6add84f3..8339e99f3f 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -18,17 +18,14 @@ find_package(TinyXML2 REQUIRED) add_library(transmission_parser SHARED src/transmission_parser.cpp) target_include_directories(transmission_parser PUBLIC include) -ament_target_dependencies(transmission_parser - hardware_interface - TinyXML2 -) +ament_target_dependencies(transmission_parser hardware_interface tinyxml2_vendor TinyXML2) +ament_export_dependencies(hardware_interface tinyxml2_vendor TinyXML2) target_compile_definitions(transmission_parser PRIVATE "TRANSMISSION_INTERFACE_BUILDING_DLL") install( DIRECTORY include/ DESTINATION include ) - install( TARGETS transmission_parser RUNTIME DESTINATION bin @@ -45,6 +42,7 @@ if(BUILD_TESTING) test_transmission_parser test/test_transmission_parser.cpp ) + target_include_directories(test_transmission_parser PUBLIC include) target_link_libraries(test_transmission_parser transmission_parser) endif() diff --git a/transmission_interface/include/transmission_interface/transmission_info.hpp b/transmission_interface/include/transmission_interface/transmission_info.hpp index 60c4e0c2c9..3e21c6bdf3 100644 --- a/transmission_interface/include/transmission_interface/transmission_info.hpp +++ b/transmission_interface/include/transmission_interface/transmission_info.hpp @@ -15,18 +15,44 @@ #ifndef TRANSMISSION_INTERFACE__TRANSMISSION_INFO_HPP_ #define TRANSMISSION_INTERFACE__TRANSMISSION_INFO_HPP_ +#include #include -#include "hardware_interface/control_type.hpp" namespace transmission_interface { +/** + * \brief Contains semantic info about a given joint loaded from XML (URDF) + */ +struct JointInfo +{ + std::string name; + std::vector interfaces; + std::string role; +}; + +/** + * \brief Contains semantic info about a given actuator loaded from XML (URDF) + */ +struct ActuatorInfo +{ + std::string name; + std::vector interfaces; + double mechanical_reduction; +}; + +/** + * \brief Contains semantic info about a given transmission loaded from XML (URDF) + */ struct TransmissionInfo { - std::string joint_name; - std::string joint_control_type; + std::string name; + std::string type; + std::vector joints; + std::vector actuators; }; } // namespace transmission_interface + #endif // TRANSMISSION_INTERFACE__TRANSMISSION_INFO_HPP_ diff --git a/transmission_interface/include/transmission_interface/transmission_parser.hpp b/transmission_interface/include/transmission_interface/transmission_parser.hpp index 98bd200f07..55f439a414 100644 --- a/transmission_interface/include/transmission_interface/transmission_parser.hpp +++ b/transmission_interface/include/transmission_interface/transmission_parser.hpp @@ -12,19 +12,50 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** + * \file + * \brief Parses \ elements into corresponding structs from XML (URDF). + * \author Dave Coleman + */ + #ifndef TRANSMISSION_INTERFACE__TRANSMISSION_PARSER_HPP_ #define TRANSMISSION_INTERFACE__TRANSMISSION_PARSER_HPP_ + +#include +#include + +#include + #include #include -#include "transmission_interface/transmission_info.hpp" -#include "transmission_interface/visibility_control.h" - namespace transmission_interface { + +/** + * \brief Parses the joint elements within transmission elements of a URDF + * If parse errors occur a std::runtime_error will be thrown with a description of the problem. + * \param[in] trans_it pointer to the current XML element being parsed + * \param[out] joints resulting list of joints in the transmission + * \return true if joint information for a transmission was successfully parsed. + */ +TRANSMISSION_INTERFACE_PUBLIC +std::vector parse_joints(tinyxml2::XMLElement * trans_it); + /** - * \brief Parse transmission information from a URDF + * \brief Parses the actuator elements within transmission elements of a URDF + * If parse errors occur a std::runtime_error will be thrown with a description of the problem. + * \param[in] trans_it pointer to the current XML element being parsed + * \param[out] actuators resulting list of actuators in the transmission + * \return true if actuator information for a transmission was successfully parsed. + */ +TRANSMISSION_INTERFACE_PUBLIC +std::vector parse_actuators(tinyxml2::XMLElement * trans_it); + + +/** + * \brief Parse transmission information from a URDF. * \param urdf A string containing the URDF xml * \return parsed transmission information * \throws std::runtime_error on malformed or empty xml @@ -32,4 +63,5 @@ namespace transmission_interface TRANSMISSION_INTERFACE_PUBLIC std::vector parse_transmissions_from_urdf(const std::string & urdf); } // namespace transmission_interface + #endif // TRANSMISSION_INTERFACE__TRANSMISSION_PARSER_HPP_ diff --git a/transmission_interface/src/transmission_parser.cpp b/transmission_interface/src/transmission_parser.cpp index 4d1076ae8b..d324da30fd 100644 --- a/transmission_interface/src/transmission_parser.cpp +++ b/transmission_interface/src/transmission_parser.cpp @@ -12,19 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "transmission_interface/transmission_parser.hpp" -#include "transmission_interface/transmission_info.hpp" +#include -#include -#include +#include #include #include namespace { +constexpr const auto kTransmissionParserLoggerName = "transmission_parser"; + constexpr const auto kTransmissionTag = "transmission"; +constexpr const auto kNameTag = "name"; constexpr const auto kJointTag = "joint"; +constexpr const auto kActuatorTag = "actuator"; +constexpr const auto kTypeTag = "type"; +constexpr const auto kRoleTag = "role"; constexpr const auto kHardwareInterfaceTag = "hardwareInterface"; +constexpr const auto kMechanicalReductionTag = "mechanicalReduction"; } // namespace namespace transmission_interface @@ -42,38 +47,151 @@ std::vector parse_transmissions_from_urdf(const std::string & } std::vector transmissions; - // Find joints in transmission tags - const tinyxml2::XMLElement * root_it = doc.RootElement(); - const tinyxml2::XMLElement * trans_it = root_it->FirstChildElement(kTransmissionTag); - while (trans_it) { - // Joint name - auto joint_it = trans_it->FirstChildElement(kJointTag); - if (!joint_it) { - throw std::runtime_error("no joint child element found"); + tinyxml2::XMLElement * root_it = doc.RootElement(); + tinyxml2::XMLElement * trans_it = nullptr; + for (trans_it = root_it->FirstChildElement(kTransmissionTag); trans_it; + trans_it = trans_it->NextSiblingElement(kTransmissionTag)) + { + transmission_interface::TransmissionInfo transmission; + + if (trans_it->Attribute(kNameTag)) { + transmission.name = trans_it->Attribute(kNameTag); + if (transmission.name.empty()) { + throw std::runtime_error("empty name attribute specified for transmission"); + } + } else { + throw std::runtime_error("no name attribute specified for transmission"); + } + + // Transmission type + tinyxml2::XMLElement * type_child = trans_it->FirstChildElement(kTypeTag); + if (!type_child) { + throw std::runtime_error( + "no type element found in transmission '" + transmission.name + "'."); } + if (!type_child->GetText()) { + throw std::runtime_error( + "expected non-empty type element in transmission '" + transmission.name + "'."); + } + transmission.type = type_child->GetText(); + + try { + // Load joints + transmission.joints = parse_joints(trans_it); + // Load actuators + transmission.actuators = parse_actuators(trans_it); + } catch (const std::runtime_error & ex) { + // add the transmission name and rethrow + throw std::runtime_error("transmission '" + transmission.name + "' " + ex.what()); + } + + transmissions.push_back(transmission); + } - const std::string joint_name = joint_it->Attribute("name"); - if (joint_name.empty()) { + return transmissions; +} + +std::vector parse_joints(tinyxml2::XMLElement * trans_it) +{ + std::vector joints; + // Loop through each available joint + auto joint_it = trans_it->FirstChildElement(kJointTag); + if (!joint_it) { + throw std::runtime_error("no joint child element found"); + } + for (; joint_it; joint_it = joint_it->NextSiblingElement(kJointTag)) { + transmission_interface::JointInfo joint; + + joint.name = joint_it->Attribute(kNameTag); + if (joint.name.empty()) { throw std::runtime_error("no joint name attribute set"); } - const auto hardware_interface_it = joint_it->FirstChildElement(kHardwareInterfaceTag); - if (!hardware_interface_it) { + const tinyxml2::XMLElement * role_it = joint_it->FirstChildElement(kRoleTag); + if (role_it) { + joint.role = role_it->GetText() ? role_it->GetText() : std::string(); + } + + // Interfaces (required) + auto interface_it = joint_it->FirstChildElement(kHardwareInterfaceTag); + if (!interface_it) { + throw std::runtime_error( + "no hardware interface tag found under transmission joint" + joint.name); + } + + for (; interface_it; interface_it = interface_it->NextSiblingElement(kHardwareInterfaceTag)) { + const std::string interface_name = interface_it->GetText(); + if (interface_name.empty()) { + throw std::runtime_error("no hardware interface specified in joint " + joint.name); + } + joint.interfaces.push_back(interface_name); + } + + if (joint.interfaces.empty()) { throw std::runtime_error( - "no hardware interface tag found under transmission joint" + joint_name); + "joint " + joint.name + " has no valid hardware interface."); } - const std::string interface_name = hardware_interface_it->GetText(); - if (interface_name.empty()) { - throw std::runtime_error("no hardware interface specified in joint " + joint_name); + joints.push_back(joint); + } + + return joints; +} + +std::vector parse_actuators(tinyxml2::XMLElement * trans_it) +{ + std::vector actuators; + // Loop through each available actuator + auto actuator_it = trans_it->FirstChildElement(kActuatorTag); + if (!actuator_it) { + throw std::runtime_error("no actuator child element found"); + } + + for (; actuator_it; actuator_it = actuator_it->NextSiblingElement(kActuatorTag)) { + transmission_interface::ActuatorInfo actuator; + + actuator.name = actuator_it->Attribute(kNameTag); + if (actuator.name.empty()) { + throw std::runtime_error("no actuator name attribute set"); } - transmissions.push_back({joint_name, interface_name}); + // Hardware interfaces (optional) + auto interface_it = actuator_it->FirstChildElement(kHardwareInterfaceTag); + if (!interface_it) { + throw std::runtime_error( + "no hardware interface tag found under transmission actuator" + actuator.name); + } + + for (; interface_it; interface_it = interface_it->NextSiblingElement(kHardwareInterfaceTag)) { + const std::string interface_name = interface_it->GetText(); + if (interface_name.empty()) { + throw std::runtime_error("no hardware interface specified in actuator " + actuator.name); + } + actuator.interfaces.push_back(interface_name); + } + + if (actuator.interfaces.empty()) { + throw std::runtime_error( + "actuator " + actuator.name + " has no valid hardware interface."); + } - trans_it = trans_it->NextSiblingElement(kTransmissionTag); + // mechanical reduction (optional) + actuator.mechanical_reduction = 1; + auto mechred_it = actuator_it->FirstChildElement(kMechanicalReductionTag); + for (; mechred_it; mechred_it = mechred_it->NextSiblingElement(kMechanicalReductionTag)) { + // optional tag but if specified, it should not be empty! + const std::string mech_red_str = mechred_it->GetText(); + if (mech_red_str.empty()) { + throw std::runtime_error("mechanical reduction tag was specified without value"); + } else { + actuator.mechanical_reduction = atoi(mech_red_str.c_str()); + } + } + + actuators.push_back(actuator); } - return transmissions; + return actuators; } } // namespace transmission_interface diff --git a/transmission_interface/test/test_transmission_parser.cpp b/transmission_interface/test/test_transmission_parser.cpp index 08e0f0f24b..0506e310da 100644 --- a/transmission_interface/test/test_transmission_parser.cpp +++ b/transmission_interface/test/test_transmission_parser.cpp @@ -175,6 +175,7 @@ class TestTransmissionParser : public Test 1 + PositionJointInterface @@ -183,7 +184,8 @@ class TestTransmissionParser : public Test VelocityJointInterface - 1 + 60 + VelocityJointInterface @@ -381,32 +383,62 @@ class TestTransmissionParser : public Test }; using transmission_interface::parse_transmissions_from_urdf; +using transmission_interface::TransmissionInfo; TEST_F(TestTransmissionParser, successfully_parse_valid_urdf) { const auto transmissions = parse_transmissions_from_urdf(valid_urdf_xml_); ASSERT_THAT(transmissions, SizeIs(2)); - EXPECT_EQ("rrbot_joint1", transmissions[0].joint_name); - EXPECT_EQ(hardware_interface::joint_control_type::POSITION, transmissions[0].joint_control_type); - EXPECT_EQ("rrbot_joint2", transmissions[1].joint_name); - EXPECT_EQ(hardware_interface::joint_control_type::VELOCITY, transmissions[1].joint_control_type); + + // first transmission + EXPECT_EQ("rrbot_tran1", transmissions[0].name); + EXPECT_EQ("transmission_interface/SimpleTransmission", transmissions[0].type); + + ASSERT_THAT(transmissions[0].joints, SizeIs(1)); + ASSERT_THAT(transmissions[0].joints[0].interfaces, SizeIs(1)); + EXPECT_EQ("rrbot_joint1", transmissions[0].joints[0].name); + EXPECT_EQ("PositionJointInterface", transmissions[0].joints[0].interfaces[0]); + + ASSERT_THAT(transmissions[0].actuators, SizeIs(1)); + ASSERT_THAT(transmissions[0].actuators[0].interfaces, SizeIs(1)); + EXPECT_EQ("rrbot_motor1", transmissions[0].actuators[0].name); + EXPECT_EQ("PositionJointInterface", transmissions[0].actuators[0].interfaces[0]); + EXPECT_EQ(1, transmissions[0].actuators[0].mechanical_reduction); + + // second transmission + EXPECT_EQ("rrbot_tran2", transmissions[1].name); + EXPECT_EQ("transmission_interface/SimpleTransmission", transmissions[1].type); + + ASSERT_THAT(transmissions[1].joints, SizeIs(1)); + ASSERT_THAT(transmissions[1].joints[0].interfaces, SizeIs(1)); + EXPECT_EQ("rrbot_joint2", transmissions[1].joints[0].name); + EXPECT_EQ("VelocityJointInterface", transmissions[1].joints[0].interfaces[0]); + + ASSERT_THAT(transmissions[1].actuators, SizeIs(1)); + ASSERT_THAT(transmissions[1].actuators[0].interfaces, SizeIs(1)); + EXPECT_EQ("rrbot_motor2", transmissions[1].actuators[0].name); + EXPECT_EQ("VelocityJointInterface", transmissions[1].actuators[0].interfaces[0]); + EXPECT_EQ(60, transmissions[1].actuators[0].mechanical_reduction); } TEST_F(TestTransmissionParser, empty_string_throws_error) { - ASSERT_THROW(parse_transmissions_from_urdf(""), std::runtime_error); + ASSERT_THROW( + transmission_interface::parse_transmissions_from_urdf(""), + std::runtime_error); } TEST_F(TestTransmissionParser, empty_urdf_returns_empty) { - const std::string empty_urdf = - ""; - const auto transmissions = parse_transmissions_from_urdf(empty_urdf); + const auto transmissions = transmission_interface::parse_transmissions_from_urdf( + ""); ASSERT_THAT(transmissions, IsEmpty()); } TEST_F(TestTransmissionParser, wrong_urdf_throws_error) { - ASSERT_THROW(parse_transmissions_from_urdf(wrong_urdf_xml_), std::runtime_error); + EXPECT_THROW( + transmission_interface::parse_transmissions_from_urdf(wrong_urdf_xml_), + std::runtime_error); }