diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index a696e605d6..74dc9d818b 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -8,6 +8,7 @@ export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs lifecycle_msgs + LibXml2 pluginlib rclcpp_lifecycle rcpputils @@ -29,6 +30,7 @@ endforeach() add_library(hardware_interface SHARED src/component_parser.cpp + src/component_validator.cpp src/resource_manager.cpp src/hardware_component.cpp src/lexical_casts.cpp @@ -47,6 +49,7 @@ target_link_libraries(hardware_interface PUBLIC realtime_tools::realtime_tools rcutils::rcutils rcpputils::rcpputils + LibXml2::LibXml2 ${joint_limits_TARGETS} ${TinyXML2_LIBRARIES} ${pal_statistics_LIBRARIES} @@ -71,6 +74,8 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(ament_index_cpp REQUIRED) + find_package(LibXml2) ament_add_gmock(test_macros test/test_macros.cpp) target_include_directories(test_macros PRIVATE include) @@ -99,6 +104,14 @@ if(BUILD_TESTING) ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface ros2_control_test_assets::ros2_control_test_assets) + ament_add_gmock(test_component_validator test/test_component_validator.cpp) + target_link_libraries(test_component_validator + hardware_interface + ament_index_cpp::ament_index_cpp + ros2_control_test_assets::ros2_control_test_assets + LibXml2::LibXml2 + ) + add_library(test_hardware_components SHARED test/test_hardware_components/test_single_joint_actuator.cpp test/test_hardware_components/test_force_torque_sensor.cpp @@ -123,6 +136,12 @@ install( DIRECTORY include/ DESTINATION include/hardware_interface ) + +install( + DIRECTORY schema + DESTINATION share/hardware_interface +) + install( TARGETS mock_components diff --git a/hardware_interface/include/hardware_interface/component_validator.hpp b/hardware_interface/include/hardware_interface/component_validator.hpp new file mode 100644 index 0000000000..792c4b3b2d --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_validator.hpp @@ -0,0 +1,70 @@ +// Copyright 2020 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__COMPONENT_VALIDATOR_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_VALIDATOR_HPP_ + +#include +#include + +#include + +#include + +namespace hardware_interface +{ + +/// Validate URDF string against an XML Schema Definition (XSD) file. +/** + * \param[in] urdf string with robot's URDF + * \param[in] xsd_file_path path to the XSD file + * \return true if the URDF is valid according to the XSD, false otherwise + */ +bool validate_urdf_with_xsd(const std::string & urdf, const std::string & xsd_file_path); + +/// Validate URDF file path against an XML Schema Definition (XSD) file. +/** + * \param[in] urdf_file_path path to URDF file + * \param[in] xsd_file_path path to the XSD file + * \return true if URDF is valid according to the XSD, false otherwise + * + */ +bool validate_urdf_file_path_with_xsd( + const std::string & urdf_file_path, std::string & xsd_file_path); + +/// Validate URDF against an XSD file provides in URDF itself +/** + * \param[in] urdf string with robot's urdf + * \return true if the URDF is valid according to the XSD, false otherwise + */ +bool validate_urdf_with_xsd_tag(const std::string & urdf); + +/// Validate URDF file path against an XSD file provides in URDF itself +/** + * \param[in] urdf_file_path string + * \return true if the URDF is valid according to the XSD, false otherwise + */ +bool validate_urdf_file_with_xsd_tag(const std::string & urdf_file_path); + +/// Extract ROS2 Control XSD tag from URDF +/** + * \param[in] urdf string with robot's urdf contains xmlns:ros2_control + * \param[out] string of xlmns:ros2_control + * \return true if extraction successful else false + */ +bool extract_ros2_control_xsd_tag(const std::string & urdf, std::string & ros2_control_xsd); + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__COMPONENT_VALIDATOR_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 153e737f7f..30485b8fd3 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -31,6 +31,8 @@ rcutils ament_cmake_gmock + ament_index_cpp + libxml2 ros2_control_test_assets diff --git a/hardware_interface/schema/ros2_control.xsd b/hardware_interface/schema/ros2_control.xsd new file mode 100644 index 0000000000..a9c237c090 --- /dev/null +++ b/hardware_interface/schema/ros2_control.xsd @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hardware_interface/src/component_validator.cpp b/hardware_interface/src/component_validator.cpp new file mode 100644 index 0000000000..b4a97b4823 --- /dev/null +++ b/hardware_interface/src/component_validator.cpp @@ -0,0 +1,216 @@ +// Copyright 2025 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. + +#include +#include +#include + +#include "hardware_interface/component_validator.hpp" + +namespace hardware_interface +{ +bool validate_urdf_with_xsd(const std::string & urdf, const std::string & xsd_file_path) +{ + if (urdf.empty()) + { + throw std::runtime_error("empty URDF passed to robot"); + } + // Load URDF + xmlDocPtr doc = xmlReadMemory(urdf.c_str(), static_cast(urdf.size()), nullptr, nullptr, 0); + if (!doc) + { + return false; + } + + // Load XSD + xmlSchemaParserCtxtPtr schemaCtx = xmlSchemaNewParserCtxt(xsd_file_path.c_str()); + if (!schemaCtx) + { + xmlFreeDoc(doc); + return false; + } + // Parse XSD + xmlSchemaPtr schema = xmlSchemaParse(schemaCtx); + if (!schema) + { + xmlSchemaFreeParserCtxt(schemaCtx); + xmlFreeDoc(doc); + return false; + } + // Validate URDF against XSD + xmlSchemaValidCtxtPtr validCtx = xmlSchemaNewValidCtxt(schema); + if (!validCtx) + { + xmlSchemaFree(schema); + xmlSchemaFreeParserCtxt(schemaCtx); + xmlFreeDoc(doc); + return false; + } + // Perform validation + int ret = xmlSchemaValidateDoc(validCtx, doc); + xmlSchemaFreeValidCtxt(validCtx); + xmlSchemaFree(schema); + xmlSchemaFreeParserCtxt(schemaCtx); + xmlFreeDoc(doc); + + return ret == 0; +} + +bool validate_urdf_file_path_with_xsd( + const std::string & urdf_file_path, std::string & xsd_file_path) +{ + std::ifstream file(urdf_file_path); + if (!file) + { + return false; + } + + std::ostringstream ss; + ss << file.rdbuf(); + const std::string urdf = ss.str(); + + if (urdf.empty()) + { + return false; + } + + return validate_urdf_with_xsd(urdf, xsd_file_path); +} + +bool validate_urdf_with_xsd_tag(const std::string & urdf) +{ + if (urdf.empty()) + { + throw std::runtime_error("empty URDF passed to robot"); + } + // extract xmlns tag from the urdf + std::string ros2_control_xsd; + bool result = extract_ros2_control_xsd_tag(urdf, ros2_control_xsd); + if (!result) + { + return false; + } + std::string xsd_package_share_dir = + ament_index_cpp::get_package_share_directory("hardware_interface"); + // If the extracted XSD is a file URI (e.g. "file:///path/to/schema.xsd"), normalize to a local + // path + if (ros2_control_xsd.find("file") != std::string::npos) + { + ros2_control_xsd.replace(0, 7, xsd_package_share_dir); + } + else if (ros2_control_xsd.find("http") != std::string::npos) + { + // Download the remote XSD to a local temporary file and point to it + std::string filename; + auto pos = ros2_control_xsd.find_last_of('/'); + if (pos == std::string::npos || pos + 1 >= ros2_control_xsd.size()) + { + filename = "ros2_control_schema.xsd"; + } + else + { + filename = ros2_control_xsd.substr(pos + 1); + } + std::string tmp_path = std::string("/tmp/") + filename; + + // Use curl to fetch the XSD; require curl to be available on PATH. + std::ostringstream cmd; + cmd << "curl -sSfL -o '" << tmp_path << "' '" + << ros2_control_xsd + std::string("ros2_control.xsd") << "'"; + + int rc = std::system(cmd.str().c_str()); + if (rc != 0) + { + // failed to download + return false; + } + + ros2_control_xsd = tmp_path; + } + else + { + return false; + } + return validate_urdf_with_xsd(urdf, ros2_control_xsd); +} + +bool validate_urdf_file_with_xsd_tag(const std::string & urdf_file_path) +{ + std::ifstream file(urdf_file_path); + if (!file) + { + return false; + } + + std::ostringstream ss; + ss << file.rdbuf(); + const std::string urdf = ss.str(); + + if (urdf.empty()) + { + return false; + } + return validate_urdf_with_xsd_tag(urdf); +} + +bool extract_ros2_control_xsd_tag(const std::string & urdf, std::string & ros2_control_xsd) +{ + if (urdf.empty()) + { + throw std::runtime_error("empty URDF passed to robot"); + } + + xmlDocPtr doc = xmlReadMemory(urdf.c_str(), static_cast(urdf.size()), nullptr, nullptr, 0); + if (!doc) + { + return false; + } + + xmlNodePtr root = xmlDocGetRootElement(doc); + bool found = false; + if (root) + { + auto check = [&](xmlNsPtr ns) -> bool + { + if ( + ns && ns->prefix && + std::strcmp(reinterpret_cast(ns->prefix), "ros2_control") == 0) + { + ros2_control_xsd = ns->href ? reinterpret_cast(ns->href) : ""; + return true; + } + return false; + }; + if (check(root->ns)) + { + found = true; + } + else + { + for (xmlNsPtr cur = root->nsDef; cur; cur = cur->next) + { + if (check(cur)) + { + found = true; + break; + } + } + } + } + + xmlFreeDoc(doc); + return found; +} + +} // namespace hardware_interface diff --git a/hardware_interface/test/test_component_validator.cpp b/hardware_interface/test/test_component_validator.cpp new file mode 100644 index 0000000000..2242872d97 --- /dev/null +++ b/hardware_interface/test/test_component_validator.cpp @@ -0,0 +1,93 @@ +// Copyright 2025 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. + +#include +#include +#include + +#include +#include "hardware_interface/component_validator.hpp" + +using namespace ::testing; // NOLINT + +class TestComponentValidator : public Test +{ +protected: + std::string valid_xml_file_path; + std::string valid_xml_file_path_with_tag; + std::string valid_xml_file_path_with_web_tag; + std::string invalid_xml_file_path; + std::string xsd_file_path; + void SetUp() override + { + // Use ament_index_cpp to get the package share directory + std::string urdf_package_share_dir = + ament_index_cpp::get_package_share_directory("ros2_control_test_assets"); + std::string xsd_package_share_dir = + ament_index_cpp::get_package_share_directory("hardware_interface"); + valid_xml_file_path = urdf_package_share_dir + "/urdf/test_hardware_components.urdf"; + valid_xml_file_path_with_tag = + urdf_package_share_dir + "/urdf/test_hardware_components_xsd_file.urdf"; + valid_xml_file_path_with_web_tag = + urdf_package_share_dir + "/urdf/test_hardware_components_xsd_web.urdf"; + invalid_xml_file_path = + urdf_package_share_dir + "/urdf/test_hardware_components_with_error.urdf"; + xsd_file_path = xsd_package_share_dir + "/schema/ros2_control.xsd"; + } +}; + +using hardware_interface::validate_urdf_file_path_with_xsd; +using hardware_interface::validate_urdf_file_with_xsd_tag; +using hardware_interface::validate_urdf_with_xsd; + +TEST_F(TestComponentValidator, DryRun) +{ + SUCCEED(); // Minimal test to allow compilation and DRY test +} + +TEST_F(TestComponentValidator, empty_string_throws_error) +{ + ASSERT_THROW(validate_urdf_with_xsd("", xsd_file_path), std::runtime_error); +} + +TEST_F(TestComponentValidator, empty_urdf_throws_error) +{ + const std::string empty_urdf = + ""; + + ASSERT_TRUE(validate_urdf_with_xsd( + empty_urdf, xsd_file_path)); // TODO(Sachin): discuss if should use throw error +} + +TEST_F(TestComponentValidator, validate_valid_urdf_with_xsd) +{ + ASSERT_TRUE(validate_urdf_file_path_with_xsd(valid_xml_file_path, xsd_file_path)); +} + +TEST_F(TestComponentValidator, validate_invalid_urdf_with_xsd) +{ + ASSERT_FALSE(validate_urdf_file_path_with_xsd(invalid_xml_file_path, xsd_file_path)); +} + +TEST_F(TestComponentValidator, validate_valid_urdf_including_xsd_file_tag) +{ + ASSERT_TRUE(validate_urdf_file_with_xsd_tag(valid_xml_file_path_with_tag)); +} + +TEST_F(TestComponentValidator, validate_valid_urdf_including_xsd_web_tag) +{ + ASSERT_FALSE(validate_urdf_file_with_xsd_tag( + valid_xml_file_path_with_web_tag)); // TODO(Sachin): Update the test to assert true when xsd + // file is uploaded to web server +} diff --git a/ros2_control_test_assets/CMakeLists.txt b/ros2_control_test_assets/CMakeLists.txt index d63fb52c86..b03b804683 100644 --- a/ros2_control_test_assets/CMakeLists.txt +++ b/ros2_control_test_assets/CMakeLists.txt @@ -14,8 +14,11 @@ install( DIRECTORY include/ DESTINATION include/ros2_control_test_assets ) -install( - FILES urdf/test_hardware_components.urdf +install(FILES + urdf/test_hardware_components.urdf + urdf/test_hardware_components_xsd_file.urdf + urdf/test_hardware_components_xsd_web.urdf + urdf/test_hardware_components_with_error.urdf DESTINATION share/ros2_control_test_assets/urdf ) install(TARGETS ros2_control_test_assets diff --git a/ros2_control_test_assets/urdf/test_hardware_components.urdf b/ros2_control_test_assets/urdf/test_hardware_components.urdf index c49c79bf55..611a7b4385 100644 --- a/ros2_control_test_assets/urdf/test_hardware_components.urdf +++ b/ros2_control_test_assets/urdf/test_hardware_components.urdf @@ -52,6 +52,7 @@ test_hardware_components/TestForceTorqueSensor + 0 @@ -68,12 +69,22 @@ test_hardware_components/TestTwoJointSystem - + + -1 + 1 + + + + 0 + 1 + + + diff --git a/ros2_control_test_assets/urdf/test_hardware_components_with_error.urdf b/ros2_control_test_assets/urdf/test_hardware_components_with_error.urdf new file mode 100644 index 0000000000..34b01aa5c1 --- /dev/null +++ b/ros2_control_test_assets/urdf/test_hardware_components_with_error.urdf @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + test_hardware_components/TestForceTorqueSensor + 0 + + + + + + + + + + + + + + test_hardware_components/TestTwoJointSystem + + + + -1 + 1 + + + + + + + + + + 0 + 1 + + + + + diff --git a/ros2_control_test_assets/urdf/test_hardware_components_xsd_file.urdf b/ros2_control_test_assets/urdf/test_hardware_components_xsd_file.urdf new file mode 100644 index 0000000000..96adc05c02 --- /dev/null +++ b/ros2_control_test_assets/urdf/test_hardware_components_xsd_file.urdf @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + test_hardware_components/TestForceTorqueSensor + 0 + + + + + + + + + + + + + + test_hardware_components/TestTwoJointSystem + + + + -1 + 1 + + + + + + + + + + 0 + 1 + + + + + diff --git a/ros2_control_test_assets/urdf/test_hardware_components_xsd_web.urdf b/ros2_control_test_assets/urdf/test_hardware_components_xsd_web.urdf new file mode 100644 index 0000000000..03ccc189ce --- /dev/null +++ b/ros2_control_test_assets/urdf/test_hardware_components_xsd_web.urdf @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + test_hardware_components/TestForceTorqueSensor + 0 + + + + + + + + + + + + + + test_hardware_components/TestTwoJointSystem + + + + -1 + 1 + + + + + + + + + + 0 + 1 + + + + +